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
  25 #ifdef USE_SERIAL_RX
  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/serial.h"
  35 #include "drivers/serial_uart.h"
  36 #include "drivers/system.h"
  37 #include "drivers/time.h"
  38
  39 #include "io/serial.h"
  40
  41 #include "rx/rx.h"
  42 #include "rx/crsf.h"
  43
  44 #include "telemetry/crsf.h"
  45
  46 #define CRSF_TIME_NEEDED_PER_FRAME_US   1100 // 700 ms + 400 ms for potential ad-hoc request
  47 #define CRSF_TIME_BETWEEN_FRAMES_US     6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz
  48
  49 #define CRSF_DIGITAL_CHANNEL_MIN 172
  50 #define CRSF_DIGITAL_CHANNEL_MAX 1811
  51
  52 #define CRSF_PAYLOAD_OFFSET offsetof(crsfFrameDef_t, type)
  53
  54 STATIC_UNIT_TESTED bool crsfFrameDone = false;
  55 STATIC_UNIT_TESTED crsfFrame_t crsfFrame;
  56 STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL];
  57
  58 static serialPort_t *serialPort;
  59 static uint32_t crsfFrameStartAtUs = 0;
  60 static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX];
  61 static uint8_t telemetryBufLen = 0;
  62
  63 /*
  64  * CRSF protocol
  65  *
  66  * CRSF protocol uses a single wire half duplex uart connection.
  67  * The master sends one frame every 4ms and the slave replies between two frames from the master.
  68  *
  69  * 420000 baud
  70  * not inverted
  71  * 8 Bit
  72  * 1 Stop bit
  73  * Big endian
  74  * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte
  75  * Max frame size is 64 bytes
  76  * A 64 byte frame plus 1 sync byte can be transmitted in 1393 microseconds.
  77  *
  78  * CRSF_TIME_NEEDED_PER_FRAME_US is set conservatively at 1500 microseconds
  79  *
  80  * Every frame has the structure:
  81  * <Device address><Frame length><Type><Payload><CRC>
  82  *
  83  * Device address: (uint8_t)
  84  * Frame length:   length in  bytes including Type (uint8_t)
  85  * Type:           (uint8_t)
  86  * CRC:            (uint8_t)
  87  *
  88  */
  89
  90 struct crsfPayloadRcChannelsPacked_s {
  91     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
  92     unsigned int chan0 : 11;
  93     unsigned int chan1 : 11;
  94     unsigned int chan2 : 11;
  95     unsigned int chan3 : 11;
  96     unsigned int chan4 : 11;
  97     unsigned int chan5 : 11;
  98     unsigned int chan6 : 11;
  99     unsigned int chan7 : 11;
 100     unsigned int chan8 : 11;
 101     unsigned int chan9 : 11;
 102     unsigned int chan10 : 11;
 103     unsigned int chan11 : 11;
 104     unsigned int chan12 : 11;
 105     unsigned int chan13 : 11;
 106     unsigned int chan14 : 11;
 107     unsigned int chan15 : 11;
 108 } __attribute__ ((__packed__));
 109
 110 typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
 111
 112 STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void)
 113 {
 114     // CRC includes type and payload
 115     uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
 116     for (int ii = 0; ii < crsfFrame.frame.frameLength - CRSF_FRAME_LENGTH_TYPE_CRC; ++ii) {
 117         crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
 118     }
 119     return crc;
 120 }
 121
 122 // Receive ISR callback, called back from serial port
 123 STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
 124 {
 125     UNUSED(data);
 126
 127     static uint8_t crsfFramePosition = 0;
 128     const uint32_t currentTimeUs = micros();
 129
 130 #ifdef DEBUG_CRSF_PACKETS
 131     debug[2] = currentTimeUs - crsfFrameStartAtUs;
 132 #endif
 133
 134     if (currentTimeUs > crsfFrameStartAtUs + CRSF_TIME_NEEDED_PER_FRAME_US) {
 135         // We've received a character after max time needed to complete a frame,
 136         // so this must be the start of a new frame.
 137         crsfFramePosition = 0;
 138     }
 139
 140     if (crsfFramePosition == 0) {
 141         crsfFrameStartAtUs = currentTimeUs;
 142     }
 143     // assume frame is 5 bytes long until we have received the frame length
 144     // full frame length includes the length of the address and framelength fields
 145     const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH;
 146
 147     if (crsfFramePosition < fullFrameLength) {
 148         crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
 149         crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true;
 150         if (crsfFrameDone) {
 151             crsfFramePosition = 0;
 152             if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 153                 const uint8_t crc = crsfFrameCRC();
 154                 if (crc == crsfFrame.bytes[fullFrameLength - 1]) {
 155                     switch (crsfFrame.frame.type)
 156                     {
 157 #if defined(USE_MSP_OVER_TELEMETRY)
 158                         case CRSF_FRAMETYPE_MSP_REQ:
 159                         case CRSF_FRAMETYPE_MSP_WRITE: ;
 160                             uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
 161                             if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) {
 162                                 crsfScheduleMspResponse();
 163                             }
 164                             break;
 165 #endif
 166                         case CRSF_FRAMETYPE_DEVICE_PING:
 167                             crsfScheduleDeviceInfoResponse();
 168                             break;
 169                         default:
 170                             break;
 171                     }
 172                 }
 173             }
 174         }
 175     }
 176 }
 177
 178 STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 179 {
 180     UNUSED(rxRuntimeConfig);
 181
 182     if (crsfFrameDone) {
 183         crsfFrameDone = false;
 184         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
 185             // CRC includes type and payload of each frame
 186             const uint8_t crc = crsfFrameCRC();
 187             if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
 188                 return RX_FRAME_PENDING;
 189             }
 190             // unpack the RC channels
 191             const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;
 192             crsfChannelData[0] = rcChannels->chan0;
 193             crsfChannelData[1] = rcChannels->chan1;
 194             crsfChannelData[2] = rcChannels->chan2;
 195             crsfChannelData[3] = rcChannels->chan3;
 196             crsfChannelData[4] = rcChannels->chan4;
 197             crsfChannelData[5] = rcChannels->chan5;
 198             crsfChannelData[6] = rcChannels->chan6;
 199             crsfChannelData[7] = rcChannels->chan7;
 200             crsfChannelData[8] = rcChannels->chan8;
 201             crsfChannelData[9] = rcChannels->chan9;
 202             crsfChannelData[10] = rcChannels->chan10;
 203             crsfChannelData[11] = rcChannels->chan11;
 204             crsfChannelData[12] = rcChannels->chan12;
 205             crsfChannelData[13] = rcChannels->chan13;
 206             crsfChannelData[14] = rcChannels->chan14;
 207             crsfChannelData[15] = rcChannels->chan15;
 208             return RX_FRAME_COMPLETE;
 209         }
 210     }
 211     return RX_FRAME_PENDING;
 212 }
 213
 214 STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 215 {
 216     UNUSED(rxRuntimeConfig);
 217     /* conversion from RC value to PWM
 218      *       RC     PWM
 219      * min  172 ->  988us
 220      * mid  992 -> 1500us
 221      * max 1811 -> 2012us
 222      * scale factor = (2012-988) / (1811-172) = 0.62477120195241
 223      * offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
 224      */
 225     return (0.62477120195241f * crsfChannelData[chan]) + 881;
 226 }
 227
 228 void crsfRxWriteTelemetryData(const void *data, int len)
 229 {
 230     len = MIN(len, (int)sizeof(telemetryBuf));
 231     memcpy(telemetryBuf, data, len);
 232     telemetryBufLen = len;
 233 }
 234
 235 void crsfRxSendTelemetryData(void)
 236 {
 237     // if there is telemetry data to write
 238     if (telemetryBufLen > 0) {
 239         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 240         telemetryBufLen = 0; // reset telemetry buffer
 241     }
 242 }
 243
 244 bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 245 {
 246     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
 247         crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408;
 248     }
 249
 250     rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL;
 251     rxRuntimeConfig->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking
 252
 253     rxRuntimeConfig->rcReadRawFn = crsfReadRawRC;
 254     rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus;
 255
 256     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 257     if (!portConfig) {
 258         return false;
 259     }
 260
 261     serialPort = openSerialPort(portConfig->identifier,
 262         FUNCTION_RX_SERIAL,
 263         crsfDataReceive,
 264         NULL,
 265         CRSF_BAUDRATE,
 266         CRSF_PORT_MODE,
 267         CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
 268         );
 269
 270     return serialPort != NULL;
 271 }
 272
 273 bool crsfRxIsActive(void)
 274 {
 275     return serialPort != NULL;
 276 }
 277 #endif