src/main/rx/spektrum.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 #include "debug.h"
  24
  25 #include "drivers/gpio.h"
  26 #include "drivers/system.h"
  27
  28 #include "drivers/light_led.h"
  29
  30 #include "drivers/serial.h"
  31 #include "drivers/serial_uart.h"
  32 #include "io/serial.h"
  33
  34 #include "config/config.h"
  35
  36 #include "rx/rx.h"
  37 #include "rx/spektrum.h"
  38
  39 // driver for spektrum satellite receiver / sbus
  40
  41 #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
  42 #define SPEKTRUM_2048_CHANNEL_COUNT 12
  43 #define SPEKTRUM_1024_CHANNEL_COUNT 7
  44
  45 #define SPEK_FRAME_SIZE 16
  46 #define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
  47
  48 #define SPEKTRUM_BAUDRATE 115200
  49
  50 static uint8_t spek_chan_shift;
  51 static uint8_t spek_chan_mask;
  52 static bool rcFrameComplete = false;
  53 static bool spekHiRes = false;
  54
  55 static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
  56
  57 static void spektrumDataReceive(uint16_t c);
  58 static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan);
  59
  60 static rxRuntimeConfig_t *rxRuntimeConfigPtr;
  61
  62 bool spektrumInit(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, rcReadRawDataPtr *callback)
  63 {
  64     rxRuntimeConfigPtr = rxRuntimeConfig;
  65
  66     switch (rxConfig->serialrx_provider) {
  67         case SERIALRX_SPEKTRUM2048:
  68             // 11 bit frames
  69             spek_chan_shift = 3;
  70             spek_chan_mask = 0x07;
  71             spekHiRes = true;
  72             rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
  73             break;
  74         case SERIALRX_SPEKTRUM1024:
  75             // 10 bit frames
  76             spek_chan_shift = 2;
  77             spek_chan_mask = 0x03;
  78             spekHiRes = false;
  79             rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
  80             break;
  81     }
  82
  83     if (callback)
  84         *callback = spektrumReadRawRC;
  85
  86     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
  87     if (!portConfig) {
  88         return false;
  89     }
  90
  91     serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, MODE_RX, SERIAL_NOT_INVERTED);
  92
  93     return spektrumPort != NULL;
  94 }
  95
  96 // Receive ISR callback
  97 static void spektrumDataReceive(uint16_t c)
  98 {
  99     uint32_t spekTime, spekTimeInterval;
 100     static uint32_t spekTimeLast = 0;
 101     static uint8_t spekFramePosition = 0;
 102
 103     spekTime = micros();
 104     spekTimeInterval = spekTime - spekTimeLast;
 105     spekTimeLast = spekTime;
 106
 107     if (spekTimeInterval > SPEKTRUM_NEEDED_FRAME_INTERVAL) {
 108         spekFramePosition = 0;
 109     }
 110
 111     if (spekFramePosition < SPEK_FRAME_SIZE) {
 112         spekFrame[spekFramePosition++] = (uint8_t)c;
 113         if (spekFramePosition == SPEK_FRAME_SIZE) {
 114             rcFrameComplete = true;
 115         } else {
 116             rcFrameComplete = false;
 117         }
 118     }
 119 }
 120
 121 static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
 122
 123 uint8_t spektrumFrameStatus(void)
 124 {
 125     uint8_t b;
 126
 127     if (!rcFrameComplete) {
 128         return SERIAL_RX_FRAME_PENDING;
 129     }
 130
 131     rcFrameComplete = false;
 132
 133     for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
 134         uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
 135         if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
 136             spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
 137         }
 138     }
 139
 140     return SERIAL_RX_FRAME_COMPLETE;
 141 }
 142
 143 static uint16_t spektrumReadRawRC(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 144 {
 145     uint16_t data;
 146
 147     if (chan >= rxRuntimeConfig->channelCount) {
 148         return 0;
 149     }
 150
 151     if (spekHiRes)
 152         data = 988 + (spekChannelData[chan] >> 1);   // 2048 mode
 153     else
 154         data = 988 + spekChannelData[chan];          // 1024 mode
 155
 156     return data;
 157 }
 158
 159 #ifdef SPEKTRUM_BIND
 160
 161 bool spekShouldBind(uint8_t spektrum_sat_bind)
 162 {
 163 #ifdef HARDWARE_BIND_PLUG
 164     gpio_config_t cfg = {
 165         BINDPLUG_PIN,
 166         Mode_IPU,
 167         Speed_2MHz
 168     };
 169     gpioInit(BINDPLUG_PORT, &cfg);
 170
 171     // Check status of bind plug and exit if not active
 172     delayMicroseconds(10);  // allow configuration to settle
 173     if (digitalIn(BINDPLUG_PORT, BINDPLUG_PIN)) {
 174         return false;
 175     }
 176 #endif
 177
 178     return !(
 179         isMPUSoftReset() ||
 180         spektrum_sat_bind == SPEKTRUM_SAT_BIND_DISABLED ||
 181         spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
 182     );
 183 }
 184 /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
 185  * Function must be called immediately after startup so that we don't miss satellite bind window.
 186  * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
 187  * 9 = DSMX 11ms / DSMX 22ms
 188  * 5 = DSM2 11ms 2048 / DSM2 22ms 1024
 189  */
 190 void spektrumBind(rxConfig_t *rxConfig)
 191 {
 192     int i;
 193     if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
 194         return;
 195     }
 196
 197     LED1_ON;
 198
 199     gpio_config_t cfg = {
 200         BIND_PIN,
 201         Mode_Out_OD,
 202         Speed_2MHz
 203     };
 204     gpioInit(BIND_PORT, &cfg);
 205
 206     // RX line, set high
 207     digitalHi(BIND_PORT, BIND_PIN);
 208
 209     // Bind window is around 20-140ms after powerup
 210     delay(60);
 211     LED1_OFF;
 212
 213     for (i = 0; i < rxConfig->spektrum_sat_bind; i++) {
 214
 215         LED0_OFF;
 216         LED2_OFF;
 217         // RX line, drive low for 120us
 218         digitalLo(BIND_PORT, BIND_PIN);
 219         delayMicroseconds(120);
 220
 221         LED0_ON;
 222         LED2_ON;
 223         // RX line, drive high for 120us
 224         digitalHi(BIND_PORT, BIND_PIN);
 225         delayMicroseconds(120);
 226
 227     }
 228
 229 #ifndef HARDWARE_BIND_PLUG
 230     // If we came here as a result of hard  reset (power up, with spektrum_sat_bind set), then reset it back to zero and write config
 231     // Don't reset if hardware bind plug is present
 232     if (!isMPUSoftReset()) {
 233         rxConfig->spektrum_sat_bind = 0;
 234         saveConfigAndNotify();
 235     }
 236 #endif
 237
 238 }
 239 #endif