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
 107 uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
 108
 109 static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
 110 {
 111     UNUSED(rxRuntimeState);
 112
 113 #if defined(USE_TELEMETRY_SRXL)
 114     static timeUs_t telemetryFrameRequestedUs = 0;
 115
 116     timeUs_t currentTimeUs = micros();
 117 #endif
 118
 119     uint8_t result = RX_FRAME_PENDING;
 120
 121     if (rcFrameComplete) {
 122         rcFrameComplete = false;
 123
 124 #if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
 125         spektrumHandleRSSI(spekFrame);
 126 #endif
 127
 128         // Get the VTX control bytes in a frame
 129         uint32_t vtxControl = ((spekFrame[SPEKTRUM_VTX_CONTROL_1] << 24) |
 130                             (spekFrame[SPEKTRUM_VTX_CONTROL_2] << 16) |
 131                             (spekFrame[SPEKTRUM_VTX_CONTROL_3] <<  8) |
 132                             (spekFrame[SPEKTRUM_VTX_CONTROL_4] <<  0) );
 133
 134         int8_t spektrumRcDataSize;
 135         // Handle VTX control frame.
 136         if ((vtxControl & SPEKTRUM_VTX_CONTROL_FRAME_MASK) == SPEKTRUM_VTX_CONTROL_FRAME &&
 137             (spekFrame[2] & 0x80) == 0 )  {
 138 #if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
 139             spektrumHandleVtxControl(vtxControl);
 140 #endif
 141             spektrumRcDataSize = SPEK_FRAME_SIZE - SPEKTRUM_VTX_CONTROL_SIZE;
 142         } else {
 143             spektrumRcDataSize = SPEK_FRAME_SIZE;
 144         }
 145
 146         // Get the RC control channel inputs
 147         for (int b = 3; b < spektrumRcDataSize; b += 2) {
 148             const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
 149             if (spekChannel < rxRuntimeStatePtr->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 #if defined(USE_TELEMETRY_SRXL)
 157         if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
 158                     telemetryFrameRequestedUs = currentTimeUs;
 159         }
 160 #endif
 161         result = RX_FRAME_COMPLETE;
 162     }
 163
 164 #if defined(USE_TELEMETRY_SRXL)
 165     if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
 166         telemetryFrameRequestedUs = 0;
 167
 168         result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
 169     }
 170 #endif
 171
 172     return result;
 173 }
 174
 175 static float spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 176 {
 177     float data;
 178
 179     if (chan >= rxRuntimeState->channelCount) {
 180         return 0;
 181     }
 182
 183     if (spekHiRes) {
 184         data = 0.5f * (float)spekChannelData[chan] + 988; // 2048 mode
 185     } else {
 186         data = spekChannelData[chan] + 988;               // 1024 mode
 187     }
 188
 189     return data;
 190 }
 191
 192 #ifdef USE_SPEKTRUM_BIND
 193
 194 bool spekShouldBind(uint8_t spektrum_sat_bind)
 195 {
 196 #ifdef USE_SPEKTRUM_BIND_PLUG
 197     IO_t BindPlug = IOGetByTag(rxConfig()->spektrum_bind_plug_ioTag);
 198
 199     if (BindPlug) {
 200         IOInit(BindPlug, OWNER_RX_BIND, 0);
 201         IOConfigGPIO(BindPlug, IOCFG_IPU);
 202
 203         // Check status of bind plug and exit if not active
 204         delayMicroseconds(10);  // allow configuration to settle
 205         if (IORead(BindPlug)) {
 206             return false;
 207         }
 208     }
 209 #endif // USE_SPEKTRUM_BIND_PLUG
 210
 211     return !(
 212         isMPUSoftReset() ||
 213         spektrum_sat_bind == SPEKTRUM_SAT_BIND_DISABLED ||
 214         spektrum_sat_bind > SPEKTRUM_SAT_BIND_MAX
 215     );
 216 }
 217
 218 /* spektrumBind function ported from Baseflight. It's used to bind satellite receiver to TX.
 219  * Function must be called immediately after startup so that we don't miss satellite bind window.
 220  * Known parameters. Tested with DSMX satellite and DX8 radio. Framerate (11ms or 22ms) must be selected from TX.
 221  * 9 = DSMX 11ms / DSMX 22ms
 222  * 5 = DSM2 11ms 2048 / DSM2 22ms 1024
 223  */
 224 void spektrumBind(rxConfig_t *rxConfig)
 225 {
 226     if (!spekShouldBind(rxConfig->spektrum_sat_bind)) {
 227         return;
 228     }
 229
 230     // Determine a pin to use
 231     ioTag_t bindPin = IO_TAG_NONE;
 232
 233     if (rxConfig->spektrum_bind_pin_override_ioTag) {
 234         bindPin = rxConfig->spektrum_bind_pin_override_ioTag;
 235     } else {
 236         const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 237         if (!portConfig) {
 238             return;
 239         }
 240
 241         int index = SERIAL_PORT_IDENTIFIER_TO_INDEX(portConfig->identifier);
 242         ioTag_t txPin = serialPinConfig()->ioTagTx[index];
 243         ioTag_t rxPin = serialPinConfig()->ioTagRx[index];
 244
 245         // Take care half-duplex case
 246         switch (rxRuntimeState.serialrxProvider) {
 247         case SERIALRX_SRXL:
 248 #if defined(USE_TELEMETRY_SRXL)
 249             if (featureIsEnabled(FEATURE_TELEMETRY) && !telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 250                 bindPin = txPin;
 251             }
 252             break;
 253 #endif // USE_TELEMETRY_SRXL
 254
 255         default:
 256             bindPin = rxConfig->halfDuplex ? txPin : rxPin;
 257         }
 258
 259         if (!bindPin) {
 260             return;
 261         }
 262     }
 263
 264     IO_t bindIO = IOGetByTag(bindPin);
 265
 266     IOInit(bindIO, OWNER_RX_BIND, 0);
 267     IOConfigGPIO(bindIO, IOCFG_OUT_PP);
 268
 269     LED1_ON;
 270
 271     // RX line, set high
 272     IOWrite(bindIO, true);
 273
 274     // Bind window is around 20-140ms after powerup
 275     delay(60);
 276     LED1_OFF;
 277
 278     for (int i = 0; i < rxConfig->spektrum_sat_bind; i++) {
 279         LED0_OFF;
 280         LED2_OFF;
 281         // RX line, drive low for 120us
 282         IOWrite(bindIO, false);
 283         delayMicroseconds(120);
 284
 285         LED0_ON;
 286         LED2_ON;
 287         // RX line, drive high for 120us
 288         IOWrite(bindIO, true);
 289         delayMicroseconds(120);
 290
 291     }
 292
 293
 294     // Release the bind pin to avoid interference with an actual rx pin,
 295     // when rxConfig->spektrum_bind_pin_override_ioTag is used.
 296     // This happens when the bind pin is connected in parallel to the rx pin.
 297
 298     if (rxConfig->spektrum_bind_pin_override_ioTag) {
 299         delay(50); // Keep it high for 50msec
 300         IOConfigGPIO(bindIO, IOCFG_IN_FLOATING);
 301     }
 302
 303     // 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
 304     // Don't reset if hardware bind plug is present
 305     // Reset only when autoreset is enabled
 306
 307     if (!rxConfig->spektrum_bind_plug_ioTag && rxConfig->spektrum_sat_bind_autoreset == 1 && !isMPUSoftReset()) {
 308         rxConfig->spektrum_sat_bind = 0;
 309         saveConfigAndNotify();
 310     }
 311 }
 312 #endif // USE_SPEKTRUM_BIND
 313
 314 #if defined(USE_TELEMETRY_SRXL)
 315 static bool spektrumProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 316 {
 317     UNUSED(rxRuntimeState);
 318
 319     // if there is telemetry data to write
 320     if (telemetryBufLen > 0) {
 321         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 322         telemetryBufLen = 0; // reset telemetry buffer
 323     }
 324
 325     return true;
 326 }
 327
 328 bool srxlTelemetryBufferEmpty()
 329 {
 330   if (telemetryBufLen == 0) {
 331       return true;
 332   } else {
 333       return false;
 334   }
 335 }
 336
 337 void srxlRxWriteTelemetryData(const void *data, int len)
 338 {
 339     len = MIN(len, (int)sizeof(telemetryBuf));
 340     memcpy(telemetryBuf, data, len);
 341     telemetryBufLen = len;
 342 }
 343 #endif
 344
 345 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 346 {
 347     rxRuntimeStatePtr = rxRuntimeState;
 348
 349     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 350     if (!portConfig) {
 351         return false;
 352     }
 353
 354     srxlEnabled = false;
 355 #if defined(USE_TELEMETRY_SRXL)
 356     bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
 357 #else
 358     bool portShared = false;
 359 #endif
 360
 361     switch (rxRuntimeState->serialrxProvider) {
 362     default:
 363
 364         break;
 365     case SERIALRX_SRXL:
 366 #if defined(USE_TELEMETRY_SRXL)
 367         srxlEnabled = (featureIsEnabled(FEATURE_TELEMETRY) && !portShared);
 368         FALLTHROUGH;
 369 #endif
 370     case SERIALRX_SPEKTRUM2048:
 371         // 11 bit frames
 372         spek_chan_shift = 3;
 373         spek_chan_mask = 0x07;
 374         spekHiRes = true;
 375         resolution = 2048;
 376         rxRuntimeState->channelCount = SPEKTRUM_2048_CHANNEL_COUNT;
 377         rxRuntimeState->rxRefreshRate = 11000;
 378         break;
 379     case SERIALRX_SPEKTRUM1024:
 380         // 10 bit frames
 381         spek_chan_shift = 2;
 382         spek_chan_mask = 0x03;
 383         spekHiRes = false;
 384         resolution = 1024;
 385         rxRuntimeState->channelCount = SPEKTRUM_1024_CHANNEL_COUNT;
 386         rxRuntimeState->rxRefreshRate = 22000;
 387         break;
 388     }
 389
 390     rxRuntimeState->rcReadRawFn = spektrumReadRawRC;
 391     rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus;
 392     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 393 #if defined(USE_TELEMETRY_SRXL)
 394     rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame;
 395 #endif
 396
 397     serialPort = openSerialPort(portConfig->identifier,
 398         FUNCTION_RX_SERIAL,
 399         spektrumDataReceive,
 400         rxRuntimeState,
 401         SPEKTRUM_BAUDRATE,
 402         portShared || srxlEnabled ? MODE_RXTX : MODE_RX,
 403         (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) |
 404         ((srxlEnabled || rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
 405         );
 406
 407 #if defined(USE_TELEMETRY_SRXL)
 408     if (portShared) {
 409         telemetrySharedPort = serialPort;
 410     }
 411 #endif
 412
 413     rssi_channel = rxConfig->rssi_channel - 1; // -1 because rxConfig->rssi_channel is 1-based and rssi_channel is 0-based.
 414     if (rssi_channel >= rxRuntimeState->channelCount) {
 415         rssi_channel = 0;
 416     }
 417
 418     return serialPort != NULL;
 419 }
 420
 421 bool srxlRxIsActive(void)
 422 {
 423     return serialPort != NULL;
 424 }
 425
 426 #endif // SERIAL_RX