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)
  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     portOptions_t options = (rxConfig->sbus_inversion) ? (SBUS_PORT_OPTIONS | SERIAL_INVERTED) : SBUS_PORT_OPTIONS;
  96     serialPort_t *sBusPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, sbusDataReceive, SBUS_BAUDRATE, MODE_RX, options);
  97
  98     return sBusPort != NULL;
  99 }
 100
 101 #define SBUS_FLAG_CHANNEL_17        (1 << 0)
 102 #define SBUS_FLAG_CHANNEL_18        (1 << 1)
 103 #define SBUS_FLAG_SIGNAL_LOSS       (1 << 2)
 104 #define SBUS_FLAG_FAILSAFE_ACTIVE   (1 << 3)
 105
 106 struct sbusFrame_s {
 107     uint8_t syncByte;
 108     // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes.
 109     unsigned int chan0 : 11;
 110     unsigned int chan1 : 11;
 111     unsigned int chan2 : 11;
 112     unsigned int chan3 : 11;
 113     unsigned int chan4 : 11;
 114     unsigned int chan5 : 11;
 115     unsigned int chan6 : 11;
 116     unsigned int chan7 : 11;
 117     unsigned int chan8 : 11;
 118     unsigned int chan9 : 11;
 119     unsigned int chan10 : 11;
 120     unsigned int chan11 : 11;
 121     unsigned int chan12 : 11;
 122     unsigned int chan13 : 11;
 123     unsigned int chan14 : 11;
 124     unsigned int chan15 : 11;
 125     uint8_t flags;
 126     /**
 127      * 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.
 128      *
 129      * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
 130      * and
 131      * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
 132      */
 133     uint8_t endByte;
 134 } __attribute__ ((__packed__));
 135
 136 typedef union {
 137     uint8_t bytes[SBUS_FRAME_SIZE];
 138     struct sbusFrame_s frame;
 139 } sbusFrame_t;
 140
 141 static sbusFrame_t sbusFrame;
 142
 143 // Receive ISR callback
 144 static void sbusDataReceive(uint16_t c)
 145 {
 146     static uint8_t sbusFramePosition = 0;
 147     static uint32_t sbusFrameStartAt = 0;
 148     uint32_t now = micros();
 149
 150     int32_t sbusFrameTime = now - sbusFrameStartAt;
 151
 152     if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
 153         sbusFramePosition = 0;
 154     }
 155
 156     if (sbusFramePosition == 0) {
 157         if (c != SBUS_FRAME_BEGIN_BYTE) {
 158             return;
 159         }
 160         sbusFrameStartAt = now;
 161     }
 162
 163     if (sbusFramePosition < SBUS_FRAME_SIZE) {
 164         sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c;
 165         if (sbusFramePosition < SBUS_FRAME_SIZE) {
 166             sbusFrameDone = false;
 167         } else {
 168             sbusFrameDone = true;
 169 #ifdef DEBUG_SBUS_PACKETS
 170         debug[2] = sbusFrameTime;
 171 #endif
 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 }