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
  24 #ifdef USE_SERIALRX_SPEKTRUM
  25
  26 #include "build/debug.h"
  27
  28 #include "drivers/io.h"
  29 #include "drivers/io_impl.h"
  30 #include "drivers/light_led.h"
  31 #include "drivers/serial.h"
  32 #include "drivers/system.h"
  33
  34 #include "fc/config.h"
  35
  36 #include "io/serial.h"
  37
  38 #ifdef TELEMETRY
  39 #include "telemetry/telemetry.h"
  40 #endif
  41
  42 #include "rx/rx.h"
  43 #include "rx/spektrum.h"
  44
  45 // driver for spektrum satellite receiver / sbus
  46
  47 #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
  48 #define SPEKTRUM_2048_CHANNEL_COUNT 12
  49 #define SPEKTRUM_1024_CHANNEL_COUNT 7
  50
  51 #define SPEK_FRAME_SIZE 16
  52 #define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
  53
  54 #define SPEKTRUM_BAUDRATE 115200
  55
  56 #define SPEKTRUM_MAX_FADE_PER_SEC 40
  57 #define SPEKTRUM_FADE_REPORTS_PER_SEC 2
  58
  59 static uint8_t spek_chan_shift;
  60 static uint8_t spek_chan_mask;
  61 static bool rcFrameComplete = false;
  62 static bool spekHiRes = false;
  63
  64 // Variables used for calculating a signal strength from satellite fade.
  65 //  This is time-variant and computed every second based on the fade
  66 //  count over the last second.
  67 static uint32_t spek_fade_last_sec = 0; // Stores the timestamp of the last second.
  68 static uint16_t spek_fade_last_sec_count = 0; // Stores the fade count at the last second.
  69 static uint8_t rssi_channel; // Stores the RX RSSI channel.
  70
  71 static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
  72
  73 static rxRuntimeConfig_t *rxRuntimeConfigPtr;
  74
  75 #ifdef SPEKTRUM_BIND
  76 static IO_t BindPin = DEFIO_IO(NONE);
  77 #endif
  78 #ifdef HARDWARE_BIND_PLUG
  79 static IO_t BindPlug = DEFIO_IO(NONE);
  80 #endif
  81
  82
  83 // Receive ISR callback
  84 static void spektrumDataReceive(uint16_t c)
  85 {
  86     timeUs_t spekTime;
  87     timeDelta_t spekTimeInterval;
  88     static timeUs_t spekTimeLast = 0;
  89     static uint8_t spekFramePosition = 0;
  90
  91     spekTime = micros();
  92     spekTimeInterval = cmpTimeUs(spekTime, spekTimeLast);
  93     spekTimeLast = spekTime;
  94
  95     if (spekTimeInterval > SPEKTRUM_NEEDED_FRAME_INTERVAL) {
  96         spekFramePosition = 0;
  97     }
  98
  99     if (spekFramePosition < SPEK_FRAME_SIZE) {
 100         spekFrame[spekFramePosition++] = (uint8_t)c;
 101         if (spekFramePosition == SPEK_FRAME_SIZE) {
 102             rcFrameComplete = true;
 103         } else {
 104             rcFrameComplete = false;
 105         }
 106     }
 107 }
 108
 109 static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
 110
 111 uint8_t spektrumFrameStatus(void)
 112 {
 113     uint8_t b;
 114     uint16_t fade;
 115     uint32_t current_secs;
 116
 117     if (!rcFrameComplete) {
 118         return RX_FRAME_PENDING;
 119     }
 120
 121     rcFrameComplete = false;
 122
 123     // Fetch the fade count
 124     fade = (spekFrame[0] << 8) + spekFrame[1];
 125     current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
 126
 127     if (spek_fade_last_sec == 0) {
 128         // This is the first frame status received.
 129         spek_fade_last_sec_count = fade;
 130         spek_fade_last_sec = current_secs;
 131     } else if(spek_fade_last_sec != current_secs) {
 132         // If the difference is > 1, then we missed several seconds worth of frames and
 133         // should just throw out the fade calc (as it's likely a full signal loss).
 134         if((current_secs - spek_fade_last_sec) == 1) {
 135             if(rssi_channel != 0) {
 136                 if (spekHiRes)
 137                     spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
 138                 else
 139                     spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
 140             }
 141         }
 142         spek_fade_last_sec_count = fade;
 143         spek_fade_last_sec = current_secs;
 144     }
 145
 146
 147     for (b = 3; b < SPEK_FRAME_SIZE; b += 2) {
 148         uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
 149         if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
 150             if(rssi_channel != 0 && spekChannel != rssi_channel) {
 151                 spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
 152             }
 153         }
 154     }
 155
 156     return RX_FRAME_COMPLETE;
 157 }
 158
 159 static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
 160 {
 161     uint16_t data;
 162
 163     if (chan >= rxRuntimeConfig->channelCount) {
 164         return 0;
 165     }
 166
 167     if (spekHiRes)
 168         data = 988 + (spekChannelData[chan] >> 1);   // 2048 mode
 169     else
 170         data = 988 + spekChannelData[chan];          // 1024 mode
 171
 172     return data;
 173 }
 174
 175 #ifdef SPEKTRUM_BIND
 176
 177 bool spekShouldBind(uint8_t spektrum_sat_bind)
 178 {
 179 #ifdef HARDWARE_BIND_PLUG
 180     BindPlug = IOGetByTag(IO_TAG(BINDPLUG_PIN));
 181     IOInit(BindPlug, OWNER_RX, RESOURCE_INPUT, 0);
 182     IOConfigGPIO(BindPlug, IOCFG_IPU);
 183
 184     // Check status of bind plug and exit if not active
 185     delayMicroseconds(10);  // allow configuration to settle
 186     if (IORead(BindPlug)) {
 187         return false;
 188     }
 189 #endif
 190
 191     return !(
 192         isMPUSoftReset() ||
 193         spektrum_sat_bind == SPEKTRUM_SAT_BIND_DISABLED ||
 194         spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
 195     );
 196 }
 197 /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
 198  * Function must be called immediately after startup so that we don't miss satellite bind window.
 199  * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
 200  * 9 = DSMX 11ms / DSMX 22ms
 201  * 5 = DSM2 11ms 2048 / DSM2 22ms 1024
 202  */
 203 void spektrumBind(rxConfig_t *rxConfig)
 204 {
 205     int i;
 206     if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
 207         return;
 208     }
 209
 210     LED1_ON;
 211
 212     BindPin = IOGetByTag(IO_TAG(BIND_PIN));
 213     IOInit(BindPin, OWNER_RX, RESOURCE_OUTPUT, 0);
 214     IOConfigGPIO(BindPin, IOCFG_OUT_PP);
 215
 216     // RX line, set high
 217     IOWrite(BindPin, true);
 218
 219     // Bind window is around 20-140ms after powerup
 220     delay(60);
 221     LED1_OFF;
 222
 223     for (i = 0; i < rxConfig->spektrum_sat_bind; i++) {
 224
 225         LED0_OFF;
 226         LED2_OFF;
 227         // RX line, drive low for 120us
 228         IOWrite(BindPin, false);
 229         delayMicroseconds(120);
 230
 231         LED0_ON;
 232         LED2_ON;
 233         // RX line, drive high for 120us
 234         IOWrite(BindPin, true);
 235         delayMicroseconds(120);
 236
 237     }
 238
 239 #ifndef HARDWARE_BIND_PLUG
 240     // 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
 241     // Don't reset if hardware bind plug is present
 242     // Reset only when autoreset is enabled
 243     if (rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
 244         rxConfig->spektrum_sat_bind = 0;
 245         saveConfigAndNotify();
 246     }
 247 #endif
 248
 249 }
 250 #endif // SPEKTRUM_BIND
 251
 252 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 253 {
 254     rxRuntimeConfigPtr = rxRuntimeConfig;
 255
 256     switch (rxConfig->serialrx_provider) {
 257     case SERIALRX_SPEKTRUM2048:
 258         // 11 bit frames
 259         spek_chan_shift = 3;
 260         spek_chan_mask = 0x07;
 261         spekHiRes = true;
 262         rxRuntimeConfig->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
 263         rxRuntimeConfig->rxRefreshRate = 11000;
 264         break;
 265     case SERIALRX_SPEKTRUM1024:
 266         // 10 bit frames
 267         spek_chan_shift = 2;
 268         spek_chan_mask = 0x03;
 269         spekHiRes = false;
 270         rxRuntimeConfig->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
 271         rxRuntimeConfig->rxRefreshRate = 22000;
 272         break;
 273     }
 274
 275     rxRuntimeConfig->rcReadRawFn = spektrumReadRawRC;
 276     rxRuntimeConfig->rcFrameStatusFn = spektrumFrameStatus;
 277
 278     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 279     if (!portConfig) {
 280         return false;
 281     }
 282
 283 #ifdef TELEMETRY
 284     bool portShared = telemetryCheckRxPortShared(portConfig);
 285 #else
 286     bool portShared = false;
 287 #endif
 288
 289     serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
 290
 291 #ifdef TELEMETRY
 292     if (portShared) {
 293         telemetrySharedPort = spektrumPort;
 294     }
 295 #endif
 296
 297     rssi_channel = rxConfig->rssi_channel - 1; // -1 because rxConfig->rssi_channel is 1-based and rssi_channel is 0-based.
 298     if (rssi_channel >= rxRuntimeConfig->channelCount) {
 299         rssi_channel = 0;
 300     }
 301
 302     return spektrumPort != NULL;
 303 }
 304 #endif // USE_SERIALRX_SPEKTRUM