mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
Merge pull request #10675 from tbs-fpv/tbs/crsfv3
This commit is contained in:
Michael Keller
GitHub
commit
80a6ab53bb
13 changed files with 555 additions and 72 deletions
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -94,6 +94,7 @@
|
|||
#include "scheduler/scheduler.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/crsf.h"
|
||||
|
||||
#ifdef USE_BST
|
||||
#include "i2c_bst.h"
|
||||
|
|
@ -409,6 +410,11 @@ void tasksInit(void)
|
|||
#ifdef USE_RCDEVICE
|
||||
setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled());
|
||||
#endif
|
||||
|
||||
#ifdef USE_CRSF_V3
|
||||
const bool useCRSF = rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
|
||||
setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF);
|
||||
#endif
|
||||
}
|
||||
|
||||
#if defined(USE_TASK_STATISTICS)
|
||||
|
|
@ -527,6 +533,10 @@ task_t tasks[TASK_COUNT] = {
|
|||
#ifdef USE_RANGEFINDER
|
||||
[TASK_RANGEFINDER] = DEFINE_TASK("RANGEFINDER", NULL, NULL, taskUpdateRangefinder, TASK_PERIOD_HZ(10), TASK_PRIORITY_IDLE),
|
||||
#endif
|
||||
|
||||
#ifdef USE_CRSF_V3
|
||||
[TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_IDLE),
|
||||
#endif
|
||||
};
|
||||
|
||||
task_t *getTask(unsigned taskId)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
@ -67,6 +69,7 @@ static serialPort_t *serialPort;
|
|||
static timeUs_t crsfFrameStartAtUs = 0;
|
||||
static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
|
||||
static uint8_t telemetryBufLen = 0;
|
||||
static float channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
|
||||
|
||||
static timeUs_t lastRcFrameTimeUs = 0;
|
||||
|
||||
|
|
@ -125,6 +128,25 @@ struct crsfPayloadRcChannelsPacked_s {
|
|||
|
||||
typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
|
||||
|
||||
/*
|
||||
* SUBSET RC FRAME 0x17
|
||||
*
|
||||
* The structure of 0x17 frame consists of 8-bit configuration data & variable length packed channel data.
|
||||
*
|
||||
* definition of the configuration byte
|
||||
* bits 0-4: number of first channel packed
|
||||
* bits 5-6: resolution configuration of the channel data (00 -> 10 bits, 01 -> 11 bits, 10 -> 12 bits, 11 -> 13 bits)
|
||||
* bit 7: reserved
|
||||
|
||||
* data structure of the channel data
|
||||
* - first channel packed with specified resolution
|
||||
* - second channel packed with specified resolution
|
||||
* - third channel packed with specified resolution
|
||||
* ...
|
||||
* - last channel packed with specified resolution
|
||||
*/
|
||||
|
||||
|
||||
#if defined(USE_CRSF_LINK_STATISTICS)
|
||||
/*
|
||||
* 0x14 Link statistics
|
||||
|
|
@ -143,6 +165,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;
|
||||
|
|
@ -156,6 +201,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; // this struct is currently not used
|
||||
|
||||
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; // currently not used
|
||||
uint8_t uplink_FPS; // currently not used
|
||||
} crsfLinkStatisticsTx_t;
|
||||
#endif
|
||||
|
||||
static timeUs_t lastLinkStatisticsFrameUs;
|
||||
|
||||
static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
|
||||
|
|
@ -206,6 +270,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)
|
||||
|
|
@ -241,12 +330,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 uint8_t crsfFrameErrorCnt = 0;
|
||||
#endif
|
||||
const timeUs_t currentTimeUs = microsISR();
|
||||
|
||||
#ifdef DEBUG_CRSF_PACKETS
|
||||
|
|
@ -272,54 +374,96 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
|
|||
crsfFramePosition = 0;
|
||||
const uint8_t crc = crsfFrameCRC();
|
||||
if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
|
||||
switch (crsfFrame.frame.type)
|
||||
{
|
||||
case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
|
||||
if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
|
||||
lastRcFrameTimeUs = currentTimeUs;
|
||||
crsfFrameDone = true;
|
||||
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
|
||||
}
|
||||
break;
|
||||
#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;
|
||||
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
|
||||
}
|
||||
break;
|
||||
|
||||
#if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY)
|
||||
case CRSF_FRAMETYPE_MSP_REQ:
|
||||
case CRSF_FRAMETYPE_MSP_WRITE: {
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
|
||||
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
|
||||
crsfScheduleMspResponse();
|
||||
}
|
||||
break;
|
||||
case CRSF_FRAMETYPE_MSP_REQ:
|
||||
case CRSF_FRAMETYPE_MSP_WRITE: {
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
|
||||
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
|
||||
crsfScheduleMspResponse();
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_CRSF_CMS_TELEMETRY)
|
||||
case CRSF_FRAMETYPE_DEVICE_PING:
|
||||
crsfScheduleDeviceInfoResponse();
|
||||
break;
|
||||
case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
|
||||
crsfProcessDisplayPortCmd(frameStart);
|
||||
break;
|
||||
}
|
||||
case CRSF_FRAMETYPE_DEVICE_PING:
|
||||
crsfScheduleDeviceInfoResponse();
|
||||
break;
|
||||
case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
|
||||
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
|
||||
crsfProcessDisplayPortCmd(frameStart);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#if defined(USE_CRSF_LINK_STATISTICS)
|
||||
|
||||
case CRSF_FRAMETYPE_LINK_STATISTICS: {
|
||||
// if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
|
||||
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 crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
|
||||
handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
|
||||
}
|
||||
break;
|
||||
case CRSF_FRAMETYPE_LINK_STATISTICS: {
|
||||
// if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
|
||||
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 crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
|
||||
handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
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_TX_PAYLOAD_SIZE)) {
|
||||
const crsfLinkStatisticsTx_t* statsFrame = (const crsfLinkStatisticsTx_t*)&crsfFrame.frame.payload;
|
||||
handleCrsfLinkStatisticsTxFrame(statsFrame, currentTimeUs);
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
#if defined(USE_CRSF_V3)
|
||||
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;
|
||||
#endif
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -334,23 +478,87 @@ 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;
|
||||
channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
|
||||
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;
|
||||
} else {
|
||||
// use subset RC frame structure (0x17)
|
||||
uint8_t readByteIndex = 0;
|
||||
const uint8_t *payload = crsfChannelDataFrame.frame.payload;
|
||||
|
||||
// get the configuration byte
|
||||
uint8_t configByte = payload[readByteIndex++];
|
||||
|
||||
// get the channel number of start channel
|
||||
uint8_t startChannel = configByte & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK;
|
||||
configByte >>= CRSF_SUBSET_RC_STARTING_CHANNEL_BITS;
|
||||
|
||||
// get the channel resolution settings
|
||||
uint8_t channelBits;
|
||||
uint16_t channelMask;
|
||||
uint8_t channelRes = configByte & CRSF_SUBSET_RC_RES_CONFIGURATION_MASK;
|
||||
configByte >>= CRSF_SUBSET_RC_RES_CONFIGURATION_BITS;
|
||||
switch (channelRes) {
|
||||
case CRSF_SUBSET_RC_RES_CONF_10B:
|
||||
channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
|
||||
channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
|
||||
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
|
||||
break;
|
||||
default:
|
||||
case CRSF_SUBSET_RC_RES_CONF_11B:
|
||||
channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
|
||||
channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
|
||||
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
|
||||
break;
|
||||
case CRSF_SUBSET_RC_RES_CONF_12B:
|
||||
channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
|
||||
channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
|
||||
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
|
||||
break;
|
||||
case CRSF_SUBSET_RC_RES_CONF_13B:
|
||||
channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
|
||||
channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
|
||||
channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
|
||||
break;
|
||||
}
|
||||
|
||||
// do nothing for the reserved configuration bit
|
||||
configByte >>= CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS;
|
||||
|
||||
// calculate the number of channels packed
|
||||
uint8_t numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits;
|
||||
|
||||
// unpack the channel data
|
||||
uint8_t bitsMerged = 0;
|
||||
uint32_t readValue = 0;
|
||||
for (uint8_t n = 0; n < numOfChannels; n++) {
|
||||
while (bitsMerged < channelBits) {
|
||||
uint8_t readByte = payload[readByteIndex++];
|
||||
readValue |= ((uint32_t) readByte) << bitsMerged;
|
||||
bitsMerged += 8;
|
||||
}
|
||||
crsfChannelData[startChannel + n] = readValue & channelMask;
|
||||
readValue >>= channelBits;
|
||||
bitsMerged -= channelBits;
|
||||
}
|
||||
}
|
||||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
return RX_FRAME_PENDING;
|
||||
|
|
@ -359,15 +567,23 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
STATIC_UNIT_TESTED float 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 * (float)crsfChannelData[chan]) + 881;
|
||||
if (channelScale == CRSF_RC_CHANNEL_SCALE_LEGACY) {
|
||||
/* conversion from RC value to PWM
|
||||
* for 0x16 RC frame
|
||||
* 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 (channelScale * (float)crsfChannelData[chan]) + 881;
|
||||
} else {
|
||||
/* conversion from RC value to PWM
|
||||
* for 0x17 Subset RC frame
|
||||
*/
|
||||
return (channelScale * (float)crsfChannelData[chan]) + 988;
|
||||
}
|
||||
}
|
||||
|
||||
void crsfRxWriteTelemetryData(const void *data, int len)
|
||||
|
|
@ -430,6 +646,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,31 @@
|
|||
#define CRSF_PORT_MODE MODE_RXTX
|
||||
|
||||
#define CRSF_MAX_CHANNEL 16
|
||||
#define CRSFV3_MAX_CHANNEL 24
|
||||
|
||||
#define CRSF_SUBSET_RC_STARTING_CHANNEL_BITS 5
|
||||
#define CRSF_SUBSET_RC_STARTING_CHANNEL_MASK 0x1F
|
||||
#define CRSF_SUBSET_RC_RES_CONFIGURATION_BITS 2
|
||||
#define CRSF_SUBSET_RC_RES_CONFIGURATION_MASK 0x03
|
||||
#define CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS 1
|
||||
|
||||
#define CRSF_RC_CHANNEL_SCALE_LEGACY 0.62477120195241f
|
||||
#define CRSF_SUBSET_RC_RES_CONF_10B 0
|
||||
#define CRSF_SUBSET_RC_RES_BITS_10B 10
|
||||
#define CRSF_SUBSET_RC_RES_MASK_10B 0x03FF
|
||||
#define CRSF_SUBSET_RC_CHANNEL_SCALE_10B 1.0f
|
||||
#define CRSF_SUBSET_RC_RES_CONF_11B 1
|
||||
#define CRSF_SUBSET_RC_RES_BITS_11B 11
|
||||
#define CRSF_SUBSET_RC_RES_MASK_11B 0x07FF
|
||||
#define CRSF_SUBSET_RC_CHANNEL_SCALE_11B 0.5f
|
||||
#define CRSF_SUBSET_RC_RES_CONF_12B 2
|
||||
#define CRSF_SUBSET_RC_RES_BITS_12B 12
|
||||
#define CRSF_SUBSET_RC_RES_MASK_12B 0x0FFF
|
||||
#define CRSF_SUBSET_RC_CHANNEL_SCALE_12B 0.25f
|
||||
#define CRSF_SUBSET_RC_RES_CONF_13B 3
|
||||
#define CRSF_SUBSET_RC_RES_BITS_13B 13
|
||||
#define CRSF_SUBSET_RC_RES_MASK_13B 0x1FFF
|
||||
#define CRSF_SUBSET_RC_CHANNEL_SCALE_13B 0.125f
|
||||
|
||||
#define CRSF_RSSI_MIN (-130)
|
||||
#define CRSF_RSSI_MAX 0
|
||||
|
|
@ -59,4 +84,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
|
||||
|
|
@ -72,6 +84,7 @@ enum {
|
|||
CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
|
||||
CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
|
||||
CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10,
|
||||
CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6,
|
||||
CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes.
|
||||
CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
|
||||
};
|
||||
|
|
|
|||
|
|
@ -150,6 +150,10 @@ typedef enum {
|
|||
TASK_PINIOBOX,
|
||||
#endif
|
||||
|
||||
#ifdef USE_CRSF_V3
|
||||
TASK_SPEED_NEGOTIATION,
|
||||
#endif
|
||||
|
||||
/* Count of real tasks */
|
||||
TASK_COUNT,
|
||||
|
||||
|
|
|
|||
|
|
@ -107,6 +107,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
|
||||
|
||||
|
|
|
|||
|
|
@ -396,6 +396,7 @@
|
|||
#define USE_SIMPLIFIED_TUNING
|
||||
#define USE_RX_LINK_UPLINK_POWER
|
||||
#define USE_GPS_PLUS_CODES
|
||||
#define USE_CRSF_V3
|
||||
#endif
|
||||
|
||||
#if (TARGET_FLASH_SIZE > 512)
|
||||
|
|
|
|||
|
|
@ -87,6 +87,39 @@ typedef struct mspBuffer_s {
|
|||
|
||||
static mspBuffer_t mspRxBuffer;
|
||||
|
||||
#if defined(USE_CRSF_V3)
|
||||
static bool isCrsfV3Running = false;
|
||||
typedef struct {
|
||||
uint8_t hasPendingReply:1;
|
||||
uint8_t isNewSpeedValid:1;
|
||||
uint8_t portID:3;
|
||||
uint8_t index;
|
||||
uint32_t confirmationTime;
|
||||
} crsfSpeedControl_s;
|
||||
|
||||
static crsfSpeedControl_s crsfSpeed = {0};
|
||||
|
||||
bool checkCrsfCustomizedSpeed(void)
|
||||
{
|
||||
return crsfSpeed.index < BAUD_COUNT ? true : false;
|
||||
}
|
||||
|
||||
uint32_t getCrsfDesiredSpeed(void)
|
||||
{
|
||||
return checkCrsfCustomizedSpeed() ? baudRates[crsfSpeed.index] : CRSF_BAUDRATE;
|
||||
}
|
||||
|
||||
void setCrsfDefaultSpeed(void)
|
||||
{
|
||||
crsfSpeed.hasPendingReply = false;
|
||||
crsfSpeed.isNewSpeedValid = false;
|
||||
crsfSpeed.confirmationTime = 0;
|
||||
crsfSpeed.index = BAUD_COUNT;
|
||||
isCrsfV3Running = false;
|
||||
crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
|
||||
}
|
||||
#endif
|
||||
|
||||
void initCrsfMspBuffer(void)
|
||||
{
|
||||
mspRxBuffer.len = 0;
|
||||
|
|
@ -326,6 +359,80 @@ 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;
|
||||
}
|
||||
|
||||
void speedNegotiationProcess(uint32_t currentTime)
|
||||
{
|
||||
if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesiredSpeed() == CRSF_BAUDRATE) {
|
||||
// to notify the RX to fall back to default baud rate by sending device info frame if telemetry is disabled
|
||||
sbuf_t crsfPayloadBuf;
|
||||
sbuf_t *dst = &crsfPayloadBuf;
|
||||
crsfInitializeFrame(dst);
|
||||
crsfFrameDeviceInfo(dst);
|
||||
crsfFinalize(dst);
|
||||
crsfRxSendTelemetryData();
|
||||
} else {
|
||||
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);
|
||||
crsfRxSendTelemetryData();
|
||||
crsfSpeed.hasPendingReply = false;
|
||||
crsfSpeed.isNewSpeedValid = true;
|
||||
crsfSpeed.confirmationTime = currentTime;
|
||||
return;
|
||||
} else if (crsfSpeed.isNewSpeedValid) {
|
||||
if (currentTime - crsfSpeed.confirmationTime >= 4000) {
|
||||
// delay 4ms before applying the new baudrate
|
||||
crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
|
||||
crsfSpeed.isNewSpeedValid = false;
|
||||
isCrsfV3Running = true;
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_CRSF_CMS_TELEMETRY)
|
||||
#define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH 50
|
||||
#define CRSF_DISPLAYPORT_BATCH_MAX 0x3F
|
||||
|
|
@ -562,6 +669,28 @@ void crsfProcessDisplayPortCmd(uint8_t *frameStart)
|
|||
|
||||
#endif
|
||||
|
||||
#if defined(USE_CRSF_V3)
|
||||
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:
|
||||
crsfProcessSpeedNegotiationCmd(&frameStart[1]);
|
||||
crsfScheduleSpeedNegotiationResponse();
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Called periodically by the scheduler
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -31,10 +31,14 @@
|
|||
#define CRSF_MSP_TX_BUF_SIZE 128
|
||||
|
||||
void initCrsfTelemetry(void);
|
||||
uint32_t getCrsfDesiredSpeed(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
|
||||
|
|
@ -42,3 +46,6 @@ void crsfProcessDisplayPortCmd(uint8_t *frameStart);
|
|||
void initCrsfMspBuffer(void);
|
||||
bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength);
|
||||
#endif
|
||||
#if defined(USE_CRSF_V3)
|
||||
void speedNegotiationProcess(uint32_t currentTime);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@ extern "C" {
|
|||
|
||||
void crsfDataReceive(uint16_t c);
|
||||
uint8_t crsfFrameCRC(void);
|
||||
uint8_t crsfFrameCmdCRC(void);
|
||||
uint8_t crsfFrameStatus(void);
|
||||
float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
|
||||
|
||||
|
|
@ -140,6 +141,20 @@ TEST(CrossFireTest, TestCrsfFrameStatus)
|
|||
}
|
||||
}
|
||||
|
||||
const uint8_t buadrateNegotiationFrame[] = {
|
||||
0xC8,0x0C,0x32,0xC8,0xEC,0x0A,0x70,0x01,0x00,0x1E,0x84,0x80,0x22,0x72
|
||||
};
|
||||
|
||||
TEST(CrossFireTest, TestCrsfCmdFrameCrc)
|
||||
{
|
||||
crsfFrame = *(const crsfFrame_t*)buadrateNegotiationFrame;
|
||||
crsfFrameDone = true;
|
||||
const uint8_t crsfCmdFrameCrc = crsfFrameCmdCRC();
|
||||
const uint8_t crsfFrameCrc = crsfFrameCRC();
|
||||
EXPECT_EQ(crsfCmdFrameCrc, crsfFrame.frame.payload[crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_ADDRESS - CRSF_FRAME_LENGTH_FRAMELENGTH - 1]);
|
||||
EXPECT_EQ(crsfFrameCrc, crsfFrame.frame.payload[crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_ADDRESS - CRSF_FRAME_LENGTH_FRAMELENGTH]);
|
||||
}
|
||||
|
||||
/*
|
||||
* Frame is of form
|
||||
* <Device address> <Frame length> < Type> <Payload> < CRC>
|
||||
|
|
@ -205,6 +220,7 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
|
|||
EXPECT_EQ(0, crsfChannelData[15]);
|
||||
}
|
||||
|
||||
// example of 0x16 RC frame
|
||||
const uint8_t capturedData[] = {
|
||||
0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAE,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x6C,
|
||||
0x00,0x18,0x16,0xBD,0x08,0x9F,0xF4,0xAA,0xF7,0xBD,0xEF,0x7D,0xEF,0xFB,0xAD,0xFD,0x45,0x2B,0x5A,0x01,0x00,0x00,0x00,0x00,0x00,0x94,
|
||||
|
|
@ -264,6 +280,52 @@ TEST(CrossFireTest, TestCapturedData)
|
|||
EXPECT_EQ(crc, crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]);
|
||||
}
|
||||
|
||||
// example of 0x17 Subset RC frame
|
||||
/* Notes of Frame Contents
|
||||
* frame type = 0x17 Subset RC Frame
|
||||
* first channel packed = 4, bits 0-4
|
||||
* channel resolution = 0x01 = 11-bit, bits 5-6
|
||||
* reserved configuration = 0, bit 7
|
||||
* first channel packed (Ch4) = 0 = 0x000, 000 0000 0000, bits 8 - 18
|
||||
* second channel packed (Ch5) = 820 = 0x334, 011 0011 0100, bits 19 - 29
|
||||
* third channel packed (Ch6) = 1959 = 0x7A7, 111 1010 0111, bits 30 - 40
|
||||
* fourth channel packed (Ch7) = 2047 = 0x7FF, 111 1111 1111, bits 41 - 51
|
||||
*/
|
||||
const uint8_t capturedSubsetData[] = {
|
||||
0xC8,0x09,0x17,0x24,0x00,0xA0,0xD9,0xE9,0xFF,0x0F,0xD1
|
||||
};
|
||||
|
||||
TEST(CrossFireTest, TestCapturedSubsetData)
|
||||
{
|
||||
crsfFrame = *(const crsfFrame_t*)capturedSubsetData;
|
||||
crsfFrameDone = true;
|
||||
memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
|
||||
|
||||
uint8_t status = crsfFrameStatus();
|
||||
EXPECT_EQ(RX_FRAME_COMPLETE, status);
|
||||
EXPECT_FALSE(crsfFrameDone);
|
||||
EXPECT_EQ(CRSF_SYNC_BYTE, crsfFrame.frame.deviceAddress);
|
||||
EXPECT_EQ(CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED, crsfFrame.frame.type);
|
||||
|
||||
uint8_t crc = crsfFrameCRC();
|
||||
uint8_t startChannel = crsfFrame.frame.payload[0] & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK;
|
||||
uint8_t channelRes = (crsfFrame.frame.payload[0] >> CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) & CRSF_SUBSET_RC_RES_CONFIGURATION_MASK;
|
||||
uint8_t reservedBit = (crsfFrame.frame.payload[0] >> (CRSF_SUBSET_RC_STARTING_CHANNEL_BITS + CRSF_SUBSET_RC_RES_CONFIGURATION_MASK)) & CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS;
|
||||
EXPECT_EQ(crc, crsfFrame.frame.payload[crsfFrame.frame.frameLength - 2]);
|
||||
EXPECT_EQ(4, startChannel);
|
||||
EXPECT_EQ(1, channelRes);
|
||||
EXPECT_EQ(0, reservedBit);
|
||||
|
||||
EXPECT_EQ(0, crsfChannelData[4]);
|
||||
EXPECT_EQ(820, crsfChannelData[5]);
|
||||
EXPECT_EQ(1959, crsfChannelData[6]);
|
||||
EXPECT_EQ(2047, crsfChannelData[7]);
|
||||
|
||||
EXPECT_EQ(988, (uint16_t)crsfReadRawRC(NULL, 4));
|
||||
EXPECT_EQ(1398, (uint16_t)crsfReadRawRC(NULL, 5));
|
||||
EXPECT_EQ(1967, (uint16_t)crsfReadRawRC(NULL, 6));
|
||||
EXPECT_EQ(2011, (uint16_t)crsfReadRawRC(NULL, 7));
|
||||
}
|
||||
|
||||
TEST(CrossFireTest, TestCrsfDataReceive)
|
||||
{
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue