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 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  62 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  63 STATIC_UNIT_TESTED crsfFrame_t crsfChannelDataFrame;
  64 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  65
  66 static serialPort_t *serialPort;
  67 static timeUs_t crsfFrameStartAtUs = 0;
  68 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  69 static uint8_t telemetryBufLen = 0;
  70
  71 static timeUs_t lastRcFrameTimeUs = 0;
  72
  73 /*
  74  * CRSF protocol
  75  *
  76  * CRSF protocol uses a single wire half duplex uart connection.
  77  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  78  *
  79  * 420000 baud
  80  * not inverted
  81  * 8 Bit
  82  * 1 Stop bit
  83  * Big endian
  84  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  85  * Max frame size is 64 bytes
  86  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  87  *
  88  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  89  *
  90  * Every frame has the structure:
  91  * <Device address><Frame length><Type><Payload><CRC>
  92  *
  93  * Device address: (uint8_t)
  94  * Frame length:   length in  bytes including Type (uint8_t)
  95  * Type:           (uint8_t)
  96  * CRC:            (uint8_t)
  97  *
  98  */
  99
 100 struct crsfPayloadRcChannelsPacked_s {
 101     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
 102     unsigned int chan0 : 11;
 103     unsigned int chan1 : 11;
 104     unsigned int chan2 : 11;
 105     unsigned int chan3 : 11;
 106     unsigned int chan4 : 11;
 107     unsigned int chan5 : 11;
 108     unsigned int chan6 : 11;
 109     unsigned int chan7 : 11;
 110     unsigned int chan8 : 11;
 111     unsigned int chan9 : 11;
 112     unsigned int chan10 : 11;
 113     unsigned int chan11 : 11;
 114     unsigned int chan12 : 11;
 115     unsigned int chan13 : 11;
 116     unsigned int chan14 : 11;
 117     unsigned int chan15 : 11;
 118 } __attribute__ ((__packed__));
 119
 120 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 121
 122 #if defined(USE_CRSF_LINK_STATISTICS)
 123 /*
 124  * 0x14 Link statistics
 125  * Payload:
 126  *
 127  * uint8_t Uplink RSSI Ant. 1 ( dBm * -1 )
 128  * uint8_t Uplink RSSI Ant. 2 ( dBm * -1 )
 129  * uint8_t Uplink Package success rate / Link quality ( % )
 130  * int8_t Uplink SNR ( db )
 131  * uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
 132  * uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
 133  * uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
 134  * uint8_t Downlink RSSI ( dBm * -1 )
 135  * uint8_t Downlink package success rate / Link quality ( % )
 136  * int8_t Downlink SNR ( db )
 137  * Uplink is the connection from the ground to the UAV and downlink the opposite direction.
 138  */
 139
 140 typedef struct crsfPayloadLinkstatistics_s {
 141     uint8_t uplink_RSSI_1;
 142     uint8_t uplink_RSSI_2;
 143     uint8_t uplink_Link_quality;
 144     int8_t uplink_SNR;
 145     uint8_t active_antenna;
 146     uint8_t rf_Mode;
 147     uint8_t uplink_TX_Power;
 148     uint8_t downlink_RSSI;
 149     uint8_t downlink_Link_quality;
 150     int8_t downlink_SNR;
 151 } crsfLinkStatistics_t;
 152
 153 static timeUs_t lastLinkStatisticsFrameUs;
 154
 155 static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr, timeUs_t currentTimeUs)
 156 {
 157     const crsfLinkStatistics_t stats = *statsPtr;
 158     lastLinkStatisticsFrameUs = currentTimeUs;
 159     if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
 160         int16_t rssiDbm = -1 * (stats.active_antenna ? stats.uplink_RSSI_2 : stats.uplink_RSSI_1);
 161         const uint16_t rssiPercentScaled = scaleRange(rssiDbm, CRSF_RSSI_MIN, 0, 0, RSSI_MAX_VALUE);
 162         setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 163 #ifdef USE_RX_RSSI_DBM
 164         if (rxConfig()->crsf_use_rx_snr) {
 165             rssiDbm = stats.uplink_SNR;
 166         }
 167         setRssiDbm(rssiDbm, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 168 #endif
 169     }
 170
 171 #ifdef USE_RX_LINK_QUALITY_INFO
 172     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
 173         setLinkQualityDirect(stats.uplink_Link_quality);
 174         rxSetRfMode(stats.rf_Mode);
 175     }
 176 #endif
 177
 178     switch (debugMode) {
 179     case DEBUG_CRSF_LINK_STATISTICS_UPLINK:
 180         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
 181         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 1, stats.uplink_RSSI_2);
 182         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 2, stats.uplink_Link_quality);
 183         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 3, stats.rf_Mode);
 184         break;
 185     case DEBUG_CRSF_LINK_STATISTICS_PWR:
 186         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 0, stats.active_antenna);
 187         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 1, stats.uplink_SNR);
 188         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_PWR, 2, stats.uplink_TX_Power);
 189         break;
 190     case DEBUG_CRSF_LINK_STATISTICS_DOWN:
 191         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 0, stats.downlink_RSSI);
 192         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 1, stats.downlink_Link_quality);
 193         DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_DOWN, 2, stats.downlink_SNR);
 194         break;
 195     }
 196
 197 }
 198 #endif
 199
 200 #if defined(USE_CRSF_LINK_STATISTICS)
 201 static void crsfCheckRssi(uint32_t currentTimeUs) {
 202
 203     if (cmpTimeUs(currentTimeUs, lastLinkStatisticsFrameUs) > CRSF_LINK_STATUS_UPDATE_TIMEOUT_US) {
 204         if (rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) {
 205             setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 206 #ifdef USE_RX_RSSI_DBM
 207             if (rxConfig()->crsf_use_rx_snr) {
 208                 setRssiDbmDirect(CRSF_SNR_MIN, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 209             } else {
 210                 setRssiDbmDirect(CRSF_RSSI_MIN, RSSI_SOURCE_RX_PROTOCOL_CRSF);
 211             }
 212 #endif
 213         }
 214 #ifdef USE_RX_LINK_QUALITY_INFO
 215         if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_CRSF) {
 216             setLinkQualityDirect(0);
 217             rxSetRfMode(0);
 218         }
 219 #endif
 220     }
 221 }
 222 #endif
 223
 224 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 225 {
 226     // CRC includes type and payload
 227     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 228     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 229         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 230     }
 231     return crc;
 232 }
 233
 234 // Receive ISR callback, called back from serial port
 235 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
 236 {
 237     UNUSED(data);
 238
 239     static uint8_t crsfFramePosition = 0;
 240     const timeUs_t currentTimeUs = microsISR();
 241
 242 #ifdef DEBUG_CRSF_PACKETS
 243     debug[2] = currentTimeUs - crsfFrameStartAtUs;
 244 #endif
 245
 246     if (cmpTimeUs(currentTimeUs, crsfFrameStartAtUs) > CRSF_TIME_NEEDED_PER_FRAME_US) {
 247         // We've received a character after max time needed to complete a frame,
 248         // so this must be the start of a new frame.
 249         crsfFramePosition = 0;
 250     }
 251
 252     if (crsfFramePosition == 0) {
 253         crsfFrameStartAtUs = currentTimeUs;
 254     }
 255     // assume frame is 5 bytes long until we have received the frame length
 256     // full frame length includes the length of the address and framelength fields
 257     const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 258
 259     if (crsfFramePosition < fullFrameLength) {
 260         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 261         if (crsfFramePosition >= fullFrameLength) {
 262             crsfFramePosition = 0;
 263             const uint8_t crc = crsfFrameCRC();
 264             if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 265                 switch (crsfFrame.frame.type)
 266                 {
 267                     case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
 268                         if (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) {
 269                             lastRcFrameTimeUs = currentTimeUs;
 270                             crsfFrameDone = true;
 271                             memcpy(&crsfChannelDataFrame, &crsfFrame, sizeof(crsfFrame));
 272                         }
 273                         break;
 274
 275 #if defined(USE_TELEMETRY_CRSF) && defined(USE_MSP_OVER_TELEMETRY)
 276                     case CRSF_FRAMETYPE_MSP_REQ:
 277                     case CRSF_FRAMETYPE_MSP_WRITE: {
 278                         uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 279                         if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 280                             crsfScheduleMspResponse();
 281                         }
 282                         break;
 283                     }
 284 #endif
 285 #if defined(USE_CRSF_CMS_TELEMETRY)
 286                     case CRSF_FRAMETYPE_DEVICE_PING:
 287                         crsfScheduleDeviceInfoResponse();
 288                         break;
 289                     case CRSF_FRAMETYPE_DISPLAYPORT_CMD: {
 290                         uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 291                         crsfProcessDisplayPortCmd(frameStart);
 292                         break;
 293                     }
 294 #endif
 295 #if defined(USE_CRSF_LINK_STATISTICS)
 296
 297                     case CRSF_FRAMETYPE_LINK_STATISTICS: {
 298                          // if to FC and 10 bytes + CRSF_FRAME_ORIGIN_DEST_SIZE
 299                          if ((rssiSource == RSSI_SOURCE_RX_PROTOCOL_CRSF) &&
 300                              (crsfFrame.frame.deviceAddress == CRSF_ADDRESS_FLIGHT_CONTROLLER) &&
 301                              (crsfFrame.frame.frameLength == CRSF_FRAME_ORIGIN_DEST_SIZE + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE)) {
 302                              const crsfLinkStatistics_t* statsFrame = (const crsfLinkStatistics_t*)&crsfFrame.frame.payload;
 303                              handleCrsfLinkStatisticsFrame(statsFrame, currentTimeUs);
 304                          }
 305                         break;
 306                     }
 307 #endif
 308                     default:
 309                         break;
 310                 }
 311             }
 312         }
 313     }
 314 }
 315
 316 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
 317 {
 318     UNUSED(rxRuntimeState);
 319
 320 #if defined(USE_CRSF_LINK_STATISTICS)
 321     crsfCheckRssi(micros());
 322 #endif
 323     if (crsfFrameDone) {
 324         crsfFrameDone = false;
 325
 326         // unpack the RC channels
 327         const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfChannelDataFrame.frame.payload;
 328         crsfChannelData[0] = rcChannels->chan0;
 329         crsfChannelData[1] = rcChannels->chan1;
 330         crsfChannelData[2] = rcChannels->chan2;
 331         crsfChannelData[3] = rcChannels->chan3;
 332         crsfChannelData[4] = rcChannels->chan4;
 333         crsfChannelData[5] = rcChannels->chan5;
 334         crsfChannelData[6] = rcChannels->chan6;
 335         crsfChannelData[7] = rcChannels->chan7;
 336         crsfChannelData[8] = rcChannels->chan8;
 337         crsfChannelData[9] = rcChannels->chan9;
 338         crsfChannelData[10] = rcChannels->chan10;
 339         crsfChannelData[11] = rcChannels->chan11;
 340         crsfChannelData[12] = rcChannels->chan12;
 341         crsfChannelData[13] = rcChannels->chan13;
 342         crsfChannelData[14] = rcChannels->chan14;
 343         crsfChannelData[15] = rcChannels->chan15;
 344         return RX_FRAME_COMPLETE;
 345     }
 346     return RX_FRAME_PENDING;
 347 }
 348
 349 STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 350 {
 351     UNUSED(rxRuntimeState);
 352     /* conversion from RC value to PWM
 353      *       RC     PWM
 354      * min  172 ->  988us
 355      * mid  992 -> 1500us
 356      * max 1811 -> 2012us
 357      * scale factor = (2012-988) / (1811-172) = 0.62477120195241
 358      * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
 359      */
 360     return (0.62477120195241f * crsfChannelData[chan]) + 881;
 361 }
 362
 363 void crsfRxWriteTelemetryData(const void *data, int len)
 364 {
 365     len = MIN(len, (int)sizeof(telemetryBuf));
 366     memcpy(telemetryBuf, data, len);
 367     telemetryBufLen = len;
 368 }
 369
 370 void crsfRxSendTelemetryData(void)
 371 {
 372     // if there is telemetry data to write
 373     if (telemetryBufLen > 0) {
 374         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 375         telemetryBufLen = 0; // reset telemetry buffer
 376     }
 377 }
 378
 379 static timeUs_t crsfFrameTimeUs(void)
 380 {
 381     const timeUs_t result = lastRcFrameTimeUs;
 382     lastRcFrameTimeUs = 0;
 383     return result;
 384 }
 385
 386 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 387 {
 388     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
 389         crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
 390     }
 391
 392     rxRuntimeState->channelCount = CRSF_MAX_CHANNEL;
 393     rxRuntimeState->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking
 394
 395     rxRuntimeState->rcReadRawFn = crsfReadRawRC;
 396     rxRuntimeState->rcFrameStatusFn = crsfFrameStatus;
 397     rxRuntimeState->rcFrameTimeUsFn = crsfFrameTimeUs;
 398
 399     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 400     if (!portConfig) {
 401         return false;
 402     }
 403
 404     serialPort = openSerialPort(portConfig->identifier,
 405         FUNCTION_RX_SERIAL,
 406         crsfDataReceive,
 407         NULL,
 408         CRSF_BAUDRATE,
 409         CRSF_PORT_MODE,
 410         CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
 411         );
 412
 413         if (rssiSource == RSSI_SOURCE_NONE) {
 414             rssiSource = RSSI_SOURCE_RX_PROTOCOL_CRSF;
 415         }
 416 #ifdef USE_RX_LINK_QUALITY_INFO
 417         if (linkQualitySource == LQ_SOURCE_NONE) {
 418             linkQualitySource = LQ_SOURCE_RX_PROTOCOL_CRSF;
 419         }
 420 #endif
 421
 422     return serialPort != NULL;
 423 }
 424
 425 bool crsfRxIsActive(void)
 426 {
 427     return serialPort != NULL;
 428 }
 429 #endif