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

support channel resolution configuration for 0x17

This commit is contained in:
tbs-fpv 2021-04-28 11:21:30 +08:00
parent 4d8845b8da
commit fbd01551ff
3 changed files with 166 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -69,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 uint8_t channelRes = CRSF_SUBSET_RC_RES_CONF_11B;
static timeUs_t lastRcFrameTimeUs = 0;
@ -127,13 +128,24 @@ 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_DIGITAL_SWITCH_RESOLUTION 1
/*
* 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)
/*
@ -489,7 +501,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
crsfChannelData[14] = rcChannels->chan14;
crsfChannelData[15] = rcChannels->chan15;
}
#if defined(USE_CRSF_V3)
else {
// use subset RC frame structure (0x17)
uint8_t n;
@ -497,29 +508,61 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
uint8_t readByteIndex = 0;
uint8_t bitsMerged = 0;
uint32_t readValue = 0;
uint8_t configByte = 0xFF;
uint8_t startChannel = 0xFF;
uint8_t channelBits = 0xFF;
uint16_t channelMask = 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;
uint8_t numOfChannels = 0;
// get the configuration byte
configByte = payload[readByteIndex++];
// get the channel number of start channel
startChannel = configByte & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK;
configByte >>= CRSF_SUBSET_RC_STARTING_CHANNEL_BITS;
// get the channel resolution settings
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;
break;
default:
case CRSF_SUBSET_RC_RES_CONF_11B:
channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
break;
case CRSF_SUBSET_RC_RES_CONF_12B:
channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
break;
case CRSF_SUBSET_RC_RES_CONF_13B:
channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
break;
}
// do nothing for the reserved configuration bit
configByte >>= CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS;
// calculate the number of channels packed
numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits;
// unpack the channel data
for (n = 0; n < numOfChannels; n++) {
while (bitsMerged < CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION) {
while (bitsMerged < channelBits) {
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 + CRSF_SUBSET_RC_CHANNELS_PACKED_DIGITAL_SWITCH_RESOLUTION);
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION - CRSF_SUBSET_RC_CHANNELS_PACKED_DIGITAL_SWITCH_RESOLUTION;
} else {
readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8;
}
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;
crsfChannelData[startChannel + n] = readValue & channelMask;
readValue >>= channelBits;
bitsMerged -= channelBits;
}
}
#endif
return RX_FRAME_COMPLETE;
}
return RX_FRAME_PENDING;
@ -528,15 +571,39 @@ 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 (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
/* 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 (0.62477120195241f * (float)crsfChannelData[chan]) + 881;
} else {
/* conversion from RC value to PWM
* for 0x17 Subset RC frame
*/
float scale;
switch (channelRes) {
case CRSF_SUBSET_RC_RES_CONF_10B:
scale = 1.0f;
break;
default:
case CRSF_SUBSET_RC_RES_CONF_11B:
scale = 0.5f;
break;
case CRSF_SUBSET_RC_RES_CONF_12B:
scale = 0.25f;
break;
case CRSF_SUBSET_RC_RES_CONF_13B:
scale = 0.125f;
break;
}
return (scale * (float)crsfChannelData[chan]) + 988;
}
}
void crsfRxWriteTelemetryData(const void *data, int len)

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

@ -29,6 +29,25 @@
#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_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_RES_CONF_11B 1
#define CRSF_SUBSET_RC_RES_BITS_11B 11
#define CRSF_SUBSET_RC_RES_MASK_11B 0x07FF
#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_RES_CONF_13B 3
#define CRSF_SUBSET_RC_RES_BITS_13B 13
#define CRSF_SUBSET_RC_RES_MASK_13B 0x1FFF
#define CRSF_RSSI_MIN (-130)
#define CRSF_RSSI_MAX 0
#define CRSF_SNR_MIN (-30)

47
src/test/unit/rx_crsf_unittest.cc
View File

@ -220,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,
@ -279,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)
{