src/main/rx/fport.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_FPORT
  29
  30 #include "build/debug.h"
  31
  32 #include "common/maths.h"
  33 #include "common/utils.h"
  34
  35 #include "drivers/time.h"
  36
  37 #include "io/serial.h"
  38
  39 #ifdef USE_TELEMETRY
  40 #include "telemetry/telemetry.h"
  41 #include "telemetry/smartport.h"
  42 #endif
  43
  44 #include "pg/rx.h"
  45
  46 #include "rx/frsky_crc.h"
  47 #include "rx/rx.h"
  48 #include "rx/sbus_channels.h"
  49 #include "rx/fport.h"
  50
  51
  52 #define FPORT_TIME_NEEDED_PER_FRAME_US 3000
  53 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
  54 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
  55 #define FPORT_MAX_TELEMETRY_AGE_MS 500
  56
  57 #define FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES 2
  58
  59
  60 #define FPORT_FRAME_MARKER 0x7E
  61
  62 #define FPORT_ESCAPE_CHAR 0x7D
  63 #define FPORT_ESCAPE_MASK 0x20
  64
  65 #define FPORT_BAUDRATE 115200
  66
  67 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
  68
  69 enum {
  70     DEBUG_FPORT_FRAME_INTERVAL = 0,
  71     DEBUG_FPORT_FRAME_ERRORS,
  72     DEBUG_FPORT_FRAME_LAST_ERROR,
  73     DEBUG_FPORT_TELEMETRY_INTERVAL,
  74 };
  75
  76 enum {
  77     DEBUG_FPORT_NO_ERROR = 0,
  78     DEBUG_FPORT_ERROR_TIMEOUT,
  79     DEBUG_FPORT_ERROR_OVERSIZE,
  80     DEBUG_FPORT_ERROR_SIZE,
  81     DEBUG_FPORT_ERROR_CHECKSUM,
  82     DEBUG_FPORT_ERROR_TYPE,
  83     DEBUG_FPORT_ERROR_TYPE_SIZE,
  84 };
  85
  86 enum {
  87     FPORT_FRAME_TYPE_CONTROL = 0x00,
  88     FPORT_FRAME_TYPE_TELEMETRY_REQUEST = 0x01,
  89     FPORT_FRAME_TYPE_TELEMETRY_RESPONSE = 0x81,
  90
  91 };
  92
  93 enum {
  94     FPORT_FRAME_ID_NULL = 0x00,     // (master/slave)
  95     FPORT_FRAME_ID_DATA = 0x10,     // (master/slave)
  96     FPORT_FRAME_ID_READ = 0x30,     // (master)
  97     FPORT_FRAME_ID_WRITE = 0x31,    // (master)
  98     FPORT_FRAME_ID_RESPONSE = 0x32, // (slave)
  99 };
 100
 101 typedef struct fportControlData_s {
 102     sbusChannels_t channels;
 103     uint8_t rssi;
 104 } fportControlData_t;
 105
 106 typedef union fportData_s {
 107     fportControlData_t controlData;
 108     smartPortPayload_t telemetryData;
 109 } fportData_t;
 110
 111 typedef struct fportFrame_s {
 112     uint8_t type;
 113     fportData_t data;
 114 } fportFrame_t;
 115
 116 #ifdef USE_TELEMETRY_SMARTPORT
 117 static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
 118 #endif
 119
 120 #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
 121 #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
 122
 123 #define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
 124 #define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
 125
 126 #define NUM_RX_BUFFERS 3
 127 #define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
 128
 129 typedef struct fportBuffer_s {
 130     uint8_t data[BUFFER_SIZE];
 131     uint8_t length;
 132     timeUs_t frameStartTimeUs;
 133 } fportBuffer_t;
 134
 135 static fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
 136
 137 static volatile uint8_t rxBufferWriteIndex = 0;
 138 static volatile uint8_t rxBufferReadIndex = 0;
 139
 140 static volatile timeUs_t lastTelemetryFrameReceivedUs;
 141 static volatile bool clearToSend = false;
 142
 143 static volatile uint8_t framePosition = 0;
 144
 145 static smartPortPayload_t *mspPayload = NULL;
 146 static timeUs_t lastRcFrameReceivedMs = 0;
 147
 148 static serialPort_t *fportPort;
 149 #ifdef USE_TELEMETRY_SMARTPORT
 150 static bool telemetryEnabled = false;
 151 #endif
 152
 153 static void reportFrameError(uint8_t errorReason) {
 154     static volatile uint16_t frameErrors = 0;
 155
 156     frameErrors++;
 157
 158     DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, frameErrors);
 159     DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason);
 160 }
 161
 162 // Receive ISR callback
 163 static void fportDataReceive(uint16_t c, void *data)
 164 {
 165     UNUSED(data);
 166
 167     static timeUs_t frameStartAt = 0;
 168     static bool escapedCharacter = false;
 169     static timeUs_t lastFrameReceivedUs = 0;
 170     static bool telemetryFrame = false;
 171
 172     const timeUs_t currentTimeUs = microsISR();
 173
 174     clearToSend = false;
 175
 176     if (framePosition > 1 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
 177         reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
 178
 179         framePosition = 0;
 180      }
 181
 182     uint8_t val = (uint8_t)c;
 183
 184     if (val == FPORT_FRAME_MARKER) {
 185         if (framePosition > 1) {
 186             const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
 187             if (nextWriteIndex != rxBufferReadIndex) {
 188                 rxBuffer[rxBufferWriteIndex].length = framePosition - 1;
 189                 rxBufferWriteIndex = nextWriteIndex;
 190             }
 191
 192             if (telemetryFrame) {
 193                 clearToSend = true;
 194                 lastTelemetryFrameReceivedUs = currentTimeUs;
 195                 telemetryFrame = false;
 196             }
 197
 198             DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
 199             lastFrameReceivedUs = currentTimeUs;
 200
 201             escapedCharacter = false;
 202         }
 203
 204         frameStartAt = currentTimeUs;
 205         framePosition = 1;
 206
 207         rxBuffer[rxBufferWriteIndex].frameStartTimeUs = currentTimeUs;
 208     } else if (framePosition > 0) {
 209         if (framePosition >= BUFFER_SIZE + 1) {
 210                 framePosition = 0;
 211
 212                 reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
 213         } else {
 214             if (escapedCharacter) {
 215                 val = val ^ FPORT_ESCAPE_MASK;
 216                 escapedCharacter = false;
 217             } else if (val == FPORT_ESCAPE_CHAR) {
 218                 escapedCharacter = true;
 219
 220                 return;
 221             }
 222
 223             if (framePosition == 2 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
 224                 telemetryFrame = true;
 225             }
 226
 227             rxBuffer[rxBufferWriteIndex].data[framePosition - 1] = val;
 228             framePosition = framePosition + 1;
 229         }
 230     }
 231 }
 232
 233 #if defined(USE_TELEMETRY_SMARTPORT)
 234 static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
 235 {
 236     framePosition = 0;
 237
 238     uint16_t checksum = 0;
 239     smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort);
 240     smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE, &checksum, fportPort);
 241     smartPortWriteFrameSerial(payload, fportPort, checksum);
 242 }
 243 #endif
 244
 245 static uint8_t fportFrameStatus(rxRuntimeState_t *rxRuntimeState)
 246 {
 247     static bool hasTelemetryRequest = false;
 248
 249 #ifdef USE_TELEMETRY_SMARTPORT
 250     static smartPortPayload_t payloadBuffer;
 251     static bool rxDrivenFrameRate = false;
 252     static uint8_t consecutiveTelemetryFrameCount = 0;
 253 #endif
 254
 255     uint8_t result = RX_FRAME_PENDING;
 256
 257
 258     if (rxBufferReadIndex != rxBufferWriteIndex) {
 259         uint8_t bufferLength = rxBuffer[rxBufferReadIndex].length;
 260         uint8_t frameLength = rxBuffer[rxBufferReadIndex].data[0];
 261         if (frameLength != bufferLength - 2) {
 262             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
 263         } else {
 264             if (!frskyCheckSumIsGood(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
 265                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
 266             } else {
 267                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
 268
 269                 switch (frame->type) {
 270                 case FPORT_FRAME_TYPE_CONTROL:
 271                     if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
 272                         reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
 273                     } else {
 274                         result = sbusChannelsDecode(rxRuntimeState, &frame->data.controlData.channels);
 275
 276                         setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
 277
 278                         lastRcFrameReceivedMs = millis();
 279
 280                         if (!(result & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED))) {
 281                             rxRuntimeState->lastRcFrameTimeUs = rxBuffer[rxBufferReadIndex].frameStartTimeUs;
 282                         }
 283                     }
 284
 285                     break;
 286                 case FPORT_FRAME_TYPE_TELEMETRY_REQUEST:
 287                     if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST) {
 288                         reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
 289                     } else {
 290 #if defined(USE_TELEMETRY_SMARTPORT)
 291                         if (!telemetryEnabled) {
 292                             break;
 293                         }
 294
 295                         switch(frame->data.telemetryData.frameId) {
 296                         case FPORT_FRAME_ID_DATA:
 297                             if (!rxDrivenFrameRate) {
 298                                 rxDrivenFrameRate = true;
 299                             }
 300
 301                             hasTelemetryRequest = true;
 302
 303                             break;
 304                         case FPORT_FRAME_ID_NULL:
 305                             if (!rxDrivenFrameRate) {
 306                                 if (consecutiveTelemetryFrameCount >= FPORT_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES && !(mspPayload && smartPortPayloadContainsMSP(mspPayload))) {
 307                                     consecutiveTelemetryFrameCount = 0;
 308                                 } else {
 309                                     hasTelemetryRequest = true;
 310
 311                                     consecutiveTelemetryFrameCount++;
 312                                 }
 313                             }
 314
 315                             break;
 316                         case FPORT_FRAME_ID_READ:
 317                         case FPORT_FRAME_ID_WRITE: // never used
 318                             memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
 319                             mspPayload = &payloadBuffer;
 320
 321                             break;
 322                         default:
 323
 324                            break;
 325                         }
 326 #endif
 327                     }
 328
 329                     break;
 330                 default:
 331                     reportFrameError(DEBUG_FPORT_ERROR_TYPE);
 332
 333                     break;
 334                 }
 335             }
 336
 337         }
 338
 339         rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
 340     }
 341
 342     if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
 343         hasTelemetryRequest = false;
 344
 345         result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
 346     }
 347
 348     if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
 349         setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
 350         lastRcFrameReceivedMs = 0;
 351     }
 352
 353     return result;
 354 }
 355
 356 static bool fportProcessFrame(const rxRuntimeState_t *rxRuntimeState)
 357 {
 358     UNUSED(rxRuntimeState);
 359
 360 #if defined(USE_TELEMETRY_SMARTPORT)
 361     static timeUs_t lastTelemetryFrameSentUs;
 362
 363     timeUs_t currentTimeUs = micros();
 364     if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
 365        clearToSend = false;
 366     }
 367
 368     if (clearToSend) {
 369         processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
 370
 371         if (clearToSend) {
 372             smartPortWriteFrameFport(&emptySmartPortFrame);
 373
 374             clearToSend = false;
 375         }
 376
 377         DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_INTERVAL, currentTimeUs - lastTelemetryFrameSentUs);
 378         lastTelemetryFrameSentUs = currentTimeUs;
 379     }
 380
 381     mspPayload = NULL;
 382 #endif
 383
 384     return true;
 385 }
 386
 387 bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 388 {
 389     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
 390     rxRuntimeState->channelData = sbusChannelData;
 391     sbusChannelsInit(rxConfig, rxRuntimeState);
 392
 393     rxRuntimeState->channelCount = SBUS_MAX_CHANNEL;
 394     rxRuntimeState->rxRefreshRate = 11000;
 395
 396     rxRuntimeState->rcFrameStatusFn = fportFrameStatus;
 397     rxRuntimeState->rcProcessFrameFn = fportProcessFrame;
 398     rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
 399
 400     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 401     if (!portConfig) {
 402         return false;
 403     }
 404
 405     fportPort = openSerialPort(portConfig->identifier,
 406         FUNCTION_RX_SERIAL,
 407         fportDataReceive,
 408         NULL,
 409         FPORT_BAUDRATE,
 410         MODE_RXTX,
 411         FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 412     );
 413
 414     if (fportPort) {
 415 #if defined(USE_TELEMETRY_SMARTPORT)
 416         telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
 417 #endif
 418
 419         if (rssiSource == RSSI_SOURCE_NONE) {
 420             rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 421         }
 422     }
 423
 424     return fportPort != NULL;
 425 }
 426 #endif