rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

support CRSF V3 protocol

This commit is contained in:
tbs-fpv 2021-04-08 12:32:20 +08:00
parent 025ee87a7a
commit ee2179b836
10 changed files with 349 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/main/common/crc.c
View File

@ -59,12 +59,12 @@ void crc16_ccitt_sbuf_append(sbuf_t *dst, uint8_t *start)
sbufWriteU16(dst, crc);
}
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
uint8_t crc8_calc(uint8_t crc, unsigned char a, uint8_t poly)
{
crc ^= a;
for (int ii = 0; ii < 8; ++ii) {
if (crc & 0x80) {
crc = (crc << 1) ^ 0xD5;
crc = (crc << 1) ^ poly;
} else {
crc = crc << 1;
}
@ -72,23 +72,23 @@ uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a)
return crc;
}
uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length)
uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length, uint8_t poly)
{
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (; p != pend; p++) {
crc = crc8_dvb_s2(crc, *p);
crc = crc8_calc(crc, *p, poly);
}
return crc;
}
void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start)
void crc8_sbuf_append(sbuf_t *dst, uint8_t *start, uint8_t poly)
{
uint8_t crc = 0;
const uint8_t * const end = dst->ptr;
for (const uint8_t *ptr = start; ptr < end; ++ptr) {
crc = crc8_dvb_s2(crc, *ptr);
crc = crc8_calc(crc, *ptr, poly);
}
sbufWriteU8(dst, crc);
}

12
src/main/common/crc.h
View File

@ -27,8 +27,14 @@ uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length);
struct sbuf_s;
void crc16_ccitt_sbuf_append(struct sbuf_s *dst, uint8_t *start);
uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a);
uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length);
void crc8_dvb_s2_sbuf_append(struct sbuf_s *dst, uint8_t *start);
uint8_t crc8_calc(uint8_t crc, unsigned char a, uint8_t poly);
uint8_t crc8_update(uint8_t crc, const void *data, uint32_t length, uint8_t poly);
void crc8_sbuf_append(struct sbuf_s *dst, uint8_t *start, uint8_t poly);
#define crc8_dvb_s2(crc, a) crc8_calc(crc, a, 0xD5)
#define crc8_dvb_s2_update(crc, data, length) crc8_update(crc, data, length, 0xD5)
#define crc8_dvb_s2_sbuf_append(dst, start) crc8_sbuf_append(dst, start, 0xD5)
#define crc8_poly_0xba(crc, a) crc8_calc(crc, a, 0xBA)
#define crc8_poly_0xba_sbuf_append(dst, start) crc8_sbuf_append(dst, start, 0xBA)
uint8_t crc8_xor_update(uint8_t crc, const void *data, uint32_t length);
void crc8_xor_sbuf_append(struct sbuf_s *dst, uint8_t *start);

3
src/main/io/serial.h
View File

@ -71,7 +71,8 @@ typedef enum {
BAUD_1000000,
BAUD_1500000,
BAUD_2000000,
BAUD_2470000
BAUD_2470000,
BAUD_COUNT
} baudRate_e;
extern const uint32_t baudRates[];

185
src/main/rx/crsf.c
View File

@ -58,6 +58,8 @@
#define CRSF_LINK_STATUS_UPDATE_TIMEOUT_US 250000 // 250ms, 4 Hz mode 1 telemetry
#define CRSF_FRAME_ERROR_COUNT_THRESHOLD 100
STATIC_UNIT_TESTED bool crsfFrameDone = false;
STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame;
@ -119,6 +121,14 @@ struct crsfPayloadRcChannelsPacked_s {
typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
// first 5 bits in the first byte hold the first channel packed
// remaining bits hold the channel data in 11-bit format
#define CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION 11
#define CRSF_SUBSET_RC_CHANNELS_PACKED_MASK 0x07FF
#define CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION 5
#define CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK 0x1F
#define CRSF_SUBSET_RC_CHANNELS_PACKED_SIZE 45 // 5 + 11 * 24 = 269 bits = 34 bytes
#if defined(USE_CRSF_LINK_STATISTICS)
/*
* 0x14 Link statistics
@ -137,6 +147,29 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
* Uplink is the connection from the ground to the UAV and downlink the opposite direction.
*/
/*
* 0x1C Link statistics RX
* Payload:
*
* uint8_t Downlink RSSI ( dBm * -1 )
* uint8_t Downlink RSSI ( % )
* uint8_t Downlink Package success rate / Link quality ( % )
* int8_t Downlink SNR ( db )
* uint8_t Uplink RF Power ( db )
*/
/*
* 0x1D Link statistics TX
* Payload:
*
* uint8_t Uplink RSSI ( dBm * -1 )
* uint8_t Uplink RSSI ( % )
* uint8_t Uplink Package success rate / Link quality ( % )
* int8_t Uplink SNR ( db )
* uint8_t Downlink RF Power ( db )
* uint8_t Uplink FPS ( FPS / 10 )
*/
typedef struct crsfPayloadLinkstatistics_s {
uint8_t uplink_RSSI_1;
uint8_t uplink_RSSI_2;
@ -150,6 +183,25 @@ typedef struct crsfPayloadLinkstatistics_s {
int8_t downlink_SNR;
} crsfLinkStatistics_t;
#if defined(USE_CRSF_V3)
typedef struct crsfPayloadLinkstatisticsRx_s {
uint8_t downlink_RSSI_1;
uint8_t downlink_RSSI_1_percentage;
uint8_t downlink_Link_quality;
int8_t downlink_SNR;
uint8_t uplink_power;
} crsfLinkStatisticsRx_t;
typedef struct crsfPayloadLinkstatisticsTx_s {
uint8_t uplink_RSSI;
uint8_t uplink_RSSI_percentage;
uint8_t uplink_Link_quality;
int8_t uplink_SNR;
uint8_t downlink_power;
uint8_t uplink_FPS;
} crsfLinkStatisticsTx_t;
#endif
static timeUs_t lastLinkStatisticsFrameUs;
static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
@ -195,6 +247,31 @@ static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr,
}
}
#if defined(USE_CRSF_V3)
static void handleCrsfLinkStatisticsTxFrame(const crsfLinkStatisticsTx_t* statsPtr, timeUs_t currentTimeUs)
{
const crsfLinkStatisticsTx_t stats = *statsPtr;
lastLinkStatisticsFrameUs = currentTimeUs;
int16_t rssiDbm = -1 * stats.uplink_RSSI;
if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
const uint16_t rssiPercentScaled = stats.uplink_RSSI_percentage;
setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
}
#ifdef USE_RX_RSSI_DBM
if (rxConfig()->crsf_use_rx_snr) {
rssiDbm = stats.uplink_SNR;
}
setRssiDbm(rssiDbm, RSSI_SOURCE_RX_PROTOCOL_CRSF);
#endif
#ifdef USE_RX_LINK_QUALITY_INFO
if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
setLinkQualityDirect(stats.uplink_Link_quality);
}
#endif
}
#endif
#endif
#if defined(USE_CRSF_LINK_STATISTICS)
@ -230,12 +307,25 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
return crc;
}
STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
{
// CRC includes type and payload
uint8_t crc = crc8_poly_0xba(0, crsfFrame.frame.type);
for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC - 1; ++ii) {
crc = crc8_poly_0xba(crc, crsfFrame.frame.payload[ii]);
}
return crc;
}
// Receive ISR callback, called back from serial port
STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
{
UNUSED(data);
static uint8_t crsfFramePosition = 0;
#if defined(USE_CRSF_V3)
static uint32_t crsfFrameErrorCnt = 0;
#endif
const timeUs_t currentTimeUs = microsISR();
#ifdef DEBUG_CRSF_PACKETS
@ -261,9 +351,13 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
crsfFramePosition = 0;
const uint8_t crc = crsfFrameCRC();
if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
#if defined(USE_CRSF_V3)
crsfFrameErrorCnt = 0;
#endif
switch (crsfFrame.frame.type)
{
case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
case CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
lastRcFrameTimeUs = currentTimeUs;
crsfFrameDone = true;
@ -303,13 +397,52 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
}
break;
}
#if defined(USE_CRSF_V3)
case CRSF_FRAMETYPE_LINK_STATISTICS_RX: {
break;
}
case CRSF_FRAMETYPE_LINK_STATISTICS_TX: {
if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
(crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
(crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) {
const crsfLinkStatisticsTx_t* statsFrame = (const crsfLinkStatisticsTx_t*)&crsfFrame.frame.payload;
handleCrsfLinkStatisticsTxFrame(statsFrame, currentTimeUs);
}
break;
}
#endif
#endif
case CRSF_FRAMETYPE_COMMAND:
if ((crsfFrame.bytes[fullFrameLength - 2] == crsfFrameCmdCRC()) &&
(crsfFrame.bytes[3] == CRSF_ADDRESS_FLIGHT_CONTROLLER)) {
crsfProcessCommand(crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE);
}
break;
default:
break;
}
}
else {
#if defined(USE_CRSF_V3)
if (crsfFrameErrorCnt < CRSF_FRAME_ERROR_COUNT_THRESHOLD)
crsfFrameErrorCnt++;
#endif
}
}
else {
#if defined(USE_CRSF_V3)
if (crsfFrameErrorCnt < CRSF_FRAME_ERROR_COUNT_THRESHOLD)
crsfFrameErrorCnt++;
#endif
}
#if defined(USE_CRSF_V3)
if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) {
// fall back to default speed if speed mismatch detected
setCrsfDefaultSpeed();
crsfFrameErrorCnt = 0;
}
#endif
}
}
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
@ -323,6 +456,8 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
crsfFrameDone = false;
// unpack the RC channels
if( crsfChannelDataFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED ) {
// use ordinary RC frame structure (0x16)
const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
crsfChannelData[0] = rcChannels->chan0;
crsfChannelData[1] = rcChannels->chan1;
@ -340,6 +475,39 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
crsfChannelData[13] = rcChannels->chan13;
crsfChannelData[14] = rcChannels->chan14;
crsfChannelData[15] = rcChannels->chan15;
}
#if defined(USE_CRSF_V3)
else {
// use subset RC frame structure (0x17)
uint8_t n;
uint8_t readByte;
uint8_t readByteIndex = 0;
uint8_t bitsMerged = 0;
uint32_t readValue = 0;
uint8_t startChannel = 0xFF;
const uint8_t *payload = crsfChannelDataFrame.frame.payload;
uint8_t numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC) * 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION) / CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION;
for (n = 0; n < numOfChannels; n++) {
while(bitsMerged < CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION) {
readByte = payload[readByteIndex++];
if(startChannel == 0xFF) {
// get the startChannel
startChannel = readByte & CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK;
readByte >>= CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION;
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION;
}
else {
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8;
}
}
crsfChannelData[startChannel + n] = readValue & CRSF_SUBSET_RC_CHANNELS_PACKED_MASK;
readValue >>= CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION;
bitsMerged -= CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION;
}
}
#endif
return RX_FRAME_COMPLETE;
}
return RX_FRAME_PENDING;
@ -348,15 +516,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
{
UNUSED(rxRuntimeState);
/* conversion from RC value to PWM
* RC PWM
* min 172 -> 988us
* mid 992 -> 1500us
* max 1811 -> 2012us
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
*/
return (0.62477120195241f * crsfChannelData[chan]) + 881;
return (crsfChannelData[chan] - 992) * 5 / 8 + 1500;
}
void crsfRxWriteTelemetryData(const void *data, int len)
@ -419,6 +579,13 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
return serialPort != NULL;
}
#if defined(USE_CRSF_V3)
void crsfRxUpdateBaudrate(uint32_t baudrate)
{
serialSetBaudRate(serialPort, baudrate);
}
#endif
bool crsfRxIsActive(void)
{
return serialPort != NULL;

2
src/main/rx/crsf.h
View File

@ -27,6 +27,7 @@
#define CRSF_PORT_MODE MODE_RXTX
#define CRSF_MAX_CHANNEL 16
#define CRSFV3_MAX_CHANNEL 24
#define CRSF_RSSI_MIN (-130)
#define CRSF_RSSI_MAX 0
@ -59,4 +60,5 @@ void crsfRxSendTelemetryData(void);
struct rxConfig_s;
struct rxRuntimeState_s;
bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeState_s *rxRuntimeState);
void crsfRxUpdateBaudrate(uint32_t baudrate);
bool crsfRxIsActive(void);

12
src/main/rx/crsf_protocol.h
View File

@ -39,6 +39,9 @@ typedef enum {
CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED = 0x17,
CRSF_FRAMETYPE_LINK_STATISTICS_RX = 0x1C,
CRSF_FRAMETYPE_LINK_STATISTICS_TX = 0x1D,
CRSF_FRAMETYPE_ATTITUDE = 0x1E,
CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
// Extended Header Frames, range: 0x28 to 0x96
@ -55,6 +58,15 @@ typedef enum {
CRSF_FRAMETYPE_DISPLAYPORT_CMD = 0x7D, // displayport control command
} crsfFrameType_e;
enum {
CRSF_COMMAND_SUBCMD_GENERAL = 0x0A, // general command
};
enum {
CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL = 0x70, // proposed new CRSF port speed
CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE = 0x71, // response to the proposed CRSF port speed
};
enum {
CRSF_DISPLAYPORT_SUBCMD_UPDATE = 0x01, // transmit displayport buffer to remote
CRSF_DISPLAYPORT_SUBCMD_CLEAR = 0X02, // clear client screen

1
src/main/target/common_post.h
View File

@ -108,6 +108,7 @@
#if !defined(USE_SERIALRX_CRSF)
#undef USE_TELEMETRY_CRSF
#undef USE_CRSF_LINK_STATISTICS
#undef USE_CRSF_V3
#undef USE_RX_RSSI_DBM
#endif

1
src/main/target/common_pre.h
View File

@ -348,6 +348,7 @@
#define USE_ESC_SENSOR_INFO
#define USE_CRSF_CMS_TELEMETRY
#define USE_CRSF_LINK_STATISTICS
#define USE_CRSF_V3
#define USE_RX_RSSI_DBM
#endif

118
src/main/telemetry/crsf.c
View File

@ -86,6 +86,42 @@ typedef struct mspBuffer_s {
static mspBuffer_t mspRxBuffer;
bool isCrsfV3Running = false;
#if defined(USE_CRSF_V3)
typedef struct {
uint8_t hasPendingReply:1;
uint8_t isNewSpeedValid:1;
uint8_t portID:3;
uint8_t index;
uint32_t confirmationTime;
} crsfSpeedControl_s;
crsfSpeedControl_s crsfSpeed = {0};
bool checkCrsfV3Running(void)
{
return isCrsfV3Running;
}
bool checkCrsfCustomizedSpeed(void)
{
return crsfSpeed.index < BAUD_COUNT ? true : false;
}
uint32_t getCrsfDesireSpeed(void)
{
return checkCrsfCustomizedSpeed() ? baudRates[crsfSpeed.index] : CRSF_BAUDRATE;
}
void setCrsfDefaultSpeed(void)
{
crsfSpeed.hasPendingReply = false;
crsfSpeed.isNewSpeedValid = true;
crsfSpeed.confirmationTime = 0;
crsfSpeed.index = BAUD_COUNT;
isCrsfV3Running = false;
}
#endif
void initCrsfMspBuffer(void)
{
mspRxBuffer.len = 0;
@ -325,6 +361,42 @@ void crsfFrameDeviceInfo(sbuf_t *dst) {
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
#if defined(USE_CRSF_V3)
void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply) {
uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL);
sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE);
sbufWriteU8(dst, crsfSpeed.portID);
sbufWriteU8(dst, reply);
crc8_poly_0xba_sbuf_append(dst, &lengthPtr[1]);
*lengthPtr = sbufPtr(dst) - lengthPtr;
}
static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart) {
uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
uint8_t ii = 0;
for (ii = 0; ii < BAUD_COUNT; ++ii) {
if(newBaudrate == baudRates[ii]) {
break;
}
}
crsfSpeed.portID = frameStart[1];
crsfSpeed.index = ii;
}
void crsfScheduleSpeedNegotiationResponse(void) {
crsfSpeed.hasPendingReply = true;
crsfSpeed.isNewSpeedValid = false;
}
#endif
#if defined(USE_CRSF_CMS_TELEMETRY)
#define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH 50
#define CRSF_DISPLAYPORT_BATCH_MAX 0x3F
@ -557,6 +629,27 @@ void crsfProcessDisplayPortCmd(uint8_t *frameStart)
#endif
void crsfProcessCommand(uint8_t *frameStart) {
uint8_t cmd = *frameStart;
uint8_t subCmd = frameStart[1];
switch (cmd) {
case CRSF_COMMAND_SUBCMD_GENERAL:
switch (subCmd) {
case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
#if defined(USE_CRSF_V3)
crsfProcessSpeedNegotiationCmd(&frameStart[1]);
crsfScheduleSpeedNegotiationResponse();
#endif
break;
default:
break;
}
break;
default:
break;
}
}
/*
* Called periodically by the scheduler
*/
@ -627,6 +720,31 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_CRSF_V3)
if (crsfSpeed.hasPendingReply) {
bool found = crsfSpeed.index < BAUD_COUNT ? true : false;
sbuf_t crsfSpeedNegotiationBuf;
sbuf_t *dst = &crsfSpeedNegotiationBuf;
crsfInitializeFrame(dst);
crsfFrameSpeedNegotiationResponse(dst, found);
crsfFinalize(dst);
crsfSpeed.hasPendingReply = false;
crsfSpeed.isNewSpeedValid = true;
crsfSpeed.confirmationTime = currentTimeUs;
crsfLastCycleTime = currentTimeUs;
return;
}
else if (crsfSpeed.isNewSpeedValid) {
if (currentTimeUs - crsfSpeed.confirmationTime >= 10000) {
// delay 10ms before applying the new baudrate
crsfRxUpdateBaudrate(getCrsfDesireSpeed());
crsfSpeed.isNewSpeedValid = false;
isCrsfV3Running = true;
return;
}
}
#endif
// Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
// Spread out scheduled frames evenly so each frame is sent at the same frequency.
if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {

5
src/main/telemetry/crsf.h
View File

@ -31,10 +31,15 @@
#define CRSF_MSP_TX_BUF_SIZE 128
void initCrsfTelemetry(void);
bool checkCrsfV3Running(void);
uint32_t getCrsfDesireSpeed(void);
void setCrsfDefaultSpeed(void);
bool checkCrsfTelemetryState(void);
void handleCrsfTelemetry(timeUs_t currentTimeUs);
void crsfScheduleDeviceInfoResponse(void);
void crsfScheduleMspResponse(void);
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
void crsfProcessCommand(uint8_t *frameStart);
#if defined(USE_CRSF_CMS_TELEMETRY)
void crsfProcessDisplayPortCmd(uint8_t *frameStart);
#endif