src/main/rx/sbus.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_SERIAL_RX
  28
  29 #include "build/debug.h"
  30
  31 #include "common/utils.h"
  32
  33 #include "drivers/time.h"
  34
  35 #include "io/serial.h"
  36
  37 #ifdef USE_TELEMETRY
  38 #include "telemetry/telemetry.h"
  39 #endif
  40
  41 #include "rx/rx.h"
  42 #include "rx/sbus.h"
  43 #include "rx/sbus_channels.h"
  44
  45 /*
  46  * Observations
  47  *
  48  * FrSky X8R
  49  * time between frames: 6ms.
  50  * time to send frame: 3ms.
  51 *
  52  * Futaba R6208SB/R6303SB
  53  * time between frames: 11ms.
  54  * time to send frame: 3ms.
  55  */
  56
  57 #define SBUS_TIME_NEEDED_PER_FRAME 3000
  58
  59 #define SBUS_STATE_FAILSAFE (1 << 0)
  60 #define SBUS_STATE_SIGNALLOSS (1 << 1)
  61
  62 #define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
  63
  64 #define SBUS_FRAME_BEGIN_BYTE 0x0F
  65
  66 #define SBUS_BAUDRATE 100000
  67
  68 #if !defined(SBUS_PORT_OPTIONS)
  69 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
  70 #endif
  71
  72 #define SBUS_DIGITAL_CHANNEL_MIN 173
  73 #define SBUS_DIGITAL_CHANNEL_MAX 1812
  74
  75 enum {
  76     DEBUG_SBUS_FRAME_FLAGS = 0,
  77     DEBUG_SBUS_STATE_FLAGS,
  78     DEBUG_SBUS_FRAME_TIME,
  79 };
  80
  81
  82 struct sbusFrame_s {
  83     uint8_t syncByte;
  84     sbusChannels_t channels;
  85     /**
  86      * 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.
  87      *
  88      * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
  89      * and
  90      * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
  91      */
  92     uint8_t endByte;
  93 } __attribute__ ((__packed__));
  94
  95 typedef union sbusFrame_u {
  96     uint8_t bytes[SBUS_FRAME_SIZE];
  97     struct sbusFrame_s frame;
  98 } sbusFrame_t;
  99
 100 typedef struct sbusFrameData_s {
 101     sbusFrame_t frame;
 102     uint32_t startAtUs;
 103     uint16_t stateFlags;
 104     uint8_t position;
 105     bool done;
 106 } sbusFrameData_t;
 107
 108
 109 // Receive ISR callback
 110 static void sbusDataReceive(uint16_t c, void *data)
 111 {
 112     sbusFrameData_t *sbusFrameData = data;
 113
 114     const uint32_t nowUs = micros();
 115
 116     const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
 117
 118     if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
 119         sbusFrameData->position = 0;
 120     }
 121
 122     if (sbusFrameData->position == 0) {
 123         if (c != SBUS_FRAME_BEGIN_BYTE) {
 124             return;
 125         }
 126         sbusFrameData->startAtUs = nowUs;
 127     }
 128
 129     if (sbusFrameData->position < SBUS_FRAME_SIZE) {
 130         sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
 131         if (sbusFrameData->position < SBUS_FRAME_SIZE) {
 132             sbusFrameData->done = false;
 133         } else {
 134             sbusFrameData->done = true;
 135             DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
 136         }
 137     }
 138 }
 139
 140 static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 141 {
 142     sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
 143     if (!sbusFrameData->done) {
 144         return RX_FRAME_PENDING;
 145     }
 146     sbusFrameData->done = false;
 147
 148     DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
 149
 150     if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
 151         sbusFrameData->stateFlags |= SBUS_STATE_SIGNALLOSS;
 152         DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 153     }
 154     if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
 155         sbusFrameData->stateFlags |= SBUS_STATE_FAILSAFE;
 156         DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 157     }
 158
 159     DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 160
 161     return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
 162 }
 163
 164 bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 165 {
 166     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
 167     static sbusFrameData_t sbusFrameData;
 168
 169     rxRuntimeConfig->channelData = sbusChannelData;
 170     rxRuntimeConfig->frameData = &sbusFrameData;
 171     sbusChannelsInit(rxConfig, rxRuntimeConfig);
 172
 173     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
 174     rxRuntimeConfig->rxRefreshRate = 11000;
 175
 176     rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
 177
 178     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 179     if (!portConfig) {
 180         return false;
 181     }
 182
 183 #ifdef USE_TELEMETRY
 184     bool portShared = telemetryCheckRxPortShared(portConfig);
 185 #else
 186     bool portShared = false;
 187 #endif
 188
 189     serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
 190         FUNCTION_RX_SERIAL,
 191         sbusDataReceive,
 192         &sbusFrameData,
 193         SBUS_BAUDRATE,
 194         portShared ? MODE_RXTX : MODE_RX,
 195         SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 196         );
 197
 198     if (rxConfig->rssi_src_frame_errors) {
 199         rssiSource = RSSI_SOURCE_FRAME_ERRORS;
 200     }
 201
 202 #ifdef USE_TELEMETRY
 203     if (portShared) {
 204         telemetrySharedPort = sBusPort;
 205     }
 206 #endif
 207
 208     return sBusPort != NULL;
 209 }
 210 #endif