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 #include "io/osd.h"
  40
  41 #include "rx/rx.h"
  42 #include "rx/crsf.h"
  43
  44 #include "telemetry/crsf.h"
  45 #define CRSF_TIME_NEEDED_PER_FRAME_US   1100 // 700 ms + 400 ms for potential ad-hoc request
  46 #define CRSF_TIME_BETWEEN_FRAMES_US     6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
  47
  48 #define CRSF_DIGITAL_CHANNEL_MIN 172
  49 #define CRSF_DIGITAL_CHANNEL_MAX 1811
  50 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
  51 #define CRSF_POWER_COUNT 9
  52
  53 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  54 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  55
  56 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  57
  58 static serialPort_t *serialPort;
  59 static timeUs_t crsfFrameStartAt = 0;
  60 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  61 static uint8_t telemetryBufLen = 0;
  62
  63 const uint16_t crsfTxPowerStatesmW[CRSF_POWER_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250, 50};
  64
  65 /*
  66  * CRSF protocol
  67  *
  68  * CRSF protocol uses a single wire half duplex uart connection.
  69  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  70  *
  71  * 420000 baud
  72  * not inverted
  73  * 8 Bit
  74  * 1 Stop bit
  75  * Big endian
  76  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  77  * Max frame size is 64 bytes
  78  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  79  *
  80  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  81  *
  82  * Every frame has the structure:
  83  * <Device address> <Frame length> < Type> <Payload> < CRC>
  84  *
  85  * Device address: (uint8_t)
  86  * Frame length:   length in  bytes including Type (uint8_t)
  87  * Type:           (uint8_t)
  88  * CRC:            (uint8_t)
  89  *
  90  */
  91
  92 struct crsfPayloadRcChannelsPacked_s {
  93     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
  94     unsigned int chan0 : 11;
  95     unsigned int chan1 : 11;
  96     unsigned int chan2 : 11;
  97     unsigned int chan3 : 11;
  98     unsigned int chan4 : 11;
  99     unsigned int chan5 : 11;
 100     unsigned int chan6 : 11;
 101     unsigned int chan7 : 11;
 102     unsigned int chan8 : 11;
 103     unsigned int chan9 : 11;
 104     unsigned int chan10 : 11;
 105     unsigned int chan11 : 11;
 106     unsigned int chan12 : 11;
 107     unsigned int chan13 : 11;
 108     unsigned int chan14 : 11;
 109     unsigned int chan15 : 11;
 110 } __attribute__ ((__packed__));
 111
 112 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 113
 114 typedef struct crsfPayloadLinkStatistics_s {
 115     uint8_t     uplinkRSSIAnt1;
 116     uint8_t     uplinkRSSIAnt2;
 117     uint8_t     uplinkLQ;
 118     int8_t      uplinkSNR;
 119     uint8_t     activeAntenna;
 120     uint8_t     rfMode;
 121     uint8_t     uplinkTXPower;
 122     uint8_t     downlinkRSSI;
 123     uint8_t     downlinkLQ;
 124     int8_t      downlinkSNR;
 125 } __attribute__ ((__packed__)) crsfPayloadLinkStatistics_t;
 126
 127 typedef struct crsfPayloadLinkStatistics_s crsfPayloadLinkStatistics_t;
 128
 129 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 130 {
 131     // CRC includes type and payload
 132     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 133     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 134         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 135     }
 136     return crc;
 137 }
 138
 139 // Receive ISR callback, called back from serial port
 140 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *rxCallbackData)
 141 {
 142     UNUSED(rxCallbackData);
 143
 144     static uint8_t crsfFramePosition = 0;
 145     const timeUs_t now = micros();
 146
 147 #ifdef DEBUG_CRSF_PACKETS
 148     debug[2] = now - crsfFrameStartAt;
 149 #endif
 150
 151     if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) {
 152         // We've received a character after max time needed to complete a frame,
 153         // so this must be the start of a new frame.
 154         crsfFramePosition = 0;
 155     }
 156
 157     if (crsfFramePosition == 0) {
 158         crsfFrameStartAt = now;
 159     }
 160     // assume frame is 5 bytes long until we have received the frame length
 161     // full frame length includes the length of the address and framelength fields
 162     const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 163
 164     if (crsfFramePosition < fullFrameLength) {
 165         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 166         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
 167         if (crsfFrameDone) {
 168             crsfFramePosition = 0;
 169             if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 170                 const uint8_t crc = crsfFrameCRC();
 171                 if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 172                     switch (crsfFrame.frame.type)
 173                     {
 174 #if defined(USE_MSP_OVER_TELEMETRY)
 175                         case CRSF_FRAMETYPE_MSP_REQ:
 176                         case CRSF_FRAMETYPE_MSP_WRITE: {
 177                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 178                             if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 179                                 crsfScheduleMspResponse();
 180                             }
 181                             break;
 182                         }
 183 #endif
 184                         default:
 185                             break;
 186                     }
 187                 }
 188             }
 189         }
 190     }
 191 }
 192
 193 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 194 {
 195     UNUSED(rxRuntimeConfig);
 196
 197     if (crsfFrameDone) {
 198         crsfFrameDone = false;
 199         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 200             // CRC includes type and payload of each frame
 201             const uint8_t crc = crsfFrameCRC();
 202             if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
 203                 return RX_FRAME_PENDING;
 204             }
 205             crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 206
 207             // unpack the RC channels
 208             const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
 209             crsfChannelData[0] = rcChannels->chan0;
 210             crsfChannelData[1] = rcChannels->chan1;
 211             crsfChannelData[2] = rcChannels->chan2;
 212             crsfChannelData[3] = rcChannels->chan3;
 213             crsfChannelData[4] = rcChannels->chan4;
 214             crsfChannelData[5] = rcChannels->chan5;
 215             crsfChannelData[6] = rcChannels->chan6;
 216             crsfChannelData[7] = rcChannels->chan7;
 217             crsfChannelData[8] = rcChannels->chan8;
 218             crsfChannelData[9] = rcChannels->chan9;
 219             crsfChannelData[10] = rcChannels->chan10;
 220             crsfChannelData[11] = rcChannels->chan11;
 221             crsfChannelData[12] = rcChannels->chan12;
 222             crsfChannelData[13] = rcChannels->chan13;
 223             crsfChannelData[14] = rcChannels->chan14;
 224             crsfChannelData[15] = rcChannels->chan15;
 225             return RX_FRAME_COMPLETE;
 226         }
 227         else if (crsfFrame.frame.type == CRSF_FRAMETYPE_LINK_STATISTICS) {
 228             // CRC includes type and payload of each frame
 229             const uint8_t crc = crsfFrameCRC();
 230             if (crc != crsfFrame.frame.payload[CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE]) {
 231                 return RX_FRAME_PENDING;
 232             }
 233             crsfFrame.frame.frameLength = CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
 234
 235             const crsfPayloadLinkStatistics_t* linkStats = (crsfPayloadLinkStatistics_t*)&crsfFrame.frame.payload;
 236             const uint8_t crsftxpowerindex = (linkStats->uplinkTXPower < CRSF_POWER_COUNT) ? linkStats->uplinkTXPower : 0;
 237
 238             rxLinkStatistics.uplinkRSSI = -1* (linkStats->activeAntenna ? linkStats->uplinkRSSIAnt2 : linkStats->uplinkRSSIAnt1);
 239             rxLinkStatistics.uplinkLQ = linkStats->uplinkLQ;
 240             rxLinkStatistics.uplinkSNR = linkStats->uplinkSNR;
 241             rxLinkStatistics.rfMode = linkStats->rfMode;
 242             rxLinkStatistics.uplinkTXPower = crsfTxPowerStatesmW[crsftxpowerindex];
 243             rxLinkStatistics.activeAntenna = linkStats->activeAntenna;
 244
 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             }
 254             else
 255                 lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
 256
 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->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking
 311
 312     rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
 313     rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
 314
 315     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 316     if (!portConfig) {
 317         return false;
 318     }
 319
 320     serialPort = openSerialPort(portConfig->identifier,
 321         FUNCTION_RX_SERIAL,
 322         crsfDataReceive,
 323         NULL,
 324         CRSF_BAUDRATE,
 325         CRSF_PORT_MODE,
 326         CRSF_PORT_OPTIONS | (tristateWithDefaultOffIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
 327         );
 328
 329     return serialPort != NULL;
 330 }
 331
 332 bool crsfRxIsActive(void)
 333 {
 334     return serialPort != NULL;
 335 }
 336 #endif