src/main/rx/sbus.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
  22 #include "platform.h"
  23
  24 #include "build_config.h"
  25
  26 #include "drivers/system.h"
  27
  28 #include "drivers/gpio.h"
  29 #include "drivers/inverter.h"
  30
  31 #include "drivers/serial.h"
  32 #include "drivers/serial_uart.h"
  33 #include "io/serial.h"
  34
  35 #include "rx/rx.h"
  36 #include "rx/sbus.h"
  37
  38 /*
  39  * Observations
  40  *
  41  * FrSky X8R
  42  * time between frames: 6ms.
  43  * time to send frame: 3ms.
  44 *
  45  * Futaba R6208SB/R6303SB
  46  * time between frames: 11ms.
  47  * time to send frame: 3ms.
  48  */
  49
  50 #define SBUS_TIME_NEEDED_PER_FRAME 3000
  51
  52 #ifndef CJMCU
  53 //#define DEBUG_SBUS_PACKETS
  54 #endif
  55
  56 #ifdef DEBUG_SBUS_PACKETS
  57 static uint16_t sbusStateFlags = 0;
  58
  59 #define SBUS_STATE_FAILSAFE (1 << 0)
  60 #define SBUS_STATE_SIGNALLOSS (1 << 1)
  61
  62 #endif
  63
  64 #define SBUS_MAX_CHANNEL 18
  65 #define SBUS_FRAME_SIZE 25
  66
  67 #define SBUS_FRAME_BEGIN_BYTE 0x0F
  68
  69 #define SBUS_BAUDRATE 100000
  70 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN | SERIAL_INVERTED)
  71
  72 #define SBUS_DIGITAL_CHANNEL_MIN 173
  73 #define SBUS_DIGITAL_CHANNEL_MAX 1812
  74
  75 static bool sbusFrameDone = false;
  76 static void sbusDataReceive(uint16_t c);
  77 static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
  78
  79 static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
  80
  81 bool sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
  82 {
  83     int b;
  84     for (b = 0; b < SBUS_MAX_CHANNEL; b++)
  85         sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
  86     if (callback)
  87         *callback = sbusReadRawRC;
  88     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
  89
  90     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
  91     if (!portConfig) {
  92         return false;
  93     }
  94
  95     serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, MODE_RX, SBUS_PORT_OPTIONS);
  96
  97     return sBusPort != NULL;
  98 }
  99
 100 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
 101 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
 102 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
 103 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
 104
 105 struct sbusFrame_s {
 106     uint8_t syncByte;
 107     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
 108     unsigned int chan0 : 11;
 109     unsigned int chan1 : 11;
 110     unsigned int chan2 : 11;
 111     unsigned int chan3 : 11;
 112     unsigned int chan4 : 11;
 113     unsigned int chan5 : 11;
 114     unsigned int chan6 : 11;
 115     unsigned int chan7 : 11;
 116     unsigned int chan8 : 11;
 117     unsigned int chan9 : 11;
 118     unsigned int chan10 : 11;
 119     unsigned int chan11 : 11;
 120     unsigned int chan12 : 11;
 121     unsigned int chan13 : 11;
 122     unsigned int chan14 : 11;
 123     unsigned int chan15 : 11;
 124     uint8_t flags;
 125     /**
 126      * The endByte is 0x00 on FrSky and some futaba RX's, on Some SBUS2 RX's the value indicates the telemetry byte that is sent after every 4th sbus frame.
 127      *
 128      * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
 129      * and
 130      * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
 131      */
 132     uint8_t endByte;
 133 } __attribute__ ((__packed__));
 134
 135 typedef union {
 136     uint8_t bytes[SBUS_FRAME_SIZE];
 137     struct sbusFrame_s frame;
 138 } sbusFrame_t;
 139
 140 static sbusFrame_t sbusFrame;
 141
 142 // Receive ISR callback
 143 static void sbusDataReceive(uint16_t c)
 144 {
 145     static uint8_t sbusFramePosition = 0;
 146     static uint32_t sbusFrameStartAt = 0;
 147     uint32_t now = micros();
 148
 149     int32_t sbusFrameTime = now - sbusFrameStartAt;
 150
 151     if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
 152         sbusFramePosition = 0;
 153     }
 154
 155     if (sbusFramePosition == 0) {
 156         if (c != SBUS_FRAME_BEGIN_BYTE) {
 157             return;
 158         }
 159         sbusFrameStartAt = now;
 160     }
 161
 162     if (sbusFramePosition < SBUS_FRAME_SIZE) {
 163         sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c;
 164         if (sbusFramePosition == SBUS_FRAME_SIZE) {
 165             // endByte currently ignored
 166             sbusFrameDone = true;
 167 #ifdef DEBUG_SBUS_PACKETS
 168             debug[2] = sbusFrameTime;
 169 #endif
 170         } else {
 171             sbusFrameDone = false;
 172         }
 173     }
 174 }
 175
 176 uint8_t sbusFrameStatus(void)
 177 {
 178     if (!sbusFrameDone) {
 179         return SERIAL_RX_FRAME_PENDING;
 180     }
 181     sbusFrameDone = false;
 182
 183 #ifdef DEBUG_SBUS_PACKETS
 184     sbusStateFlags = 0;
 185     debug[1] = sbusFrame.frame.flags;
 186 #endif
 187
 188     sbusChannelData[0] = sbusFrame.frame.chan0;
 189     sbusChannelData[1] = sbusFrame.frame.chan1;
 190     sbusChannelData[2] = sbusFrame.frame.chan2;
 191     sbusChannelData[3] = sbusFrame.frame.chan3;
 192     sbusChannelData[4] = sbusFrame.frame.chan4;
 193     sbusChannelData[5] = sbusFrame.frame.chan5;
 194     sbusChannelData[6] = sbusFrame.frame.chan6;
 195     sbusChannelData[7] = sbusFrame.frame.chan7;
 196     sbusChannelData[8] = sbusFrame.frame.chan8;
 197     sbusChannelData[9] = sbusFrame.frame.chan9;
 198     sbusChannelData[10] = sbusFrame.frame.chan10;
 199     sbusChannelData[11] = sbusFrame.frame.chan11;
 200     sbusChannelData[12] = sbusFrame.frame.chan12;
 201     sbusChannelData[13] = sbusFrame.frame.chan13;
 202     sbusChannelData[14] = sbusFrame.frame.chan14;
 203     sbusChannelData[15] = sbusFrame.frame.chan15;
 204
 205     if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_17) {
 206         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MAX;
 207     } else {
 208         sbusChannelData[16] = SBUS_DIGITAL_CHANNEL_MIN;
 209     }
 210
 211     if (sbusFrame.frame.flags & SBUS_FLAG_CHANNEL_18) {
 212         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MAX;
 213     } else {
 214         sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
 215     }
 216
 217     if (sbusFrame.frame.flags & SBUS_FLAG_SIGNAL_LOSS) {
 218 #ifdef DEBUG_SBUS_PACKETS
 219         sbusStateFlags |= SBUS_STATE_SIGNALLOSS;
 220         debug[0] = sbusStateFlags;
 221 #endif
 222     }
 223     if (sbusFrame.frame.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
 224         // internal failsafe enabled and rx failsafe flag set
 225 #ifdef DEBUG_SBUS_PACKETS
 226         sbusStateFlags |= SBUS_STATE_FAILSAFE;
 227         debug[0] = sbusStateFlags;
 228 #endif
 229         // RX *should* still be sending valid channel data, so use it.
 230         return SERIAL_RX_FRAME_COMPLETE | SERIAL_RX_FRAME_FAILSAFE;
 231     }
 232
 233 #ifdef DEBUG_SBUS_PACKETS
 234     debug[0] = sbusStateFlags;
 235 #endif
 236     return SERIAL_RX_FRAME_COMPLETE;
 237 }
 238
 239 static uint16_t sbusReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 240 {
 241     UNUSED(rxRuntimeConfig);
 242     // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
 243     // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
 244     return (0.625f * sbusChannelData[chan]) + 880;
 245 }