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     4500        // fastest frame rate = 222.22Hz, or 4500us
  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 STATIC_UNIT_TESTED volatile bool ghstFrameAvailable = false;
  69 STATIC_UNIT_TESTED volatile bool ghstValidatedFrameAvailable = false;
  70 STATIC_UNIT_TESTED volatile bool ghstTransmittingTelemetry = false;
  71
  72 STATIC_UNIT_TESTED ghstFrame_t ghstIncomingFrame;   // incoming frame, raw, not CRC checked, destination address not checked
  73 STATIC_UNIT_TESTED ghstFrame_t ghstValidatedFrame;  // validated frame, CRC is ok, destination address is ok, ready for decode
  74
  75 STATIC_UNIT_TESTED uint32_t ghstChannelData[GHST_MAX_NUM_CHANNELS];
  76
  77 enum {
  78     DEBUG_GHST_CRC_ERRORS = 0,
  79     DEBUG_GHST_UNKNOWN_FRAMES,
  80     DEBUG_GHST_RX_RSSI,
  81     DEBUG_GHST_RX_LQ,
  82 };
  83
  84 static serialPort_t *serialPort;
  85 static timeUs_t ghstRxFrameStartAtUs = 0;
  86 static timeUs_t ghstRxFrameEndAtUs = 0;
  87 static uint8_t telemetryBuf[GHST_FRAME_SIZE_MAX];
  88 static uint8_t telemetryBufLen = 0;
  89
  90 /* GHST Protocol
  91  * Ghost uses 420k baud single-wire, half duplex connection, connected to a FC UART 'Tx' pin
  92  * Each control packet is interleaved with one or more corresponding downlink packets
  93  *
  94  * Uplink packet format (Control packets)
  95  * <Addr><Len><Type><Payload><CRC>
  96  *
  97  * Addr:        u8      Destination address
  98  * Len          u8      Length includes the packet ID, but not the CRC
  99  * CRC          u8
 100  *
 101  * Ghost packets are designed to be as short as possible, for minimum latency.
 102  *
 103  * Note that the GHST protocol does not handle, itself, failsafe conditions. Packets are passed from
 104  * the Ghost receiver to Betaflight as and when they arrive. Betaflight itself is responsible for
 105  * determining when a failsafe is necessary based on dropped packets.
 106  *
 107   */
 108
 109 #define GHST_FRAME_LENGTH_ADDRESS       1
 110 #define GHST_FRAME_LENGTH_FRAMELENGTH   1
 111 #define GHST_FRAME_LENGTH_TYPE_CRC      1
 112
 113 // called from telemetry/ghst.c
 114 void ghstRxWriteTelemetryData(const void *data, int len)
 115 {
 116     len = MIN(len, (int)sizeof(telemetryBuf));
 117     memcpy(telemetryBuf, data, len);
 118     telemetryBufLen = len;
 119 }
 120
 121 void ghstRxSendTelemetryData(void)
 122 {
 123     // if there is telemetry data to write
 124     if (telemetryBufLen > 0) {
 125         serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
 126         telemetryBufLen = 0; // reset telemetry buffer
 127     }
 128 }
 129
 130 STATIC_UNIT_TESTED uint8_t ghstFrameCRC(ghstFrame_t *pGhstFrame)
 131 {
 132     // CRC includes type and payload
 133     uint8_t crc = crc8_dvb_s2(0, pGhstFrame->frame.type);
 134     for (int i = 0; i < pGhstFrame->frame.len - GHST_FRAME_LENGTH_TYPE_CRC - 1; ++i) {
 135         crc = crc8_dvb_s2(crc, pGhstFrame->frame.payload[i]);
 136     }
 137     return crc;
 138 }
 139
 140 // Receive ISR callback, called back from serial port
 141 STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data)
 142 {
 143     UNUSED(data);
 144
 145     static uint8_t ghstFrameIdx = 0;
 146     const timeUs_t currentTimeUs = microsISR();
 147
 148     if (cmpTimeUs(currentTimeUs, ghstRxFrameStartAtUs) > GHST_MAX_FRAME_TIME_US) {
 149         // Character received after the max. frame time, assume that this is a new frame
 150         ghstFrameIdx = 0;
 151     }
 152
 153     if (ghstFrameIdx == 0) {
 154         // timestamp the start of the frame, to allow us to detect frame sync issues
 155         ghstRxFrameStartAtUs = currentTimeUs;
 156     }
 157
 158     // assume frame is 5 bytes long until we have received the frame length
 159     // full frame length includes the length of the address and framelength fields
 160     const int fullFrameLength = ghstFrameIdx < 3 ? 5 : ghstIncomingFrame.frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
 161
 162     if (ghstFrameIdx < fullFrameLength) {
 163         ghstIncomingFrame.bytes[ghstFrameIdx++] = (uint8_t)c;
 164         if (ghstFrameIdx >= fullFrameLength) {
 165             ghstFrameIdx = 0;
 166
 167             // NOTE: this data is not yet CRC checked, nor do we know whether we are the correct recipient, this is
 168             // handled in ghstFrameStatus
 169             memcpy(&ghstValidatedFrame, &ghstIncomingFrame, sizeof(ghstIncomingFrame));
 170             ghstFrameAvailable = true;
 171
 172             // remember what time the incoming (Rx) packet ended, so that we can ensure a quite bus before sending telemetry
 173             ghstRxFrameEndAtUs = microsISR();
 174         }
 175     }
 176 }
 177
 178 static bool shouldSendTelemetryFrame(void)
 179 {
 180     const timeUs_t now = micros();
 181     const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs);
 182     return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US;
 183 }
 184
 185 STATIC_UNIT_TESTED uint8_t ghstFrameStatus(rxRuntimeState_t *rxRuntimeState)
 186 {
 187     UNUSED(rxRuntimeState);
 188     static int16_t crcErrorCount = 0;
 189
 190     if (ghstFrameAvailable) {
 191         ghstFrameAvailable = false;
 192
 193         const uint8_t crc = ghstFrameCRC(&ghstValidatedFrame);
 194         const int fullFrameLength = ghstValidatedFrame.frame.len + GHST_FRAME_LENGTH_ADDRESS + GHST_FRAME_LENGTH_FRAMELENGTH;
 195         if (crc == ghstValidatedFrame.bytes[fullFrameLength - 1] && ghstValidatedFrame.frame.addr == GHST_ADDR_FC) {
 196             ghstValidatedFrameAvailable = true;
 197             return RX_FRAME_COMPLETE | RX_FRAME_PROCESSING_REQUIRED;            // request callback through ghstProcessFrame to do the decoding  work
 198         }
 199
 200         if (crc != ghstValidatedFrame.bytes[fullFrameLength - 1]) {
 201             DEBUG_SET(DEBUG_GHST, DEBUG_GHST_CRC_ERRORS, ++crcErrorCount);
 202         }
 203
 204         return RX_FRAME_DROPPED;                            // frame was invalid
 205     }
 206
 207     if (shouldSendTelemetryFrame()) {
 208         return RX_FRAME_PROCESSING_REQUIRED;
 209     }
 210
 211     return RX_FRAME_PENDING;
 212 }
 213
 214 static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 215 {
 216     // Assume that the only way we get here is if ghstFrameStatus returned RX_FRAME_PROCESSING_REQUIRED, which indicates that the CRC
 217     // is correct, and the message was actually for us.
 218
 219     UNUSED(rxRuntimeState);
 220
 221     static int16_t unknownFrameCount = 0;
 222
 223     // do we have a telemetry buffer to send?
 224     if (shouldSendTelemetryFrame()) {
 225         ghstTransmittingTelemetry = true;
 226         ghstRxSendTelemetryData();
 227     }
 228
 229     if (ghstValidatedFrameAvailable) {
 230         int startIdx = 0;
 231
 232         if (ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST &&
 233             ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST) {
 234             const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload;
 235
 236             // all uplink frames contain CH1..4 data (12 bit)
 237             ghstChannelData[0] = rcChannels->ch1to4.ch1 >> 1;
 238             ghstChannelData[1] = rcChannels->ch1to4.ch2 >> 1;
 239             ghstChannelData[2] = rcChannels->ch1to4.ch3 >> 1;
 240             ghstChannelData[3] = rcChannels->ch1to4.ch4 >> 1;
 241
 242             switch(ghstValidatedFrame.frame.type) {
 243                 case GHST_UL_RC_CHANS_HS4_RSSI: {
 244                     const ghstPayloadPulsesRssi_t* const rssiFrame = (ghstPayloadPulsesRssi_t*)&ghstValidatedFrame.frame.payload;
 245
 246                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_RSSI, -rssiFrame->rssi);
 247                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_RX_LQ, rssiFrame->lq);
 248
 249                     if (rssiSource == RSSI_SOURCE_RX_PROTOCOL) {
 250                         // rssi sent sign-inverted
 251                         const uint16_t rssiPercentScaled = scaleRange(-rssiFrame->rssi, GHST_RSSI_DBM_MIN, 0, GHST_RSSI_DBM_MAX, RSSI_MAX_VALUE);
 252                         setRssi(rssiPercentScaled, RSSI_SOURCE_RX_PROTOCOL);
 253                     }
 254
 255 #ifdef USE_RX_RSSI_DBM
 256                     setRssiDbm(-rssiFrame->rssi, RSSI_SOURCE_RX_PROTOCOL);
 257 #endif
 258
 259 #ifdef USE_RX_LINK_QUALITY_INFO
 260                     if (linkQualitySource == LQ_SOURCE_RX_PROTOCOL_GHST) {
 261                         setLinkQualityDirect(rssiFrame->lq);
 262                     }
 263 #endif
 264                     break;
 265                 }
 266
 267                 case GHST_UL_RC_CHANS_HS4_5TO8:     startIdx = 4;  break;
 268                 case GHST_UL_RC_CHANS_HS4_9TO12:    startIdx = 8;  break;
 269                 case GHST_UL_RC_CHANS_HS4_13TO16:   startIdx = 12; break;
 270                 default:
 271                     DEBUG_SET(DEBUG_GHST, DEBUG_GHST_UNKNOWN_FRAMES, ++unknownFrameCount);
 272                     break;
 273             }
 274
 275             if (startIdx > 0)
 276             {
 277                 // remainder of uplink frame contains 4 more channels (8 bit), sent in a round-robin fashion
 278
 279                 ghstChannelData[startIdx++] = rcChannels->cha << 3;
 280                 ghstChannelData[startIdx++] = rcChannels->chb << 3;
 281                 ghstChannelData[startIdx++] = rcChannels->chc << 3;
 282                 ghstChannelData[startIdx++] = rcChannels->chd << 3;
 283             }
 284         }
 285     }
 286
 287     return true;
 288 }
 289
 290 STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
 291 {
 292     UNUSED(rxRuntimeState);
 293
 294     // derived from original SBus scaling, with slight correction for offset (now symmetrical
 295     // around OpenTx 0 value)
 296     // scaling is:
 297     //      OpenTx   RC     PWM
 298     // min  -1024    172    988us
 299     // ctr  0        992    1500us
 300     // max  1024     1811   2012us
 301     //
 302
 303     return (5 * ((float)ghstChannelData[chan] + 1) / 8) + 880;
 304 }
 305
 306 // UART idle detected (inter-packet)
 307 static void ghstIdle()
 308 {
 309     if (ghstTransmittingTelemetry) {
 310         ghstTransmittingTelemetry = false;
 311     }
 312 }
 313
 314 bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 315 {
 316     for (int iChan = 0; iChan < GHST_MAX_NUM_CHANNELS; ++iChan) {
 317         ghstChannelData[iChan] = (16 * rxConfig->midrc) / 10 - 1408;
 318     }
 319
 320     rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS;
 321     rxRuntimeState->rxRefreshRate = GHST_TIME_BETWEEN_FRAMES_US;
 322
 323     rxRuntimeState->rcReadRawFn = ghstReadRawRC;
 324     rxRuntimeState->rcFrameStatusFn = ghstFrameStatus;
 325     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 326     rxRuntimeState->rcProcessFrameFn = ghstProcessFrame;
 327
 328     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 329     if (!portConfig) {
 330         return false;
 331     }
 332
 333     serialPort = openSerialPort(portConfig->identifier,
 334         FUNCTION_RX_SERIAL,
 335         ghstDataReceive,
 336         NULL,
 337         GHST_RX_BAUDRATE,
 338         GHST_PORT_MODE,
 339         GHST_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0)
 340         );
 341     serialPort->idleCallback = ghstIdle;
 342
 343     if (rssiSource == RSSI_SOURCE_NONE) {
 344         rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 345     }
 346
 347 #ifdef USE_RX_LINK_QUALITY_INFO
 348     if (linkQualitySource == LQ_SOURCE_NONE) {
 349         linkQualitySource = LQ_SOURCE_RX_PROTOCOL_GHST;
 350     }
 351 #endif
 352
 353     return serialPort != NULL;
 354 }
 355
 356 bool ghstRxIsActive(void)
 357 {
 358     return serialPort != NULL;
 359 }
 360 #endif