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 FILE_COMPILE_FOR_SPEED
  25 #ifdef USE_SERIALRX_CRSF
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/crc.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/time.h"
  35 #include "drivers/serial.h"
  36 #include "drivers/serial_uart.h"
  37
  38 #include "io/serial.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
  51 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  52 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  53
  54 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  55
  56 static serialPort_t *serialPort;
  57 static timeUs_t crsfFrameStartAt = 0;
  58 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  59 static uint8_t telemetryBufLen = 0;
  60
  61
  62 /*
  63  * CRSF protocol
  64  *
  65  * CRSF protocol uses a single wire half duplex uart connection.
  66  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  67  *
  68  * 420000 baud
  69  * not inverted
  70  * 8 Bit
  71  * 1 Stop bit
  72  * Big endian
  73  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  74  * Max frame size is 64 bytes
  75  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  76  *
  77  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  78  *
  79  * Every frame has the structure:
  80  * <Device address> <Frame length> < Type> <Payload> < CRC>
  81  *
  82  * Device address: (uint8_t)
  83  * Frame length:   length in  bytes including Type (uint8_t)
  84  * Type:           (uint8_t)
  85  * CRC:            (uint8_t)
  86  *
  87  */
  88
  89 struct crsfPayloadRcChannelsPacked_s {
  90     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
  91     unsigned int chan0 : 11;
  92     unsigned int chan1 : 11;
  93     unsigned int chan2 : 11;
  94     unsigned int chan3 : 11;
  95     unsigned int chan4 : 11;
  96     unsigned int chan5 : 11;
  97     unsigned int chan6 : 11;
  98     unsigned int chan7 : 11;
  99     unsigned int chan8 : 11;
 100     unsigned int chan9 : 11;
 101     unsigned int chan10 : 11;
 102     unsigned int chan11 : 11;
 103     unsigned int chan12 : 11;
 104     unsigned int chan13 : 11;
 105     unsigned int chan14 : 11;
 106     unsigned int chan15 : 11;
 107 } __attribute__ ((__packed__));
 108
 109 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 110
 111 struct crsfPayloadLinkStatistics_s {
 112     uint8_t     uplinkRSSIAnt1;
 113     uint8_t     uplinkRSSIAnt2;
 114     uint8_t     uplinkLQ;
 115     int8_t      uplinkSNR;
 116     uint8_t     activeAntenna;
 117     uint8_t     rfMode;
 118     uint8_t     uplinkTXPower;
 119     uint8_t     downlinkRSSI;
 120     uint8_t     downlinkLQ;
 121     int8_t      downlinkSNR;
 122 } __attribute__ ((__packed__));
 123
 124 typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
 125
 126 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 127 {
 128     // CRC includes type and payload
 129     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 130     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 131         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 132     }
 133     return crc;
 134 }
 135
 136 // Receive ISR callback, called back from serial port
 137 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
 138 {
 139     UNUSED(rxCallbackData);
 140
 141     static uint8_t crsfFramePosition = 0;
 142     const timeUs_t now = micros();
 143
 144 #ifdef DEBUG_CRSF_PACKETS
 145     debug[2] = now - crsfFrameStartAt;
 146 #endif
 147
 148     if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
 149         // We've received a character after max time needed to complete a frame,
 150         // so this must be the start of a new frame.
 151         crsfFramePosition = 0;
 152     }
 153
 154     if (crsfFramePosition == 0) {
 155         crsfFrameStartAt = now;
 156     }
 157     // assume frame is 5 bytes long until we have received the frame length
 158     // full frame length includes the length of the address and framelength fields
 159     const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 160
 161     if (crsfFramePosition < fullFrameLength) {
 162         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 163         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
 164         if (crsfFrameDone) {
 165             crsfFramePosition = 0;
 166             if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 167                 const uint8_t crc = crsfFrameCRC();
 168                 if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 169                     switch (crsfFrame.frame.type)
 170                     {
 171 #if defined(USE_MSP_OVER_TELEMETRY)
 172                         case CRSF_FRAMETYPE_MSP_REQ:
 173                         case CRSF_FRAMETYPE_MSP_WRITE: {
 174                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 175                             if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 176                                 crsfScheduleMspResponse();
 177                             }
 178                             break;
 179                         }
 180 #endif
 181                         default:
 182                             break;
 183                     }
 184                 }
 185             }
 186         }
 187     }
 188 }
 189
 190 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 191 {
 192     UNUSED(rxRuntimeConfig);
 193
 194     if (crsfFrameDone) {
 195         crsfFrameDone = false;
 196         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 197             // CRC includes type and payload of each frame
 198             const uint8_t crc = crsfFrameCRC();
 199             if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
 200                 return RX_FRAME_PENDING;
 201             }
 202             crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 203
 204             // unpack the RC channels
 205             const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
 206             crsfChannelData[0] = rcChannels->chan0;
 207             crsfChannelData[1] = rcChannels->chan1;
 208             crsfChannelData[2] = rcChannels->chan2;
 209             crsfChannelData[3] = rcChannels->chan3;
 210             crsfChannelData[4] = rcChannels->chan4;
 211             crsfChannelData[5] = rcChannels->chan5;
 212             crsfChannelData[6] = rcChannels->chan6;
 213             crsfChannelData[7] = rcChannels->chan7;
 214             crsfChannelData[8] = rcChannels->chan8;
 215             crsfChannelData[9] = rcChannels->chan9;
 216             crsfChannelData[10] = rcChannels->chan10;
 217             crsfChannelData[11] = rcChannels->chan11;
 218             crsfChannelData[12] = rcChannels->chan12;
 219             crsfChannelData[13] = rcChannels->chan13;
 220             crsfChannelData[14] = rcChannels->chan14;
 221             crsfChannelData[15] = rcChannels->chan15;
 222             return RX_FRAME_COMPLETE;
 223         }
 224         else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
 225             // CRC includes type and payload of each frame
 226             const uint8_t crc = crsfFrameCRC();
 227             if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
 228                 return RX_FRAME_PENDING;
 229             }
 230             crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 231
 232             // Inject link quality into channel 17
 233             const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
 234
 235             crsfChannelData[16] = scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 191, 1791);    // will map to [1000;2000] range
 236
 237             lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(linkStats->uplinkLQ, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
 238
 239             // This is not RC channels frame, update channel value but don't indicate frame completion
 240             return RX_FRAME_PENDING;
 241         }
 242     }
 243     return RX_FRAME_PENDING;
 244 }
 245
 246 STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 247 {
 248     UNUSED(rxRuntimeConfig);
 249     /* conversion from RC value to PWM
 250      *       RC     PWM
 251      * min  172 ->  988us
 252      * mid  992 -> 1500us
 253      * max 1811 -> 2012us
 254      * scale factor = (2012-988) / (1811-172) = 0.62477120195241
 255      * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
 256      */
 257     return (crsfChannelData[chan] * 1024 / 1639) + 881;
 258 }
 259
 260 void crsfRxWriteTelemetryData(const void *data, int len)
 261 {
 262     len = MIN(len, (int)sizeof(telemetryBuf));
 263     memcpy(telemetryBuf, data, len);
 264     telemetryBufLen = len;
 265 }
 266
 267 void crsfRxSendTelemetryData(void)
 268 {
 269     // if there is telemetry data to write
 270     if (telemetryBufLen > 0) {
 271         // 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)
 272         // and that there is time to send the telemetry frame before the next RX frame arrives
 273         if (CRSF_PORT_OPTIONS & SERIAL_BIDIR) {
 274             const timeDelta_t timeSinceStartOfFrame = cmpTimeUs(micros(), crsfFrameStartAt);
 275             if ((timeSinceStartOfFrame < CRSF_TIME_NEEDED_PER_FRAME_US) ||
 276                 (timeSinceStartOfFrame > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) {
 277                 return;
 278             }
 279         }
 280         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 281         telemetryBufLen = 0; // reset telemetry buffer
 282     }
 283 }
 284
 285 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 286 {
 287     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
 288         crsfChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408;
 289     }
 290
 291     rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
 292     rxRuntimeConfig->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking
 293
 294     rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
 295     rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
 296
 297     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 298     if (!portConfig) {
 299         return false;
 300     }
 301
 302     serialPort = openSerialPort(portConfig->identifier,
 303         FUNCTION_RX_SERIAL,
 304         crsfDataReceive,
 305         NULL,
 306         CRSF_BAUDRATE,
 307         CRSF_PORT_MODE,
 308         CRSF_PORT_OPTIONS | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 309         );
 310
 311     return serialPort != NULL;
 312 }
 313
 314 bool crsfRxIsActive(void)
 315 {
 316     return serialPort != NULL;
 317 }
 318 #endif