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 #ifdef USE_SERIAL_RX
  25
  26 #include "build/debug.h"
  27
  28 #include "common/utils.h"
  29
  30 #include "drivers/time.h"
  31
  32 #include "io/serial.h"
  33
  34 #ifdef USE_TELEMETRY
  35 #include "telemetry/telemetry.h"
  36 #endif
  37
  38 #include "rx/rx.h"
  39 #include "rx/sbus.h"
  40 #include "rx/sbus_channels.h"
  41
  42 /*
  43  * Observations
  44  *
  45  * FrSky X8R
  46  * time between frames: 6ms.
  47  * time to send frame: 3ms.
  48 *
  49  * Futaba R6208SB/R6303SB
  50  * time between frames: 11ms.
  51  * time to send frame: 3ms.
  52  */
  53
  54 #define SBUS_TIME_NEEDED_PER_FRAME 3000
  55
  56 #define SBUS_STATE_FAILSAFE (1 << 0)
  57 #define SBUS_STATE_SIGNALLOSS (1 << 1)
  58
  59 #define SBUS_FRAME_SIZE (SBUS_CHANNEL_DATA_LENGTH + 2)
  60
  61 #define SBUS_FRAME_BEGIN_BYTE 0x0F
  62
  63 #define SBUS_BAUDRATE 100000
  64
  65 #if !defined(SBUS_PORT_OPTIONS)
  66 #define SBUS_PORT_OPTIONS (SERIAL_STOPBITS_2 | SERIAL_PARITY_EVEN)
  67 #endif
  68
  69 #define SBUS_DIGITAL_CHANNEL_MIN 173
  70 #define SBUS_DIGITAL_CHANNEL_MAX 1812
  71
  72 enum {
  73     DEBUG_SBUS_FRAME_FLAGS = 0,
  74     DEBUG_SBUS_STATE_FLAGS,
  75     DEBUG_SBUS_FRAME_TIME,
  76 };
  77
  78
  79 struct sbusFrame_s {
  80     uint8_t syncByte;
  81     sbusChannels_t channels;
  82     /**
  83      * 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.
  84      *
  85      * See https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101027349
  86      * and
  87      * https://github.com/cleanflight/cleanflight/issues/590#issuecomment-101706023
  88      */
  89     uint8_t endByte;
  90 } __attribute__ ((__packed__));
  91
  92 typedef union sbusFrame_u {
  93     uint8_t bytes[SBUS_FRAME_SIZE];
  94     struct sbusFrame_s frame;
  95 } sbusFrame_t;
  96
  97 typedef struct sbusFrameData_s {
  98     sbusFrame_t frame;
  99     uint32_t startAtUs;
 100     uint16_t stateFlags;
 101     uint8_t position;
 102     bool done;
 103 } sbusFrameData_t;
 104
 105
 106 // Receive ISR callback
 107 static void sbusDataReceive(uint16_t c, void *data)
 108 {
 109     sbusFrameData_t *sbusFrameData = data;
 110
 111     const uint32_t nowUs = micros();
 112
 113     const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
 114
 115     if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
 116         sbusFrameData->position = 0;
 117     }
 118
 119     if (sbusFrameData->position == 0) {
 120         if (c != SBUS_FRAME_BEGIN_BYTE) {
 121             return;
 122         }
 123         sbusFrameData->startAtUs = nowUs;
 124     }
 125
 126     if (sbusFrameData->position < SBUS_FRAME_SIZE) {
 127         sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
 128         if (sbusFrameData->position < SBUS_FRAME_SIZE) {
 129             sbusFrameData->done = false;
 130         } else {
 131             sbusFrameData->done = true;
 132             DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
 133         }
 134     }
 135 }
 136
 137 static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 138 {
 139     sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
 140     if (!sbusFrameData->done) {
 141         return RX_FRAME_PENDING;
 142     }
 143     sbusFrameData->done = false;
 144
 145     DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
 146
 147     if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
 148         sbusFrameData->stateFlags |= SBUS_STATE_SIGNALLOSS;
 149         DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 150     }
 151     if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
 152         sbusFrameData->stateFlags |= SBUS_STATE_FAILSAFE;
 153         DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 154     }
 155
 156     DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
 157
 158     return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
 159 }
 160
 161 bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 162 {
 163     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
 164     static sbusFrameData_t sbusFrameData;
 165
 166     rxRuntimeConfig->channelData = sbusChannelData;
 167     rxRuntimeConfig->frameData = &sbusFrameData;
 168     sbusChannelsInit(rxConfig, rxRuntimeConfig);
 169
 170     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
 171     rxRuntimeConfig->rxRefreshRate = 11000;
 172
 173     rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
 174
 175     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 176     if (!portConfig) {
 177         return false;
 178     }
 179
 180 #ifdef USE_TELEMETRY
 181     bool portShared = telemetryCheckRxPortShared(portConfig);
 182 #else
 183     bool portShared = false;
 184 #endif
 185
 186     serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
 187         FUNCTION_RX_SERIAL,
 188         sbusDataReceive,
 189         &sbusFrameData,
 190         SBUS_BAUDRATE,
 191         portShared ? MODE_RXTX : MODE_RX,
 192         SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 193         );
 194
 195     if (rxConfig->rssi_src_frame_errors) {
 196         rssiSource = RSSI_SOURCE_FRAME_ERRORS;
 197     }
 198
 199 #ifdef USE_TELEMETRY
 200     if (portShared) {
 201         telemetrySharedPort = sBusPort;
 202     }
 203 #endif
 204
 205     return sBusPort != NULL;
 206 }
 207 #endif