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