src/main/rx/crsf.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_SERIALRX_CRSF
  29
  30 #include "build/build_config.h"
  31 #include "build/debug.h"
  32
  33 #include "common/crc.h"
  34 #include "common/maths.h"
  35 #include "common/utils.h"
  36
  37 #include "config/config.h"
  38
  39 #include "pg/rx.h"
  40
  41 #include "drivers/persistent.h"
  42 #include "drivers/serial.h"
  43 #include "drivers/serial_uart.h"
  44 #include "drivers/system.h"
  45 #include "drivers/time.h"
  46
  47 #include "io/serial.h"
  48
  49 #include "rx/rx.h"
  50 #include "rx/crsf.h"
  51
  52 #include "telemetry/crsf.h"
  53
  54 #define CRSF_TIME_NEEDED_PER_FRAME_US   1750 // a maximally sized 64byte payload will take ~1550us, round up to 1750.
  55 #define CRSF_TIME_BETWEEN_FRAMES_US     6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
  56
  57 #define CRSF_DIGITAL_CHANNEL_MIN 172
  58 #define CRSF_DIGITAL_CHANNEL_MAX 1811
  59
  60 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
  61
  62 #define CRSF_LINK_STATUS_UPDATE_TIMEOUT_US  250000 // 250ms, 4 Hz mode 1 telemetry
  63
  64 #define CRSF_FRAME_ERROR_COUNT_THRESHOLD    3
  65
  66 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  67 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  68 STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame;
  69 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  70
  71 static serialPort_t *serialPort;
  72 static timeUs_t crsfFrameStartAtUs = 0;
  73 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  74 static uint8_t telemetryBufLen = 0;
  75 static float channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
  76
  77 #ifdef USE_RX_LINK_UPLINK_POWER
  78 #define CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT 9
  79 // Uplink power levels by uplinkTXPower expressed in mW (250 mW is from ver >=4.00, 50 mW in a future version and for ExpressLRS)
  80 const uint16_t uplinkTXPowerStatesMw[CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};
  81 #endif
  82
  83 /*
  84  * CRSF protocol
  85  *
  86  * CRSF protocol uses a single wire half duplex uart connection.
  87  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  88  *
  89  * 420000 baud
  90  * not inverted
  91  * 8 Bit
  92  * 1 Stop bit
  93  * Big endian
  94  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  95  * Max frame size is 64 bytes
  96  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  97  *
  98  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  99  *
 100  * Every frame has the structure:
 101  * <Device address><Frame length><Type><Payload><CRC>
 102  *
 103  * Device address: (uint8_t)
 104  * Frame length:   length in  bytes including Type (uint8_t)
 105  * Type:           (uint8_t)
 106  * CRC:            (uint8_t)
 107  *
 108  */
 109
 110 struct crsfPayloadRcChannelsPacked_s {
 111     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
 112     unsigned int chan0 : 11;
 113     unsigned int chan1 : 11;
 114     unsigned int chan2 : 11;
 115     unsigned int chan3 : 11;
 116     unsigned int chan4 : 11;
 117     unsigned int chan5 : 11;
 118     unsigned int chan6 : 11;
 119     unsigned int chan7 : 11;
 120     unsigned int chan8 : 11;
 121     unsigned int chan9 : 11;
 122     unsigned int chan10 : 11;
 123     unsigned int chan11 : 11;
 124     unsigned int chan12 : 11;
 125     unsigned int chan13 : 11;
 126     unsigned int chan14 : 11;
 127     unsigned int chan15 : 11;
 128 } __attribute__ ((__packed__));
 129
 130 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 131
 132 /*
 133 * SUBSET RC FRAME 0x17
 134 *
 135 * The structure of 0x17 frame consists of 8-bit configuration data & variable length packed channel data.
 136 *
 137 * definition of the configuration byte
 138 * bits 0-4: number of first channel packed
 139 * bits 5-6: resolution configuration of the channel data (00 -> 10 bits, 01 -> 11 bits, 10 -> 12 bits, 11 -> 13 bits)
 140 * bit 7:    reserved
 141
 142 * data structure of the channel data
 143 *  - first channel packed with specified resolution
 144 *  - second channel packed with specified resolution
 145 *  - third channel packed with specified resolution
 146 *                       ...
 147 *  - last channel packed with specified resolution
 148 */
 149
 150
 151 #if defined(USE_CRSF_LINK_STATISTICS)
 152 /*
 153  * 0x14 Link statistics
 154  * Payload:
 155  *
 156  * uint8_t Uplink RSSI Ant. 1 ( dBm * -1 )
 157  * uint8_t Uplink RSSI Ant. 2 ( dBm * -1 )
 158  * uint8_t Uplink Package success rate / Link quality ( % )
 159  * int8_t Uplink SNR ( db )
 160  * uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
 161  * uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
 162  * uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW, 250mW )
 163  * uint8_t Downlink RSSI ( dBm * -1 )
 164  * uint8_t Downlink package success rate / Link quality ( % )
 165  * int8_t Downlink SNR ( db )
 166  * Uplink is the connection from the ground to the UAV and downlink the opposite direction.
 167  */
 168
 169 /*
 170  * 0x1C Link statistics RX
 171  * Payload:
 172  *
 173  * uint8_t Downlink RSSI ( dBm * -1 )
 174  * uint8_t Downlink RSSI ( % )
 175  * uint8_t Downlink Package success rate / Link quality ( % )
 176  * int8_t Downlink SNR ( db )
 177  * uint8_t Uplink RF Power ( db )
 178  */
 179
 180 /*
 181  * 0x1D Link statistics TX
 182  * Payload:
 183  *
 184  * uint8_t Uplink RSSI ( dBm * -1 )
 185  * uint8_t Uplink RSSI ( % )
 186  * uint8_t Uplink Package success rate / Link quality ( % )
 187  * int8_t Uplink SNR ( db )
 188  * uint8_t Downlink RF Power ( db )
 189  * uint8_t Uplink FPS ( FPS / 10 )
 190  */
 191
 192 typedef struct crsfPayloadLinkstatistics_s {
 193     uint8_t uplink_RSSI_1;
 194     uint8_t uplink_RSSI_2;
 195     uint8_t uplink_Link_quality;
 196     int8_t uplink_SNR;
 197     uint8_t active_antenna;
 198     uint8_t rf_Mode;
 199     uint8_t uplink_TX_Power;
 200     uint8_t downlink_RSSI;
 201     uint8_t downlink_Link_quality;
 202     int8_t downlink_SNR;
 203 } crsfLinkStatistics_t;
 204
 205 #if defined(USE_CRSF_V3)
 206 typedef struct crsfPayloadLinkstatisticsRx_s {
 207     uint8_t downlink_RSSI_1;
 208     uint8_t downlink_RSSI_1_percentage;
 209     uint8_t downlink_Link_quality;
 210     int8_t downlink_SNR;
 211     uint8_t uplink_power;
 212 } crsfLinkStatisticsRx_t; // this struct is currently not used
 213
 214 typedef struct crsfPayloadLinkstatisticsTx_s {
 215     uint8_t uplink_RSSI;
 216     uint8_t uplink_RSSI_percentage;
 217     uint8_t uplink_Link_quality;
 218     int8_t uplink_SNR;
 219     uint8_t downlink_power; // currently not used
 220     uint8_t uplink_FPS; // currently not used
 221 } crsfLinkStatisticsTx_t;
 222 #endif
 223
 224 static timeUs_t lastLinkStatisticsFrameUs;
 225
 226 static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
 227 {
 228     const crsfLinkStatistics_t stats = *statsPtr;
 229     lastLinkStatisticsFrameUs = currentTimeUs;
 230     int16_t rssiDbm = -1 * (stats.active_antenna ? stats.uplink_RSSI_2 : stats.uplink_RSSI_1);
 231     if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
 232         const uint16_t rssiPercentScaled = scaleRange(rssiDbm, CRSF_RSSI_MIN, CRSF_RSSI_MAX, 0, RSSI_MAX_VALUE);
 233         setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 234     }
 235 #ifdef USE_RX_RSSI_DBM
 236     setRssiDbm(rssiDbm, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 237     setActiveAntenna(stats.active_antenna);
 238 #endif
 239
 240 #ifdef USE_RX_RSNR
 241     setRsnr(stats.uplink_SNR);
 242 #endif
 243
 244 #ifdef USE_RX_LINK_QUALITY_INFO
 245     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
 246         setLinkQualityDirect(stats.uplink_Link_quality);
 247         rxSetRfMode(stats.rf_Mode);
 248     }
 249 #endif
 250
 251 #ifdef USE_RX_LINK_UPLINK_POWER
 252     const uint8_t crsfUplinkPowerStatesItemIndex = (stats.uplink_TX_Power < CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT) ? stats.uplink_TX_Power : 0;
 253     rxSetUplinkTxPwrMw(uplinkTXPowerStatesMw[crsfUplinkPowerStatesItemIndex]);
 254 #endif
 255
 256     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
 257     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 1, stats.uplink_RSSI_2);
 258     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 2, stats.uplink_Link_quality);
 259     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 3, stats.rf_Mode);
 260
 261     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 0, stats.active_antenna);
 262     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 1, stats.uplink_SNR);
 263     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 2, stats.uplink_TX_Power);
 264
 265     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 0, stats.downlink_RSSI);
 266     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 1, stats.downlink_Link_quality);
 267     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 2, stats.downlink_SNR);
 268 }
 269
 270 #if defined(USE_CRSF_V3)
 271 static void handleCrsfLinkStatisticsTxFrame(const crsfLinkStatisticsTx_t* statsPtr, timeUs_t currentTimeUs)
 272 {
 273     const crsfLinkStatisticsTx_t stats = *statsPtr;
 274     lastLinkStatisticsFrameUs = currentTimeUs;
 275     if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
 276         const uint16_t rssiPercentScaled = scaleRange(stats.uplink_RSSI_percentage, 0, 100, 0, RSSI_MAX_VALUE);
 277         setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 278     }
 279 #ifdef USE_RX_RSSI_DBM
 280     int16_t rssiDbm = -1 * stats.uplink_RSSI;
 281     setRssiDbm(rssiDbm, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 282 #endif
 283
 284 #ifdef USE_RX_RSNR
 285     setRsnr(stats.uplink_SNR);
 286 #endif
 287
 288 #ifdef USE_RX_LINK_QUALITY_INFO
 289     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
 290         setLinkQualityDirect(stats.uplink_Link_quality);
 291     }
 292 #endif
 293
 294     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI);
 295     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 1, stats.uplink_SNR);
 296     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 2, stats.uplink_Link_quality);
 297     DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 3, stats.uplink_RSSI_percentage);
 298 }
 299 #endif
 300 #endif
 301
 302 #if defined(USE_CRSF_LINK_STATISTICS)
 303 static void crsfCheckRssi(uint32_t currentTimeUs)
 304 {
 305
 306     if (cmpTimeUs(currentTimeUs, lastLinkStatisticsFrameUs) > CRSF_LINK_STATUS_UPDATE_TIMEOUT_US) {
 307         if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
 308             setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 309 #ifdef USE_RX_RSSI_DBM
 310             setRssiDbmDirect(CRSF_RSSI_MIN, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 311 #endif
 312 #ifdef USE_RX_RSNR
 313             setRsnrDirect(CRSF_SNR_MIN);
 314 #endif
 315         }
 316 #ifdef USE_RX_LINK_QUALITY_INFO
 317         if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
 318             setLinkQualityDirect(0);
 319         }
 320 #endif
 321     }
 322 }
 323 #endif
 324
 325 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 326 {
 327     // CRC includes type and payload
 328     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 329     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 330         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 331     }
 332     return crc;
 333 }
 334
 335 #if defined(USE_CRSF_V3) || defined(UNIT_TEST)
 336 STATIC_UNIT_TESTED uint8_t crsfFrameCmdCRC(void)
 337 {
 338     // CRC includes type and payload
 339     uint8_t crc = crc8_poly_0xba(0, crsfFrame.frame.type);
 340     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC - 1; ++ii) {
 341         crc = crc8_poly_0xba(crc, crsfFrame.frame.payload[ii]);
 342     }
 343     return crc;
 344 }
 345 #endif
 346
 347 // Receive ISR callback, called back from serial port
 348 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
 349 {
 350     rxRuntimeState_t *const rxRuntimeState = (rxRuntimeState_t *const)data;
 351
 352     static uint8_t crsfFramePosition = 0;
 353 #if defined(USE_CRSF_V3)
 354     static uint8_t crsfFrameErrorCnt = 0;
 355 #endif
 356     const timeUs_t currentTimeUs = microsISR();
 357
 358 #ifdef DEBUG_CRSF_PACKETS
 359     debug[2] = currentTimeUs - crsfFrameStartAtUs;
 360 #endif
 361
 362     if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) {
 363         // We've received a character after max time needed to complete a frame,
 364         // so this must be the start of a new frame.
 365 #if defined(USE_CRSF_V3)
 366         if (crsfFramePosition > 0) {
 367             // count an error if full valid frame not received within the allowed time.
 368             crsfFrameErrorCnt++;
 369         }
 370 #endif
 371         crsfFramePosition = 0;
 372     }
 373
 374     if (crsfFramePosition == 0) {
 375         crsfFrameStartAtUs = currentTimeUs;
 376     }
 377     // assume frame is 5 bytes long until we have received the frame length
 378     // full frame length includes the length of the address and framelength fields
 379     // sometimes we can receive some garbage data. So, we need to check max size for preventing buffer overrun.
 380     const int fullFrameLength = crsfFramePosition < 3 ? 5 : MIN(crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH, CRSF_FRAME_SIZE_MAX);
 381
 382     if (crsfFramePosition < fullFrameLength) {
 383         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 384         if (crsfFramePosition >= fullFrameLength) {
 385             crsfFramePosition = 0;
 386             const uint8_t crc = crsfFrameCRC();
 387             if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 388 #if defined(USE_CRSF_V3)
 389                 crsfFrameErrorCnt = 0;
 390 #endif
 391                 switch (crsfFrame.frame.type) {
 392                 case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
 393                 case CRSF_FRAMETYPE_SUBSET_RC_CHANNELS_PACKED:
 394                     if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
 395                         rxRuntimeState->lastRcFrameTimeUs = currentTimeUs;
 396                         crsfFrameDone = true;
 397                         memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
 398                     }
 399                     break;
 400
 401 #if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY)
 402                 case CRSF_FRAMETYPE_MSP_REQ:
 403                 case CRSF_FRAMETYPE_MSP_WRITE: {
 404                     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 405                     if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) {
 406                         crsfScheduleMspResponse(crsfFrame.frame.payload[1]);
 407                     }
 408                     break;
 409                 }
 410 #endif
 411 #if defined(USE_CRSF_CMS_TELEMETRY)
 412                 case CRSF_FRAMETYPE_DEVICE_PING:
 413                     crsfScheduleDeviceInfoResponse();
 414                     break;
 415                 case CRSF_FRAMETYPE_DEVICE_INFO:
 416                     crsfHandleDeviceInfoResponse(crsfFrame.frame.payload);
 417                     break;
 418                 case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
 419                     uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 420                     crsfProcessDisplayPortCmd(frameStart);
 421                     break;
 422                 }
 423 #endif
 424 #if defined(USE_CRSF_LINK_STATISTICS)
 425
 426                 case CRSF_FRAMETYPE_LINK_STATISTICS: {
 427                     // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
 428                     if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
 429                         (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
 430                         (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) {
 431                         const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
 432                         handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
 433                     }
 434                     break;
 435                 }
 436 #if defined(USE_CRSF_V3)
 437                 case CRSF_FRAMETYPE_LINK_STATISTICS_RX: {
 438                     break;
 439                 }
 440                 case CRSF_FRAMETYPE_LINK_STATISTICS_TX: {
 441                     if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
 442                         (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
 443                         (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE)) {
 444                         const crsfLinkStatisticsTx_t* statsFrame = (const crsfLinkStatisticsTx_t*)&crsfFrame.frame.payload;
 445                         handleCrsfLinkStatisticsTxFrame(statsFrame, currentTimeUs);
 446                     }
 447                     break;
 448                 }
 449 #endif
 450 #endif
 451 #if defined(USE_CRSF_V3)
 452                 case CRSF_FRAMETYPE_COMMAND:
 453                     if ((crsfFrame.bytes[fullFrameLength - 2] == crsfFrameCmdCRC()) &&
 454                         (crsfFrame.bytes[3] == CRSF_ADDRESS_FLIGHT_CONTROLLER)) {
 455                         crsfProcessCommand(crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE);
 456                     }
 457                     break;
 458 #endif
 459                 default:
 460                     break;
 461                 }
 462             } else {
 463 #if defined(USE_CRSF_V3)
 464                 if (crsfFrameErrorCnt < CRSF_FRAME_ERROR_COUNT_THRESHOLD)
 465                     crsfFrameErrorCnt++;
 466 #endif
 467             }
 468         }
 469 #if defined(USE_CRSF_V3)
 470         if (crsfBaudNegotiationInProgress() || isEepromWriteInProgress()) {
 471             // don't count errors when negotiation or eeprom write is in progress
 472             crsfFrameErrorCnt = 0;
 473         } else if (crsfFrameErrorCnt >= CRSF_FRAME_ERROR_COUNT_THRESHOLD) {
 474             // fall back to default speed if speed mismatch detected
 475             setCrsfDefaultSpeed();
 476             crsfFrameErrorCnt = 0;
 477         }
 478 #endif
 479     }
 480 }
 481
 482 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
 483 {
 484     UNUSED(rxRuntimeState);
 485
 486 #if defined(USE_CRSF_LINK_STATISTICS)
 487     crsfCheckRssi(micros());
 488 #endif
 489     if (crsfFrameDone) {
 490         crsfFrameDone = false;
 491
 492         // unpack the RC channels
 493         if (crsfChannelDataFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 494             // use ordinary RC frame structure (0x16)
 495             const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
 496             channelScale = CRSF_RC_CHANNEL_SCALE_LEGACY;
 497             crsfChannelData[0] = rcChannels->chan0;
 498             crsfChannelData[1] = rcChannels->chan1;
 499             crsfChannelData[2] = rcChannels->chan2;
 500             crsfChannelData[3] = rcChannels->chan3;
 501             crsfChannelData[4] = rcChannels->chan4;
 502             crsfChannelData[5] = rcChannels->chan5;
 503             crsfChannelData[6] = rcChannels->chan6;
 504             crsfChannelData[7] = rcChannels->chan7;
 505             crsfChannelData[8] = rcChannels->chan8;
 506             crsfChannelData[9] = rcChannels->chan9;
 507             crsfChannelData[10] = rcChannels->chan10;
 508             crsfChannelData[11] = rcChannels->chan11;
 509             crsfChannelData[12] = rcChannels->chan12;
 510             crsfChannelData[13] = rcChannels->chan13;
 511             crsfChannelData[14] = rcChannels->chan14;
 512             crsfChannelData[15] = rcChannels->chan15;
 513         } else {
 514             // use subset RC frame structure (0x17)
 515             uint8_t readByteIndex = 0;
 516             const uint8_t *payload = crsfChannelDataFrame.frame.payload;
 517
 518             // get the configuration byte
 519             uint8_t configByte = payload[readByteIndex++];
 520
 521             // get the channel number of start channel
 522             uint8_t startChannel = configByte & CRSF_SUBSET_RC_STARTING_CHANNEL_MASK;
 523             configByte >>= CRSF_SUBSET_RC_STARTING_CHANNEL_BITS;
 524
 525             // get the channel resolution settings
 526             uint8_t channelBits;
 527             uint16_t channelMask;
 528             uint8_t channelRes = configByte & CRSF_SUBSET_RC_RES_CONFIGURATION_MASK;
 529             configByte >>= CRSF_SUBSET_RC_RES_CONFIGURATION_BITS;
 530             switch (channelRes) {
 531             case CRSF_SUBSET_RC_RES_CONF_10B:
 532                 channelBits = CRSF_SUBSET_RC_RES_BITS_10B;
 533                 channelMask = CRSF_SUBSET_RC_RES_MASK_10B;
 534                 channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_10B;
 535                 break;
 536             default:
 537             case CRSF_SUBSET_RC_RES_CONF_11B:
 538                 channelBits = CRSF_SUBSET_RC_RES_BITS_11B;
 539                 channelMask = CRSF_SUBSET_RC_RES_MASK_11B;
 540                 channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_11B;
 541                 break;
 542             case CRSF_SUBSET_RC_RES_CONF_12B:
 543                 channelBits = CRSF_SUBSET_RC_RES_BITS_12B;
 544                 channelMask = CRSF_SUBSET_RC_RES_MASK_12B;
 545                 channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_12B;
 546                 break;
 547             case CRSF_SUBSET_RC_RES_CONF_13B:
 548                 channelBits = CRSF_SUBSET_RC_RES_BITS_13B;
 549                 channelMask = CRSF_SUBSET_RC_RES_MASK_13B;
 550                 channelScale = CRSF_SUBSET_RC_CHANNEL_SCALE_13B;
 551                 break;
 552             }
 553
 554             // do nothing for the reserved configuration bit
 555             configByte >>= CRSF_SUBSET_RC_RESERVED_CONFIGURATION_BITS;
 556
 557             // calculate the number of channels packed
 558             uint8_t numOfChannels = ((crsfChannelDataFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC - 1) * 8) / channelBits;
 559
 560             // unpack the channel data
 561             uint8_t bitsMerged = 0;
 562             uint32_t readValue = 0;
 563             for (uint8_t n = 0; n < numOfChannels; n++) {
 564                 while (bitsMerged < channelBits) {
 565                     uint8_t readByte = payload[readByteIndex++];
 566                     readValue |= ((uint32_t) readByte) << bitsMerged;
 567                     bitsMerged += 8;
 568                 }
 569                 crsfChannelData[startChannel + n] = readValue & channelMask;
 570                 readValue >>= channelBits;
 571                 bitsMerged -= channelBits;
 572             }
 573         }
 574         return RX_FRAME_COMPLETE;
 575     }
 576     return RX_FRAME_PENDING;
 577 }
 578
 579 STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 580 {
 581     UNUSED(rxRuntimeState);
 582     if (channelScale == CRSF_RC_CHANNEL_SCALE_LEGACY) {
 583         /* conversion from RC value to PWM
 584         * for 0x16 RC frame
 585         *       RC     PWM
 586         * min  172 ->  988us
 587         * mid  992 -> 1500us
 588         * max 1811 -> 2012us
 589         * scale factor = (2012-988) / (1811-172) = 0.62477120195241
 590         * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
 591         */
 592         return (channelScale * (float)crsfChannelData[chan]) + 881;
 593     } else {
 594         /* conversion from RC value to PWM
 595         * for 0x17 Subset RC frame
 596         */
 597         return (channelScale * (float)crsfChannelData[chan]) + 988;
 598     }
 599 }
 600
 601 void crsfRxWriteTelemetryData(const void *data, int len)
 602 {
 603     len = MIN(len, (int)sizeof(telemetryBuf));
 604     memcpy(telemetryBuf, data, len);
 605     telemetryBufLen = len;
 606 }
 607
 608 void crsfRxSendTelemetryData(void)
 609 {
 610     // if there is telemetry data to write
 611     if (telemetryBufLen > 0) {
 612         if (serialPort != NULL) {
 613             serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 614         }
 615         telemetryBufLen = 0; // reset telemetry buffer
 616     }
 617 }
 618
 619 bool crsfRxIsTelemetryBufEmpty(void)
 620 {
 621     return telemetryBufLen == 0;
 622 }
 623
 624 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 625 {
 626     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
 627         crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
 628     }
 629
 630     rxRuntimeState->channelCount = CRSF_MAX_CHANNEL;
 631     rxRuntimeState->rcReadRawFn = crsfReadRawRC;
 632     rxRuntimeState->rcFrameStatusFn = crsfFrameStatus;
 633     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 634
 635     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 636     if (!portConfig) {
 637         return false;
 638     }
 639
 640     uint32_t crsfBaudrate = CRSF_BAUDRATE;
 641
 642 #if defined(USE_CRSF_V3)
 643     crsfBaudrate = rxConfig->crsf_use_negotiated_baud ? getCrsfCachedBaudrate() : CRSF_BAUDRATE;
 644 #endif
 645
 646     serialPort = openSerialPort(portConfig->identifier,
 647         FUNCTION_RX_SERIAL,
 648         crsfDataReceive,
 649         rxRuntimeState,
 650         crsfBaudrate,
 651         CRSF_PORT_MODE,
 652         CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
 653         );
 654
 655     if (rssiSource == RSSI_SOURCE_NONE) {
 656         rssiSource = RSSI_SOURCE_RX_PROTOCOL_CRSF;
 657     }
 658 #ifdef USE_RX_LINK_QUALITY_INFO
 659     if (linkQualitySource == LQ_SOURCE_NONE) {
 660         linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
 661     }
 662 #endif
 663
 664     return serialPort != NULL;
 665 }
 666
 667 #if defined(USE_CRSF_V3)
 668 void crsfRxUpdateBaudrate(uint32_t baudrate)
 669 {
 670     serialSetBaudRate(serialPort, baudrate);
 671     persistentObjectWrite(PERSISTENT_OBJECT_SERIALRX_BAUD, baudrate);
 672 }
 673
 674 bool crsfRxUseNegotiatedBaud(void)
 675 {
 676     return rxConfig()->crsf_use_negotiated_baud;
 677 }
 678 #endif
 679
 680 bool crsfRxIsActive(void)
 681 {
 682     return serialPort != NULL;
 683 }
 684
 685 void crsfRxBind(void)
 686 {
 687     if (serialPort != NULL) {
 688         uint8_t bindFrame[] = {
 689             CRSF_SYNC_BYTE,
 690             0x07,  // frame length
 691             CRSF_FRAMETYPE_COMMAND,
 692             CRSF_ADDRESS_CRSF_RECEIVER,
 693             CRSF_ADDRESS_FLIGHT_CONTROLLER,
 694             CRSF_COMMAND_SUBCMD_RX,
 695             CRSF_COMMAND_SUBCMD_RX_BIND,
 696             0x9E,  // Command CRC8
 697             0xE8,  // Packet CRC8
 698         };
 699         serialWriteBuf(serialPort, bindFrame, 9);
 700     }
 701 }
 702 #endif