src/main/rx/ghst.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 <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_SERIALRX_GHST
  29
  30 #include "build/build_config.h"
  31 #include "build/debug.h"
  32
  33 #include "common/crc.h"
  34 #include "common/maths.h"
  35 #include "common/utils.h"
  36
  37 #include "pg/rx.h"
  38
  39 #include "drivers/serial.h"
  40 #include "drivers/serial_uart.h"
  41 #include "drivers/system.h"
  42 #include "drivers/time.h"
  43
  44 #include "io/serial.h"
  45
  46 #include "rx/rx.h"
  47 #include "rx/ghst.h"
  48
  49 #include "telemetry/ghst.h"
  50
  51 #define GHST_PORT_OPTIONS               (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR | SERIAL_BIDIR_PP)
  52 #define GHST_PORT_MODE                  MODE_RXTX   // bidirectional on single pin
  53
  54 #define GHST_MAX_FRAME_TIME_US          500         // 14 bytes @ 420k = ~450us
  55 #define GHST_TIME_BETWEEN_FRAMES_US     2000        // fastest frame rate = 500Hz, or 2000us
  56
  57 #define GHST_RSSI_DBM_MIN (-117)            // Long Range mode value
  58 #define GHST_RSSI_DBM_MAX (-60)             // Typical RSSI with typical power levels, typical antennas, and a few feet/meters between Tx and Rx
  59
  60 // define the time window after the end of the last received packet where telemetry packets may be sent
  61 // NOTE: This allows the Rx to double-up on Rx packets to transmit data other than servo data, but
  62 // only if sent < 1ms after the servo data packet.
  63 #define GHST_RX_TO_TELEMETRY_MIN_US     1000
  64 #define GHST_RX_TO_TELEMETRY_MAX_US     2000
  65
  66 #define GHST_PAYLOAD_OFFSET offsetof(ghstFrameDef_t, type)
  67
  68 #define GHST_RC_CTR_VAL_12BIT_PRIMARY 2048
  69 #define GHST_RC_CTR_VAL_12BIT_AUX     (128 << 2)
  70
  71 STATIC_UNIT_TESTED volatile bool ghstFrameAvailable = false;
  72 STATIC_UNIT_TESTED volatile bool ghstValidatedFrameAvailable = false;
  73 STATIC_UNIT_TESTED volatile bool ghstTransmittingTelemetry = false;
  74
  75 STATIC_UNIT_TESTED ghstFrame_t ghstFrameBuffer[2];
  76
  77 ghstFrame_t *ghstIncomingFrame  = &ghstFrameBuffer[0];  // incoming frame, raw, not CRC checked, destination address not checked
  78 ghstFrame_t *ghstValidatedFrame = &ghstFrameBuffer[1];  // validated frame, CRC is ok, destination address is ok, ready for decode
  79
  80 STATIC_UNIT_TESTED uint16_t ghstChannelData[GHST_MAX_NUM_CHANNELS];
  81 static ghstRfProtocol_e ghstRfProtocol = GHST_RF_PROTOCOL_UNDEFINED;
  82
  83 enum {
  84     DEBUG_GHST_CRC_ERRORS = 0,
  85     DEBUG_GHST_UNKNOWN_FRAMES,
  86     DEBUG_GHST_RX_RSSI,
  87     DEBUG_GHST_RX_LQ,
  88 };
  89
  90 static serialPort_t *serialPort;
  91 static timeUs_t ghstRxFrameStartAtUs = 0;
  92 static timeUs_t ghstRxFrameEndAtUs = 0;
  93 static uint8_t telemetryBuf[GHST_FRAME_SIZE];
  94 static uint8_t telemetryBufLen = 0;
  95
  96 /* GHST Protocol
  97  * Ghost uses 420k baud single-wire, half duplex connection, connected to a FC UART 'Tx' pin
  98  * Each control packet is interleaved with one or more corresponding downlink packets
  99  *
 100  * Uplink packet format (Control packets)
 101  * <Addr><Len><Type><Payload><CRC>
 102  *
 103  * Addr:        u8      Destination address
 104  * Len          u8      Length includes the packet ID, but not the CRC
 105  * CRC          u8
 106  *
 107  * Ghost packets are designed to be as short as possible, for minimum latency.
 108  *
 109  * Note that the GHST protocol does not handle, itself, failsafe conditions. Packets are passed from
 110  * the Ghost receiver to Betaflight as and when they arrive. Betaflight itself is responsible for
 111  * determining when a failsafe is necessary based on dropped packets.
 112  *
 113  */
 114
 115
 116 // called from telemetry/ghst.c
 117 void ghstRxWriteTelemetryData(const void *const data, const int len)
 118 {
 119     telemetryBufLen = MIN(len, (int)sizeof(telemetryBuf));
 120     memcpy(telemetryBuf, data, telemetryBufLen);
 121 }
 122
 123 void ghstRxSendTelemetryData(void)
 124 {
 125     // if there is telemetry data to write
 126     if (telemetryBufLen > 0) {
 127         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 128         telemetryBufLen = 0; // reset telemetry buffer
 129     }
 130 }
 131
 132 uint8_t ghstRxGetTelemetryBufLen(void)
 133 {
 134     return telemetryBufLen;
 135 }
 136
 137 STATIC_UNIT_TESTED uint8_t ghstFrameCRC(const ghstFrame_t *const pGhstFrame)
 138 {
 139     // CRC includes type and payload
 140     uint8_t crc = crc8_dvb_s2(0, pGhstFrame->frame.type);
 141     for (int i = 0; i < pGhstFrame->frame.len - GHST_FRAME_LENGTH_TYPE - GHST_FRAME_LENGTH_CRC; ++i) {
 142         crc = crc8_dvb_s2(crc, pGhstFrame->frame.payload[i]);
 143     }
 144     return crc;
 145 }
 146
 147 static void rxSwapFrameBuffers(void)
 148 {
 149     ghstFrame_t *const tmp = ghstIncomingFrame;
 150     ghstIncomingFrame = ghstValidatedFrame;
 151     ghstValidatedFrame = tmp;
 152 }
 153
 154 // Receive ISR callback, called back from serial port
 155 STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
 156 {
 157     UNUSED(data);
 158
 159     static uint8_t ghstFrameIdx = 0;
 160     const timeUs_t currentTimeUs = microsISR();
 161
 162     if (cmpTimeUs(currentTimeUs, ghstRxFrameStartAtUs) > GHST_MAX_FRAME_TIME_US) {
 163         // Character received after the max. frame time, assume that this is a new frame
 164         ghstFrameIdx = 0;
 165     }
 166
 167     if (ghstFrameIdx == 0) {
 168         // timestamp the start of the frame, to allow us to detect frame sync issues
 169         ghstRxFrameStartAtUs = currentTimeUs;
 170     }
 171
 172     // assume frame is 5 bytes long until we have received the frame length
 173     // full frame length includes the length of the address and framelength fields
 174     const int fullFrameLength = ghstFrameIdx < 3 ? 5 : ghstIncomingFrame->frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
 175
 176     if (ghstFrameIdx < fullFrameLength && ghstFrameIdx < sizeof(ghstFrame_t)) {
 177         ghstIncomingFrame->bytes[ghstFrameIdx++] = (uint8_t)c;
 178         if (ghstFrameIdx >= fullFrameLength) {
 179             ghstFrameIdx = 0;
 180
 181             // NOTE: this data is not yet CRC checked, nor do we know whether we are the correct recipient, this is
 182             // handled in ghstFrameStatus
 183
 184             // Not CRC checked but we are interested just in frame for us
 185             // eg. telemetry frames are read back here also, skip them
 186             if (ghstIncomingFrame->frame.addr == GHST_ADDR_FC) {
 187
 188                 rxSwapFrameBuffers();
 189                 ghstFrameAvailable = true;
 190
 191                 // remember what time the incoming (Rx) packet ended, so that we can ensure a quite bus before sending telemetry
 192                 ghstRxFrameEndAtUs = microsISR();
 193             }
 194         }
 195     }
 196 }
 197
 198 static bool shouldSendTelemetryFrame(void)
 199 {
 200     const timeUs_t now = micros();
 201     const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
 202     return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US;
 203 }
 204
 205 STATIC_UNIT_TESTED uint8_t ghstFrameStatus(rxRuntimeState_t *rxRuntimeState)
 206 {
 207     UNUSED(rxRuntimeState);
 208     static int16_t crcErrorCount = 0;
 209     uint8_t status = RX_FRAME_PENDING;
 210
 211     if (ghstFrameAvailable) {
 212         ghstFrameAvailable = false;
 213
 214         const uint8_t crc = ghstFrameCRC(ghstValidatedFrame);
 215         const int fullFrameLength = ghstValidatedFrame->frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
 216         if (crc == ghstValidatedFrame->bytes[fullFrameLength - 1]) {
 217             ghstValidatedFrameAvailable = true;
 218             rxRuntimeState->lastRcFrameTimeUs = ghstRxFrameEndAtUs;
 219             status = RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED;      // request callback through ghstProcessFrame to do the decoding work
 220         } else {
 221             DEBUG_SET(DEBUG_GHST, DEBUG_GHST_CRC_ERRORS, ++crcErrorCount);
 222             status = RX_FRAME_DROPPED;                            // frame was invalid
 223         }
 224     } else if (checkGhstTelemetryState() && shouldSendTelemetryFrame()) {
 225         status = RX_FRAME_PROCESSING_REQUIRED;
 226     }
 227
 228     return status;
 229 }
 230
 231 static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 232 {
 233     // Assume that the only way we get here is if ghstFrameStatus returned RX_FRAME_PROCESSING_REQUIRED, which indicates that the CRC
 234     // is correct, and the message was actually for us.
 235
 236     UNUSED(rxRuntimeState);
 237     static int16_t unknownFrameCount = 0;
 238
 239     // do we have a telemetry buffer to send?
 240     if (checkGhstTelemetryState() && shouldSendTelemetryFrame()) {
 241         ghstTransmittingTelemetry = true;
 242         ghstRxSendTelemetryData();
 243     }
 244
 245     if (ghstValidatedFrameAvailable) {
 246         ghstValidatedFrameAvailable = false;
 247
 248         const uint8_t ghstFrameType = ghstValidatedFrame->frame.type;
 249         const bool scalingLegacy = ghstFrameType >= GHST_UL_RC_CHANS_HS4_FIRST && ghstFrameType <= GHST_UL_RC_CHANS_HS4_LAST;
 250         const bool scaling12bit = ghstFrameType >= GHST_UL_RC_CHANS_HS4_12_FIRST && ghstFrameType <= GHST_UL_RC_CHANS_HS4_12_LAST;
 251
 252         if ( scaling12bit || scalingLegacy ) {
 253
 254             int startIdx = 0;
 255
 256             switch (ghstFrameType) {
 257                 case GHST_UL_RC_CHANS_HS4_RSSI:
 258                 case GHST_UL_RC_CHANS_HS4_12_RSSI: {
 259                     const ghstPayloadPulsesRssi_t* const rssiFrame = (ghstPayloadPulsesRssi_t*)&ghstValidatedFrame->frame.payload;
 260
 261                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_RSSI, -rssiFrame->rssi);
 262                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_LQ, rssiFrame->lq);
 263
 264                     ghstRfProtocol = rssiFrame->rfProtocol;
 265                     // Enable telemetry just for modes that support it
 266                     setGhstTelemetryState(ghstRfProtocol == GHST_RF_PROTOCOL_NORMAL
 267                                        || ghstRfProtocol == GHST_RF_PROTOCOL_RACE
 268                                        || ghstRfProtocol == GHST_RF_PROTOCOL_LONGRANGE
 269                                        || ghstRfProtocol == GHST_RF_PROTOCOL_RACE250);
 270
 271                     if (rssiSource == RSSI_SOURCE_RX_PROTOCOL) {
 272                         // rssi sent sign-inverted
 273                         uint16_t rssiPercentScaled = scaleRange(-rssiFrame->rssi, GHST_RSSI_DBM_MIN, GHST_RSSI_DBM_MAX, 0, RSSI_MAX_VALUE);
 274                         rssiPercentScaled = MIN(rssiPercentScaled, RSSI_MAX_VALUE);
 275                         setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL);
 276                     }
 277
 278 #ifdef USE_RX_RSSI_DBM
 279                     setRssiDbm(-rssiFrame->rssi, RSSI_SOURCE_RX_PROTOCOL);
 280 #endif
 281
 282 #ifdef USE_RX_LINK_QUALITY_INFO
 283                     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) {
 284                         setLinkQualityDirect(rssiFrame->lq);
 285                     }
 286 #endif
 287                     break;
 288                 }
 289
 290                 case GHST_UL_RC_CHANS_HS4_5TO8:
 291                 case GHST_UL_RC_CHANS_HS4_12_5TO8:
 292                     startIdx = 4;
 293                     break;
 294
 295                 case GHST_UL_RC_CHANS_HS4_9TO12:
 296                 case GHST_UL_RC_CHANS_HS4_12_9TO12:
 297                     startIdx = 8;
 298                     break;
 299
 300                 case GHST_UL_RC_CHANS_HS4_13TO16:
 301                 case GHST_UL_RC_CHANS_HS4_12_13TO16:
 302                     startIdx = 12;
 303                     break;
 304
 305                 default:
 306                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount);
 307                     break;
 308             }
 309
 310             // We need to wait for the first RSSI frame to know ghstRfProtocol
 311             if (ghstRfProtocol != GHST_RF_PROTOCOL_UNDEFINED) {
 312                 const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame->frame.payload;
 313
 314                 // all uplink frames contain CH1..4 data (12 bit)
 315                 ghstChannelData[0] = rcChannels->ch1to4.ch1;
 316                 ghstChannelData[1] = rcChannels->ch1to4.ch2;
 317                 ghstChannelData[2] = rcChannels->ch1to4.ch3;
 318                 ghstChannelData[3] = rcChannels->ch1to4.ch4;
 319
 320                 if (startIdx > 0) {
 321                     // remainder of uplink frame contains 4 more channels (8 bit), sent in a round-robin fashion
 322                     ghstChannelData[startIdx++] = rcChannels->cha << 2;
 323                     ghstChannelData[startIdx++] = rcChannels->chb << 2;
 324                     ghstChannelData[startIdx++] = rcChannels->chc << 2;
 325                     ghstChannelData[startIdx++] = rcChannels->chd << 2;
 326                 }
 327
 328                 // Recalculate old scaling to the new one
 329                 if (scalingLegacy) {
 330                     for (int i = 0; i < 4; i++) {
 331                         ghstChannelData[i] = ((5 * ghstChannelData[i]) >> 2) - 430;  // Primary
 332                         if (startIdx > 4) {
 333                             --startIdx;
 334                             ghstChannelData[startIdx] = 5 * (ghstChannelData[startIdx] >> 2) - 108; // Aux
 335                         }
 336                     }
 337                 }
 338
 339             }
 340         } else {
 341             switch(ghstFrameType) {
 342
 343 #if defined(USE_TELEMETRY_GHST) && defined(USE_MSP_OVER_TELEMETRY)
 344             case GHST_UL_MSP_REQ:
 345             case GHST_UL_MSP_WRITE: {
 346                 static uint8_t mspFrameCounter = 0;
 347                 DEBUG_SET(DEBUG_GHST_MSP, 0, ++mspFrameCounter);
 348                 if (handleMspFrame(ghstValidatedFrame->frame.payload, ghstValidatedFrame->frame.len - GHST_FRAME_LENGTH_CRC - GHST_FRAME_LENGTH_TYPE, NULL)) {
 349                     ghstScheduleMspResponse();
 350                 }
 351                 break;
 352             }
 353 #endif
 354             default:
 355                 DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount);
 356                 break;
 357             }
 358         }
 359     }
 360
 361     return true;
 362 }
 363
 364 STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 365 {
 366     UNUSED(rxRuntimeState);
 367
 368     // Scaling 12bit channels (8bit channels in brackets)
 369     //      OpenTx          RC   PWM (BF)
 370     // min  -1024        0(  0)     988us
 371     // ctr      0     2048(128)    1500us
 372     // max   1024     4096(256)    2012us
 373     //
 374
 375     // Scaling legacy (nearly 10bit)
 376     // derived from original SBus scaling, with slight correction for offset
 377     // now symmetrical around OpenTx 0 value
 378     // scaling is:
 379     //      OpenTx         RC   PWM (BF)
 380     // min  -1024     172( 22)     988us
 381     // ctr      0     992(124)    1500us
 382     // max   1024    1811(226)    2012us
 383     //
 384
 385     float pwm = ghstChannelData[chan];
 386
 387     if (chan < 4) {
 388         pwm = 0.25f * pwm;
 389     }
 390
 391     return pwm + 988;
 392 }
 393
 394 // UART idle detected (inter-packet)
 395 static void ghstIdle(void)
 396 {
 397     if (ghstTransmittingTelemetry) {
 398         ghstTransmittingTelemetry = false;
 399     }
 400 }
 401
 402 bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 403 {
 404     rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS;
 405     rxRuntimeState->rcReadRawFn = ghstReadRawRC;
 406     rxRuntimeState->rcFrameStatusFn = ghstFrameStatus;
 407     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 408     rxRuntimeState->rcProcessFrameFn = ghstProcessFrame;
 409
 410     for (int iChan = 0; iChan < rxRuntimeState->channelCount; ++iChan) {
 411         if (iChan < 4 ) {
 412             ghstChannelData[iChan] = GHST_RC_CTR_VAL_12BIT_PRIMARY;
 413         } else {
 414             ghstChannelData[iChan] = GHST_RC_CTR_VAL_12BIT_AUX;
 415         }
 416     }
 417
 418     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 419     if (!portConfig) {
 420         return false;
 421     }
 422
 423     serialPort = openSerialPort(portConfig->identifier,
 424         FUNCTION_RX_SERIAL,
 425         ghstDataReceive,
 426         NULL,
 427         GHST_RX_BAUDRATE,
 428         GHST_PORT_MODE,
 429         GHST_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
 430         );
 431     serialPort->idleCallback = ghstIdle;
 432
 433     if (rssiSource == RSSI_SOURCE_NONE) {
 434         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 435     }
 436
 437 #ifdef USE_RX_LINK_QUALITY_INFO
 438     if (linkQualitySource == LQ_SOURCE_NONE) {
 439         linkQualitySource = LQ_SOURCE_RX_PROTOCOL_GHST;
 440     }
 441 #endif
 442
 443     return serialPort != NULL;
 444 }
 445
 446 bool ghstRxIsActive(void)
 447 {
 448     return serialPort != NULL;
 449 }
 450 #endif