support CRSF V3 protocol
This commit is contained in:
parent
025ee87a7a
commit
ee2179b836
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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[];
|
||||
|
|
|
@ -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,12 +397,51 @@ 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
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -323,23 +456,58 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
crsfFrameDone = false;
|
||||
|
||||
// unpack the RC channels
|
||||
const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
|
||||
crsfChannelData[0] = rcChannels->chan0;
|
||||
crsfChannelData[1] = rcChannels->chan1;
|
||||
crsfChannelData[2] = rcChannels->chan2;
|
||||
crsfChannelData[3] = rcChannels->chan3;
|
||||
crsfChannelData[4] = rcChannels->chan4;
|
||||
crsfChannelData[5] = rcChannels->chan5;
|
||||
crsfChannelData[6] = rcChannels->chan6;
|
||||
crsfChannelData[7] = rcChannels->chan7;
|
||||
crsfChannelData[8] = rcChannels->chan8;
|
||||
crsfChannelData[9] = rcChannels->chan9;
|
||||
crsfChannelData[10] = rcChannels->chan10;
|
||||
crsfChannelData[11] = rcChannels->chan11;
|
||||
crsfChannelData[12] = rcChannels->chan12;
|
||||
crsfChannelData[13] = rcChannels->chan13;
|
||||
crsfChannelData[14] = rcChannels->chan14;
|
||||
crsfChannelData[15] = rcChannels->chan15;
|
||||
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;
|
||||
crsfChannelData[2] = rcChannels->chan2;
|
||||
crsfChannelData[3] = rcChannels->chan3;
|
||||
crsfChannelData[4] = rcChannels->chan4;
|
||||
crsfChannelData[5] = rcChannels->chan5;
|
||||
crsfChannelData[6] = rcChannels->chan6;
|
||||
crsfChannelData[7] = rcChannels->chan7;
|
||||
crsfChannelData[8] = rcChannels->chan8;
|
||||
crsfChannelData[9] = rcChannels->chan9;
|
||||
crsfChannelData[10] = rcChannels->chan10;
|
||||
crsfChannelData[11] = rcChannels->chan11;
|
||||
crsfChannelData[12] = rcChannels->chan12;
|
||||
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;
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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
|
||||
|
||||
|
|
|
@ -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)) {
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue