src/main/rx/spektrum.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "platform.h"
  22
  23 #ifdef USE_SERIALRX_SPEKTRUM
  24
  25 #include <string.h>
  26
  27 #include "common/maths.h"
  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/system.h"
  35 #include "drivers/time.h"
  36 #include "drivers/serial.h"
  37 #include "drivers/serial_uart.h"
  38
  39 #include "io/serial.h"
  40
  41 #include "config/config.h"
  42
  43 #include "io/spektrum_rssi.h"
  44 #include "io/spektrum_vtx_control.h"
  45
  46 #include "telemetry/telemetry.h"
  47 #include "telemetry/srxl.h"
  48
  49 #include "pg/rx.h"
  50
  51 #include "rx/rx.h"
  52 #include "rx/spektrum.h"
  53
  54 #include "config/feature.h"
  55
  56 // driver for spektrum satellite receiver / sbus
  57
  58 #define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
  59
  60 bool srxlEnabled = false;
  61 int32_t resolution;
  62 uint8_t rssi_channel;
  63
  64 static uint8_t spek_chan_shift;
  65 static uint8_t spek_chan_mask;
  66 static bool rcFrameComplete = false;
  67 static bool spekHiRes = false;
  68
  69 static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
  70
  71 static rxRuntimeState_t *rxRuntimeStatePtr;
  72 static serialPort_t *serialPort;
  73
  74 #if defined(USE_TELEMETRY_SRXL)
  75 static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
  76 static uint8_t telemetryBufLen = 0;
  77 #endif
  78
  79 // Receive ISR callback
  80 static void spektrumDataReceive(uint16_t c, void *data)
  81 {
  82     rxRuntimeState_t *const rxRuntimeState = (rxRuntimeState_t *const)data;
  83
  84     static timeUs_t spekTimeLast = 0;
  85     static uint8_t spekFramePosition = 0;
  86
  87     const timeUs_t now = microsISR();
  88     const timeUs_t spekTimeInterval = cmpTimeUs(now, spekTimeLast);
  89     spekTimeLast = now;
  90
  91     if (spekTimeInterval > SPEKTRUM_NEEDED_FRAME_INTERVAL) {
  92         spekFramePosition = 0;
  93     }
  94
  95     if (spekFramePosition < SPEK_FRAME_SIZE) {
  96         spekFrame[spekFramePosition++] = (uint8_t)c;
  97         if (spekFramePosition < SPEK_FRAME_SIZE) {
  98             rcFrameComplete = false;
  99         } else {
 100             rxRuntimeState->lastRcFrameTimeUs = now;
 101             rcFrameComplete = true;
 102         }
 103     }
 104 }
 105
 106 uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
 107
 108 static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
 109 {
 110     UNUSED(rxRuntimeState);
 111
 112 #if defined(USE_TELEMETRY_SRXL)
 113     static timeUs_t telemetryFrameRequestedUs = 0;
 114
 115     timeUs_t currentTimeUs = micros();
 116 #endif
 117
 118     uint8_t result = RX_FRAME_PENDING;
 119
 120     if (rcFrameComplete) {
 121         rcFrameComplete = false;
 122
 123 #if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_VIRTUAL_RSSI)
 124         spektrumHandleRSSI(spekFrame);
 125 #endif
 126
 127         // Get the VTX control bytes in a frame
 128         uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
 129                             (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
 130                             (spekFrame[SPEKTRUM_VTX_CONTROL_3] <<  8) |
 131                             (spekFrame[SPEKTRUM_VTX_CONTROL_4] <<  0) );
 132
 133         int8_t spektrumRcDataSize;
 134         // Handle VTX control frame.
 135         if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
 136             (spekFrame[2] & 0x80) == 0 )  {
 137 #if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
 138             spektrumHandleVtxControl(vtxControl);
 139 #endif
 140             spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE;
 141         } else {
 142             spektrumRcDataSize = SPEK_FRAME_SIZE;
 143         }
 144
 145         // Get the RC control channel inputs
 146         for (int b = 3; b < spektrumRcDataSize; b += 2) {
 147             const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
 148             if (spekChannel < rxRuntimeStatePtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
 149                 if (rssi_channel == 0 || spekChannel != rssi_channel) {
 150                     spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
 151                 }
 152             }
 153         }
 154
 155 #if defined(USE_TELEMETRY_SRXL)
 156         if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
 157                     telemetryFrameRequestedUs = currentTimeUs;
 158         }
 159 #endif
 160         result = RX_FRAME_COMPLETE;
 161     }
 162
 163 #if defined(USE_TELEMETRY_SRXL)
 164     if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
 165         telemetryFrameRequestedUs = 0;
 166
 167         result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
 168     }
 169 #endif
 170
 171     return result;
 172 }
 173
 174 static float spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 175 {
 176     float data;
 177
 178     if (chan >= rxRuntimeState->channelCount) {
 179         return 0;
 180     }
 181
 182     if (spekHiRes) {
 183         data = 0.5f * (float)spekChannelData[chan] + 988; // 2048 mode
 184     } else {
 185         data = spekChannelData[chan] + 988;               // 1024 mode
 186     }
 187
 188     return data;
 189 }
 190
 191 #ifdef USE_SPEKTRUM_BIND
 192
 193 bool spekShouldBind(uint8_t spektrum_sat_bind)
 194 {
 195 #ifdef USE_SPEKTRUM_BIND_PLUG
 196     IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag);
 197
 198     if (BindPlug) {
 199         IOInit(BindPlug, OWNER_RX_BIND, 0);
 200         IOConfigGPIO(BindPlug, IOCFG_IPU);
 201
 202         // Check status of bind plug and exit if not active
 203         delayMicroseconds(10);  // allow configuration to settle
 204         if (IORead(BindPlug)) {
 205             return false;
 206         }
 207     }
 208 #endif // USE_SPEKTRUM_BIND_PLUG
 209
 210     return !(
 211         isMPUSoftReset() ||
 212         spektrum_sat_bind == SPEKTRUM_SAT_BIND_DISABLED ||
 213         spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
 214     );
 215 }
 216
 217 /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
 218  * Function must be called immediately after startup so that we don't miss satellite bind window.
 219  * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
 220  * 9 = DSMX 11ms / DSMX 22ms
 221  * 5 = DSM2 11ms 2048 / DSM2 22ms 1024
 222  */
 223 void spektrumBind(rxConfig_t *rxConfig)
 224 {
 225     if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
 226         return;
 227     }
 228
 229     // Determine a pin to use
 230     ioTag_t bindPin = IO_TAG_NONE;
 231
 232     if (rxConfig->spektrum_bind_pin_override_ioTag) {
 233         bindPin = rxConfig->spektrum_bind_pin_override_ioTag;
 234     } else {
 235         const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 236         if (!portConfig) {
 237             return;
 238         }
 239 #if defined(USE_UART) || defined(USE_LPUART) || defined(USE_SOFTSERIAL)
 240         const int resourceIndex = serialResourceIndex(portConfig->identifier);
 241         const ioTag_t txPin = serialPinConfig()->ioTagTx[resourceIndex];
 242         const ioTag_t rxPin = serialPinConfig()->ioTagRx[resourceIndex];
 243
 244         // Take care half-duplex case
 245         switch (rxRuntimeState.serialrxProvider) {
 246         case SERIALRX_SRXL:
 247 #if defined(USE_TELEMETRY_SRXL)
 248             if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 249                 bindPin = txPin;
 250             }
 251             break;
 252 #endif // USE_TELEMETRY_SRXL
 253
 254         default:
 255             bindPin = rxConfig->halfDuplex ? txPin : rxPin;
 256         }
 257 #endif
 258         if (!bindPin) {
 259             return;
 260         }
 261     }
 262
 263     IO_t bindIO = IOGetByTag(bindPin);
 264
 265     IOInit(bindIO, OWNER_RX_BIND, 0);
 266     IOConfigGPIO(bindIO, IOCFG_OUT_PP);
 267
 268     LED1_ON;
 269
 270     // RX line, set high
 271     IOWrite(bindIO, true);
 272
 273     // Bind window is around 20-140ms after powerup
 274     delay(60);
 275     LED1_OFF;
 276
 277     for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) {
 278         LED0_OFF;
 279         LED2_OFF;
 280         // RX line, drive low for 120us
 281         IOWrite(bindIO, false);
 282         delayMicroseconds(120);
 283
 284         LED0_ON;
 285         LED2_ON;
 286         // RX line, drive high for 120us
 287         IOWrite(bindIO, true);
 288         delayMicroseconds(120);
 289
 290     }
 291
 292     // Release the bind pin to avoid interference with an actual rx pin,
 293     // when rxConfig->spektrum_bind_pin_override_ioTag is used.
 294     // This happens when the bind pin is connected in parallel to the rx pin.
 295
 296     if (rxConfig->spektrum_bind_pin_override_ioTag) {
 297         delay(50); // Keep it high for 50msec
 298         IOConfigGPIO(bindIO, IOCFG_IN_FLOATING);
 299     }
 300
 301     // 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
 302     // Don't reset if hardware bind plug is present
 303     // Reset only when autoreset is enabled
 304
 305     if (!rxConfig->spektrum_bind_plug_ioTag && rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
 306         rxConfig->spektrum_sat_bind = 0;
 307         saveConfigAndNotify();
 308     }
 309 }
 310 #endif // USE_SPEKTRUM_BIND
 311
 312 #if defined(USE_TELEMETRY_SRXL)
 313 static bool spektrumProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 314 {
 315     UNUSED(rxRuntimeState);
 316
 317     // if there is telemetry data to write
 318     if (telemetryBufLen > 0) {
 319         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 320         telemetryBufLen = 0; // reset telemetry buffer
 321     }
 322
 323     return true;
 324 }
 325
 326 bool srxlTelemetryBufferEmpty(void)
 327 {
 328   if (telemetryBufLen == 0) {
 329       return true;
 330   } else {
 331       return false;
 332   }
 333 }
 334
 335 void srxlRxWriteTelemetryData(const void *data, int len)
 336 {
 337     len = MIN(len, (int)sizeof(telemetryBuf));
 338     memcpy(telemetryBuf, data, len);
 339     telemetryBufLen = len;
 340 }
 341 #endif
 342
 343 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 344 {
 345     rxRuntimeStatePtr = rxRuntimeState;
 346
 347     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 348     if (!portConfig) {
 349         return false;
 350     }
 351
 352     srxlEnabled = false;
 353 #if defined(USE_TELEMETRY_SRXL)
 354     bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
 355 #else
 356     bool portShared = false;
 357 #endif
 358
 359     switch (rxRuntimeState->serialrxProvider) {
 360     default:
 361
 362         break;
 363     case SERIALRX_SRXL:
 364 #if defined(USE_TELEMETRY_SRXL)
 365         srxlEnabled = (featureIsEnabled(FEATURE_TELEMETRY) && !portShared);
 366         FALLTHROUGH;
 367 #endif
 368     case SERIALRX_SPEKTRUM2048:
 369         // 11 bit frames
 370         spek_chan_shift = 3;
 371         spek_chan_mask = 0x07;
 372         spekHiRes = true;
 373         resolution = 2048;
 374         rxRuntimeState->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
 375         break;
 376     case SERIALRX_SPEKTRUM1024:
 377         // 10 bit frames
 378         spek_chan_shift = 2;
 379         spek_chan_mask = 0x03;
 380         spekHiRes = false;
 381         resolution = 1024;
 382         rxRuntimeState->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
 383         break;
 384     }
 385
 386     rxRuntimeState->rcReadRawFn = spektrumReadRawRC;
 387     rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus;
 388 #if defined(USE_TELEMETRY_SRXL)
 389     rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame;
 390 #endif
 391
 392     serialPort = openSerialPort(portConfig->identifier,
 393         FUNCTION_RX_SERIAL,
 394         spektrumDataReceive,
 395         rxRuntimeState,
 396         SPEKTRUM_BAUDRATE,
 397         portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
 398         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) |
 399         ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
 400         );
 401
 402 #if defined(USE_TELEMETRY_SRXL)
 403     if (portShared) {
 404         telemetrySharedPort = serialPort;
 405     }
 406 #endif
 407
 408     rssi_channel = rxConfig->rssi_channel - 1; // -1 because rxConfig->rssi_channel is 1-based and rssi_channel is 0-based.
 409     if (rssi_channel >= rxRuntimeState->channelCount) {
 410         rssi_channel = 0;
 411     }
 412
 413     return serialPort != NULL;
 414 }
 415
 416 bool srxlRxIsActive(void)
 417 {
 418     return serialPort != NULL;
 419 }
 420
 421 #endif // SERIAL_RX