src/main/rx/crsf.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24 #ifdef USE_SERIALRX_CRSF
  25
  26 #include "build/build_config.h"
  27 #include "build/debug.h"
  28
  29 #include "common/crc.h"
  30 #include "common/maths.h"
  31 #include "common/utils.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/serial.h"
  35 #include "drivers/serial_uart.h"
  36
  37 #include "io/serial.h"
  38 #include "io/osd.h"
  39
  40 #include "rx/rx.h"
  41 #include "rx/crsf.h"
  42
  43 #include "telemetry/crsf.h"
  44 #define CRSF_TIME_NEEDED_PER_FRAME_US   1100 // 700 ms + 400 ms for potential ad-hoc request
  45 #define CRSF_TIME_BETWEEN_FRAMES_US     6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
  46
  47 #define CRSF_DIGITAL_CHANNEL_MIN 172
  48 #define CRSF_DIGITAL_CHANNEL_MAX 1811
  49 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
  50 #define CRSF_POWER_COUNT 9
  51
  52 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  53 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  54
  55 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  56
  57 static serialPort_t *serialPort;
  58 static timeUs_t crsfFrameStartAt = 0;
  59 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  60 static uint8_t telemetryBufLen = 0;
  61
  62 const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};
  63
  64 /*
  65  * CRSF protocol
  66  *
  67  * CRSF protocol uses a single wire half duplex uart connection.
  68  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  69  *
  70  * 420000 baud
  71  * not inverted
  72  * 8 Bit
  73  * 1 Stop bit
  74  * Big endian
  75  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  76  * Max frame size is 64 bytes
  77  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  78  *
  79  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  80  *
  81  * Every frame has the structure:
  82  * <Device address> <Frame length> < Type> <Payload> < CRC>
  83  *
  84  * Device address: (uint8_t)
  85  * Frame length:   length in  bytes including Type (uint8_t)
  86  * Type:           (uint8_t)
  87  * CRC:            (uint8_t)
  88  *
  89  */
  90
  91 struct crsfPayloadRcChannelsPacked_s {
  92     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
  93     unsigned int chan0 : 11;
  94     unsigned int chan1 : 11;
  95     unsigned int chan2 : 11;
  96     unsigned int chan3 : 11;
  97     unsigned int chan4 : 11;
  98     unsigned int chan5 : 11;
  99     unsigned int chan6 : 11;
 100     unsigned int chan7 : 11;
 101     unsigned int chan8 : 11;
 102     unsigned int chan9 : 11;
 103     unsigned int chan10 : 11;
 104     unsigned int chan11 : 11;
 105     unsigned int chan12 : 11;
 106     unsigned int chan13 : 11;
 107     unsigned int chan14 : 11;
 108     unsigned int chan15 : 11;
 109 } __attribute__ ((__packed__));
 110
 111 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 112
 113 typedef struct crsfPayloadLinkStatistics_s {
 114     uint8_t     uplinkRSSIAnt1;
 115     uint8_t     uplinkRSSIAnt2;
 116     uint8_t     uplinkLQ;
 117     int8_t      uplinkSNR;
 118     uint8_t     activeAntenna;
 119     uint8_t     rfMode;
 120     uint8_t     uplinkTXPower;
 121     uint8_t     downlinkRSSI;
 122     uint8_t     downlinkLQ;
 123     int8_t      downlinkSNR;
 124 } __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t;
 125
 126 typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
 127
 128 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 129 {
 130     // CRC includes type and payload
 131     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 132     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 133         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 134     }
 135     return crc;
 136 }
 137
 138 // Receive ISR callback, called back from serial port
 139 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
 140 {
 141     UNUSED(rxCallbackData);
 142
 143     static uint8_t crsfFramePosition = 0;
 144     const timeUs_t now = micros();
 145
 146 #ifdef DEBUG_CRSF_PACKETS
 147     debug[2] = now - crsfFrameStartAt;
 148 #endif
 149
 150     if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
 151         // We've received a character after max time needed to complete a frame,
 152         // so this must be the start of a new frame.
 153         crsfFramePosition = 0;
 154     }
 155
 156     if (crsfFramePosition == 0) {
 157         crsfFrameStartAt = now;
 158     }
 159     // assume frame is 5 bytes long until we have received the frame length
 160     // full frame length includes the length of the address and framelength fields
 161     const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 162
 163     if (crsfFramePosition < fullFrameLength) {
 164         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 165         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
 166         if (crsfFrameDone) {
 167             crsfFramePosition = 0;
 168             if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 169                 const uint8_t crc = crsfFrameCRC();
 170                 if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 171                     switch (crsfFrame.frame.type)
 172                     {
 173 #if defined(USE_MSP_OVER_TELEMETRY)
 174                         case CRSF_FRAMETYPE_MSP_REQ:
 175                         case CRSF_FRAMETYPE_MSP_WRITE: {
 176                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 177                             if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 178                                 crsfScheduleMspResponse();
 179                             }
 180                             break;
 181                         }
 182 #endif
 183                         default:
 184                             break;
 185                     }
 186                 }
 187             }
 188         }
 189     }
 190 }
 191
 192 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 193 {
 194     UNUSED(rxRuntimeConfig);
 195
 196     if (crsfFrameDone) {
 197         crsfFrameDone = false;
 198         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 199             // CRC includes type and payload of each frame
 200             const uint8_t crc = crsfFrameCRC();
 201             if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
 202                 return RX_FRAME_PENDING;
 203             }
 204             crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 205
 206             // unpack the RC channels
 207             const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
 208             crsfChannelData[0] = rcChannels->chan0;
 209             crsfChannelData[1] = rcChannels->chan1;
 210             crsfChannelData[2] = rcChannels->chan2;
 211             crsfChannelData[3] = rcChannels->chan3;
 212             crsfChannelData[4] = rcChannels->chan4;
 213             crsfChannelData[5] = rcChannels->chan5;
 214             crsfChannelData[6] = rcChannels->chan6;
 215             crsfChannelData[7] = rcChannels->chan7;
 216             crsfChannelData[8] = rcChannels->chan8;
 217             crsfChannelData[9] = rcChannels->chan9;
 218             crsfChannelData[10] = rcChannels->chan10;
 219             crsfChannelData[11] = rcChannels->chan11;
 220             crsfChannelData[12] = rcChannels->chan12;
 221             crsfChannelData[13] = rcChannels->chan13;
 222             crsfChannelData[14] = rcChannels->chan14;
 223             crsfChannelData[15] = rcChannels->chan15;
 224             return RX_FRAME_COMPLETE;
 225         }
 226         else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
 227             // CRC includes type and payload of each frame
 228             const uint8_t crc = crsfFrameCRC();
 229             if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
 230                 return RX_FRAME_PENDING;
 231             }
 232             crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 233
 234             const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
 235             const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0;
 236
 237             rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1);
 238             rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ;
 239             rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR;
 240             rxLinkStatistics.rfMode = linkStats->rfMode;
 241             rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex];
 242             rxLinkStatistics.activeAntenna = linkStats->activeAntenna;
 243
 244 #ifdef USE_OSD
 245             if (rxLinkStatistics.uplinkLQ > 0) {
 246                 int16_t uplinkStrength;   // RSSI dBm converted to %
 247                 uplinkStrength = constrain((100 * sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)) - (100 * sq((osdConfig()->rssi_dbm_max  - rxLinkStatistics.uplinkRSSI)))) / sq((osdConfig()->rssi_dbm_max - osdConfig()->rssi_dbm_min)),0,100);
 248                 if (rxLinkStatistics.uplinkRSSI >= osdConfig()->rssi_dbm_max )
 249                     uplinkStrength = 99;
 250                 else if (rxLinkStatistics.uplinkRSSI < osdConfig()->rssi_dbm_min)
 251                     uplinkStrength = 0;
 252                 lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(uplinkStrength, 0, 99, 0, RSSI_MAX_VALUE));
 253             } else {
 254                 lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
 255             }
 256 #endif
 257             // This is not RC channels frame, update channel value but don't indicate frame completion
 258             return RX_FRAME_PENDING;
 259         }
 260     }
 261     return RX_FRAME_PENDING;
 262 }
 263
 264 STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 265 {
 266     UNUSED(rxRuntimeConfig);
 267     /* conversion from RC value to PWM
 268      *       RC     PWM
 269      * min  172 ->  988us
 270      * mid  992 -> 1500us
 271      * max 1811 -> 2012us
 272      * scale factor = (2012-988) / (1811-172) = 0.62477120195241
 273      * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
 274      */
 275     return (crsfChannelData[chan] * 1024 / 1639) + 881;
 276 }
 277
 278 void crsfRxWriteTelemetryData(const void *data, int len)
 279 {
 280     len = MIN(len, (int)sizeof(telemetryBuf));
 281     memcpy(telemetryBuf, data, len);
 282     telemetryBufLen = len;
 283 }
 284
 285 void crsfRxSendTelemetryData(void)
 286 {
 287     // if there is telemetry data to write
 288     if (telemetryBufLen > 0) {
 289         // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame)
 290         // and that there is time to send the telemetry frame before the next RX frame arrives
 291         if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
 292             const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAt);
 293             if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) ||
 294                 (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
 295                 return;
 296             }
 297         }
 298         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 299         telemetryBufLen = 0; // reset telemetry buffer
 300     }
 301 }
 302
 303 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 304 {
 305     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
 306         crsfChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
 307     }
 308
 309     rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
 310     rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
 311     rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
 312
 313     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 314     if (!portConfig) {
 315         return false;
 316     }
 317
 318     serialPort = openSerialPort(portConfig->identifier,
 319         FUNCTION_RX_SERIAL,
 320         crsfDataReceive,
 321         NULL,
 322         CRSF_BAUDRATE,
 323         CRSF_PORT_MODE,
 324         CRSF_PORT_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
 325         );
 326
 327     return serialPort != NULL;
 328 }
 329
 330 bool crsfRxIsActive(void)
 331 {
 332     return serialPort != NULL;
 333 }
 334
 335
 336 void crsfBind(void)
 337 {
 338     if (serialPort != NULL) {
 339         uint8_t bindFrame[] = {
 340             CRSF_SYNC_BYTE,
 341             0x07,  // frame length
 342             CRSF_FRAMETYPE_COMMAND,
 343             CRSF_ADDRESS_CRSF_RECEIVER,
 344             CRSF_ADDRESS_FLIGHT_CONTROLLER,
 345             CRSF_COMMAND_SUBCMD_RX,
 346             CRSF_COMMAND_SUBCMD_RX_BIND,
 347             0x9E,  // Command CRC8
 348             0xE8,  // Packet CRC8
 349         };
 350         serialWriteBuf(serialPort, bindFrame, 9);
 351     }
 352 }
 353
 354 #endif