src/rx_sbus.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3
   4 #include "platform.h"
   5
   6 #include "drivers/system_common.h"
   7
   8 #include "drivers/serial_common.h"
   9 #include "drivers/serial_uart.h"
  10 #include "runtime_config.h"
  11
  12 #include "failsafe.h"
  13
  14 #include "rx_common.h"
  15 #include "rx_sbus.h"
  16
  17 // driver for SBUS receiver using UART2
  18
  19 #define SBUS_MAX_CHANNEL 8
  20 #define SBUS_FRAME_SIZE 25
  21 #define SBUS_SYNCBYTE 0x0F
  22 #define SBUS_OFFSET 988
  23
  24 static bool sbusFrameDone = false;
  25 static void sbusDataReceive(uint16_t c);
  26 static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
  27
  28 static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
  29
  30 //rxConfig_t *rxConfig;
  31
  32 void sbusInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
  33 {
  34     int b;
  35
  36     //rxConfig = initialRxConfig;
  37
  38     for (b = 0; b < SBUS_MAX_CHANNEL; b++)
  39         sbusChannelData[b] = 2 * (rxConfig->midrc - SBUS_OFFSET);
  40     core.rcvrport = uartOpen(USART2, sbusDataReceive, 100000, (portMode_t)(MODE_RX | MODE_SBUS));
  41     if (callback)
  42         *callback = sbusReadRawRC;
  43     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
  44 }
  45
  46 struct sbus_dat
  47 {
  48     unsigned int chan0 : 11;
  49     unsigned int chan1 : 11;
  50     unsigned int chan2 : 11;
  51     unsigned int chan3 : 11;
  52     unsigned int chan4 : 11;
  53     unsigned int chan5 : 11;
  54     unsigned int chan6 : 11;
  55     unsigned int chan7 : 11;
  56     unsigned int chan8 : 11;
  57     unsigned int chan9 : 11;
  58     unsigned int chan10 : 11;
  59     unsigned int chan11 : 11;
  60 } __attribute__ ((__packed__));
  61
  62 typedef union
  63 {
  64     uint8_t in[SBUS_FRAME_SIZE];
  65     struct sbus_dat msg;
  66 } sbus_msg;
  67
  68 static sbus_msg sbus;
  69
  70 // Receive ISR callback
  71 static void sbusDataReceive(uint16_t c)
  72 {
  73     uint32_t sbusTime;
  74     static uint32_t sbusTimeLast;
  75     static uint8_t  sbusFramePosition;
  76
  77     sbusTime = micros();
  78     if ((sbusTime - sbusTimeLast) > 2500) // sbus2 fast timing
  79         sbusFramePosition = 0;
  80     sbusTimeLast = sbusTime;
  81
  82     if (sbusFramePosition == 0 && c != SBUS_SYNCBYTE)
  83         return;
  84
  85     sbusFrameDone = false; // lazy main loop didnt fetch the stuff
  86     if (sbusFramePosition != 0)
  87         sbus.in[sbusFramePosition - 1] = (uint8_t)c;
  88
  89     if (sbusFramePosition == SBUS_FRAME_SIZE - 1) {
  90         sbusFrameDone = true;
  91         sbusFramePosition = 0;
  92     } else {
  93         sbusFramePosition++;
  94     }
  95 }
  96
  97 bool sbusFrameComplete(void)
  98 {
  99     if (sbusFrameDone) {
 100         if (!((sbus.in[22] >> 3) & 0x0001)) { // failsave flag
 101             failsafeCnt = 0; // clear FailSafe counter
 102             sbusChannelData[0] = sbus.msg.chan0;
 103             sbusChannelData[1] = sbus.msg.chan1;
 104             sbusChannelData[2] = sbus.msg.chan2;
 105             sbusChannelData[3] = sbus.msg.chan3;
 106             sbusChannelData[4] = sbus.msg.chan4;
 107             sbusChannelData[5] = sbus.msg.chan5;
 108             sbusChannelData[6] = sbus.msg.chan6;
 109             sbusChannelData[7] = sbus.msg.chan7;
 110             // need more channels? No problem. Add them.
 111             sbusFrameDone = false;
 112             return true;
 113         }
 114         sbusFrameDone = false;
 115     }
 116     return false;
 117 }
 118
 119 static uint16_t sbusReadRawRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 120 {
 121     return sbusChannelData[rxConfig->rcmap[chan]] / 2 + SBUS_OFFSET;
 122 }
 123