Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

cosmetics

Browse source
This commit is contained in:
tbs-fpv 2021-04-26 17:08:03 +08:00
parent 483b3cc5a7
commit e24d55f0a7
2 changed files with 15 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

9
src/main/rx/crsf.c
View file

@ -457,7 +457,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
crsfFrameDone = false; crsfFrameDone = false;
// unpack the RC channels // unpack the RC channels
if( crsfChannelDataFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED ) { if (crsfChannelDataFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
// use ordinary RC frame structure (0x16) // use ordinary RC frame structure (0x16)
const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload; const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
crsfChannelData[0] = rcChannels->chan0; crsfChannelData[0] = rcChannels->chan0;
@ -489,16 +489,15 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
const uint8_t *payload = crsfChannelDataFrame.frame.payload; 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 = ((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++) { for (n = 0; n < numOfChannels; n++) {
while(bitsMerged < CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION) { while (bitsMerged < CRSF_SUBSET_RC_CHANNELS_PACKED_RESOLUTION) {
readByte = payload[readByteIndex++]; readByte = payload[readByteIndex++];
if(startChannel == 0xFF) { if (startChannel == 0xFF) {
// get the startChannel // get the startChannel
startChannel = readByte & CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK; startChannel = readByte & CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_MASK;
readByte >>= CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION; readByte >>= CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION;
readValue |= ((uint32_t) readByte) << bitsMerged; readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION; bitsMerged += 8 - CRSF_SUBSET_RC_CHANNELS_PACKED_STARTING_CHANNEL_RESOLUTION;
} } else {
else {
readValue |= ((uint32_t) readByte) << bitsMerged; readValue |= ((uint32_t) readByte) << bitsMerged;
bitsMerged += 8; bitsMerged += 8;
} }

23
src/main/telemetry/crsf.c
View file

@ -86,8 +86,8 @@ typedef struct mspBuffer_s {
static mspBuffer_t mspRxBuffer; static mspBuffer_t mspRxBuffer;
bool isCrsfV3Running = false;
#if defined(USE_CRSF_V3) #if defined(USE_CRSF_V3)
static bool isCrsfV3Running = false;
typedef struct { typedef struct {
uint8_t hasPendingReply:1; uint8_t hasPendingReply:1;
uint8_t isNewSpeedValid:1; uint8_t isNewSpeedValid:1;
@ -634,20 +634,20 @@ void crsfProcessCommand(uint8_t *frameStart) {
uint8_t cmd = *frameStart; uint8_t cmd = *frameStart;
uint8_t subCmd = frameStart[1]; uint8_t subCmd = frameStart[1];
switch (cmd) { switch (cmd) {
case CRSF_COMMAND_SUBCMD_GENERAL: case CRSF_COMMAND_SUBCMD_GENERAL:
switch (subCmd) { switch (subCmd) {
case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL: case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
#if defined(USE_CRSF_V3) #if defined(USE_CRSF_V3)
crsfProcessSpeedNegotiationCmd(&frameStart[1]); crsfProcessSpeedNegotiationCmd(&frameStart[1]);
crsfScheduleSpeedNegotiationResponse(); crsfScheduleSpeedNegotiationResponse();
#endif #endif
break;
default:
break;
}
break; break;
default: default:
break; break;
}
break;
default:
break;
} }
} }
#endif #endif
@ -735,8 +735,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
crsfSpeed.confirmationTime = currentTimeUs; crsfSpeed.confirmationTime = currentTimeUs;
crsfLastCycleTime = currentTimeUs; crsfLastCycleTime = currentTimeUs;
return; return;
} } else if (crsfSpeed.isNewSpeedValid) {
else if (crsfSpeed.isNewSpeedValid) {
if (currentTimeUs - crsfSpeed.confirmationTime >= 10000) { if (currentTimeUs - crsfSpeed.confirmationTime >= 10000) {
// delay 10ms before applying the new baudrate // delay 10ms before applying the new baudrate
crsfRxUpdateBaudrate(getCrsfDesireSpeed()); crsfRxUpdateBaudrate(getCrsfDesireSpeed());