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