src/main/rx/fport.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #if defined(USE_SERIAL_RX)
  26
  27 #include "build/debug.h"
  28
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "drivers/time.h"
  33
  34 #include "io/serial.h"
  35
  36 #ifdef USE_TELEMETRY
  37 #include "telemetry/telemetry.h"
  38 #include "telemetry/smartport.h"
  39 #endif
  40
  41 #include "rx/rx.h"
  42 #include "rx/sbus_channels.h"
  43 #include "rx/fport.h"
  44
  45
  46 #define FPORT_TIME_NEEDED_PER_FRAME_US 3000
  47 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000
  48 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
  49 #define FPORT_MAX_TELEMETRY_AGE_MS 500
  50
  51
  52 #define FPORT_FRAME_MARKER 0x7E
  53
  54 #define FPORT_ESCAPE_CHAR 0x7D
  55 #define FPORT_ESCAPE_MASK 0x20
  56
  57 #define FPORT_CRC_VALUE 0xFF
  58
  59 #define FPORT_BAUDRATE 115200
  60
  61 #define FPORT_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
  62
  63 enum {
  64     DEBUG_FPORT_FRAME_INTERVAL = 0,
  65     DEBUG_FPORT_FRAME_ERRORS,
  66     DEBUG_FPORT_FRAME_LAST_ERROR,
  67     DEBUG_FPORT_TELEMETRY_DELAY,
  68 };
  69
  70 enum {
  71     DEBUG_FPORT_NO_ERROR = 0,
  72     DEBUG_FPORT_ERROR_TIMEOUT,
  73     DEBUG_FPORT_ERROR_OVERSIZE,
  74     DEBUG_FPORT_ERROR_SIZE,
  75     DEBUG_FPORT_ERROR_CHECKSUM,
  76     DEBUG_FPORT_ERROR_TYPE,
  77     DEBUG_FPORT_ERROR_TYPE_SIZE,
  78 };
  79
  80 enum {
  81     FPORT_FRAME_TYPE_CONTROL = 0x00,
  82     FPORT_FRAME_TYPE_TELEMETRY_REQUEST = 0x01,
  83     FPORT_FRAME_TYPE_TELEMETRY_RESPONSE = 0x81,
  84
  85 };
  86
  87 enum {
  88     FPORT_FRAME_ID_NULL = 0x00,     // (master/slave)
  89     FPORT_FRAME_ID_DATA = 0x10,     // (master/slave)
  90     FPORT_FRAME_ID_READ = 0x30,     // (master)
  91     FPORT_FRAME_ID_WRITE = 0x31,    // (master)
  92     FPORT_FRAME_ID_RESPONSE = 0x32, // (slave)
  93 };
  94
  95 typedef struct fportControlData_s {
  96     sbusChannels_t channels;
  97     uint8_t rssi;
  98 } fportControlData_t;
  99
 100 typedef union fportData_s {
 101     fportControlData_t controlData;
 102     smartPortPayload_t telemetryData;
 103 } fportData_t;
 104
 105 typedef struct fportFrame_s {
 106     uint8_t type;
 107     fportData_t data;
 108 } fportFrame_t;
 109
 110 static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
 111
 112 #define FPORT_REQUEST_FRAME_LENGTH sizeof(fportFrame_t)
 113 #define FPORT_RESPONSE_FRAME_LENGTH (sizeof(uint8_t) + sizeof(smartPortPayload_t))
 114
 115 #define FPORT_FRAME_PAYLOAD_LENGTH_CONTROL (sizeof(uint8_t) + sizeof(fportControlData_t))
 116 #define FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST (sizeof(uint8_t) + sizeof(smartPortPayload_t))
 117
 118 #define NUM_RX_BUFFERS 3
 119 #define BUFFER_SIZE (FPORT_REQUEST_FRAME_LENGTH + 2 * sizeof(uint8_t))
 120
 121 typedef struct fportBuffer_s {
 122     uint8_t data[BUFFER_SIZE];
 123     uint8_t length;
 124 } fportBuffer_t;
 125
 126 static fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
 127
 128 static volatile uint8_t rxBufferWriteIndex = 0;
 129 static volatile uint8_t rxBufferReadIndex = 0;
 130
 131 static volatile timeUs_t lastTelemetryFrameReceivedUs;
 132 static volatile bool clearToSend = false;
 133
 134 static volatile uint8_t framePosition = 0;
 135
 136 static smartPortPayload_t *mspPayload = NULL;
 137 static timeUs_t lastRcFrameReceivedMs = 0;
 138
 139 static serialPort_t *fportPort;
 140 static bool telemetryEnabled = false;
 141
 142 static void reportFrameError(uint8_t errorReason) {
 143     static volatile uint16_t frameErrors = 0;
 144
 145     DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors);
 146     DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason);
 147 }
 148
 149 // Receive ISR callback
 150 static void fportDataReceive(uint16_t c, void *data)
 151 {
 152     UNUSED(data);
 153
 154     static timeUs_t frameStartAt = 0;
 155     static bool escapedCharacter = false;
 156     static timeUs_t lastFrameReceivedUs = 0;
 157     static bool telemetryFrame = false;
 158
 159     const timeUs_t currentTimeUs = micros();
 160
 161     clearToSend = false;
 162
 163     if (framePosition > 1 && cmpTimeUs(currentTimeUs, frameStartAt) > FPORT_TIME_NEEDED_PER_FRAME_US + 500) {
 164         reportFrameError(DEBUG_FPORT_ERROR_TIMEOUT);
 165
 166         framePosition = 0;
 167      }
 168
 169     uint8_t val = (uint8_t)c;
 170
 171     if (val == FPORT_FRAME_MARKER) {
 172         if (framePosition > 1) {
 173             const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
 174             if (nextWriteIndex != rxBufferReadIndex) {
 175                 rxBuffer[rxBufferWriteIndex].length = framePosition - 1;
 176                 rxBufferWriteIndex = nextWriteIndex;
 177             }
 178
 179             if (telemetryFrame) {
 180                 clearToSend = true;
 181                 lastTelemetryFrameReceivedUs = currentTimeUs;
 182                 telemetryFrame = false;
 183             }
 184
 185             DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_INTERVAL, currentTimeUs - lastFrameReceivedUs);
 186             lastFrameReceivedUs = currentTimeUs;
 187
 188             escapedCharacter = false;
 189         }
 190
 191         frameStartAt = currentTimeUs;
 192         framePosition = 1;
 193     } else if (framePosition > 0) {
 194         if (framePosition >= BUFFER_SIZE + 1) {
 195                 framePosition = 0;
 196
 197                 reportFrameError(DEBUG_FPORT_ERROR_OVERSIZE);
 198         } else {
 199             if (escapedCharacter) {
 200                 val = val ^ FPORT_ESCAPE_MASK;
 201                 escapedCharacter = false;
 202             } else if (val == FPORT_ESCAPE_CHAR) {
 203                 escapedCharacter = true;
 204
 205                 return;
 206             }
 207
 208             if (framePosition == 2 && val == FPORT_FRAME_TYPE_TELEMETRY_REQUEST) {
 209                 telemetryFrame = true;
 210             }
 211
 212             rxBuffer[rxBufferWriteIndex].data[framePosition - 1] = val;
 213             framePosition = framePosition + 1;
 214         }
 215     }
 216 }
 217
 218 #if defined(USE_TELEMETRY_SMARTPORT)
 219 static void smartPortWriteFrameFport(const smartPortPayload_t *payload)
 220 {
 221     framePosition = 0;
 222
 223     uint16_t checksum = 0;
 224     smartPortSendByte(FPORT_RESPONSE_FRAME_LENGTH, &checksum, fportPort);
 225     smartPortSendByte(FPORT_FRAME_TYPE_TELEMETRY_RESPONSE, &checksum, fportPort);
 226     smartPortWriteFrameSerial(payload, fportPort, checksum);
 227 }
 228 #endif
 229
 230 static bool checkChecksum(uint8_t *data, uint8_t length)
 231 {
 232     uint16_t checksum = 0;
 233     for (unsigned i = 0; i < length; i++) {
 234         checksum = checksum + *(uint8_t *)(data + i);
 235     }
 236
 237     checksum = (checksum & 0xff) + (checksum >> 8);
 238
 239     return checksum == FPORT_CRC_VALUE;
 240 }
 241
 242 static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 243 {
 244     static smartPortPayload_t payloadBuffer;
 245     static bool hasTelemetryRequest = false;
 246
 247     uint8_t result = RX_FRAME_PENDING;
 248
 249     if (rxBufferReadIndex != rxBufferWriteIndex) {
 250         uint8_t bufferLength = rxBuffer[rxBufferReadIndex].length;
 251         uint8_t frameLength = rxBuffer[rxBufferReadIndex].data[0];
 252         if (frameLength != bufferLength - 2) {
 253             reportFrameError(DEBUG_FPORT_ERROR_SIZE);
 254         } else {
 255             if (!checkChecksum(&rxBuffer[rxBufferReadIndex].data[0], bufferLength)) {
 256                 reportFrameError(DEBUG_FPORT_ERROR_CHECKSUM);
 257             } else {
 258                 fportFrame_t *frame = (fportFrame_t *)&rxBuffer[rxBufferReadIndex].data[1];
 259
 260                 switch (frame->type) {
 261                 case FPORT_FRAME_TYPE_CONTROL:
 262                     if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
 263                         reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
 264                     } else {
 265                         result |= sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
 266
 267                         setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
 268
 269                         lastRcFrameReceivedMs = millis();
 270                     }
 271
 272                     break;
 273                 case FPORT_FRAME_TYPE_TELEMETRY_REQUEST:
 274                     if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_TELEMETRY_REQUEST) {
 275                         reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
 276                     } else {
 277 #if defined(USE_TELEMETRY_SMARTPORT)
 278                         if (!telemetryEnabled) {
 279                             break;
 280                         }
 281
 282                         switch(frame->data.telemetryData.frameId) {
 283                         case FPORT_FRAME_ID_NULL:
 284                         case FPORT_FRAME_ID_DATA: // never used
 285                             hasTelemetryRequest = true;
 286
 287                             break;
 288                         case FPORT_FRAME_ID_READ:
 289                         case FPORT_FRAME_ID_WRITE: // never used
 290                             memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
 291                             mspPayload = &payloadBuffer;
 292
 293                             break;
 294                         default:
 295
 296                            break;
 297                         }
 298 #endif
 299                     }
 300
 301                     break;
 302                 default:
 303                     reportFrameError(DEBUG_FPORT_ERROR_TYPE);
 304
 305                     break;
 306                 }
 307             }
 308
 309         }
 310
 311         rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
 312     }
 313
 314     if ((mspPayload || hasTelemetryRequest) && cmpTimeUs(micros(), lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
 315         hasTelemetryRequest = false;
 316
 317         result = result | RX_FRAME_PROCESSING_REQUIRED;
 318     }
 319
 320     if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
 321         setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL);
 322         lastRcFrameReceivedMs = 0;
 323     }
 324
 325     return result;
 326 }
 327
 328 static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 329 {
 330     UNUSED(rxRuntimeConfig);
 331
 332 #if defined(USE_TELEMETRY_SMARTPORT)
 333     timeUs_t currentTimeUs = micros();
 334     if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
 335        clearToSend = false;
 336     }
 337
 338     if (clearToSend) {
 339         DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
 340
 341         processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
 342
 343         if (clearToSend) {
 344             smartPortWriteFrameFport(&emptySmartPortFrame);
 345
 346             clearToSend = false;
 347         }
 348     }
 349
 350     mspPayload = NULL;
 351 #endif
 352
 353     return true;
 354 }
 355
 356 bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
 357 {
 358     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
 359     rxRuntimeConfig->channelData = sbusChannelData;
 360     sbusChannelsInit(rxConfig, rxRuntimeConfig);
 361
 362     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
 363     rxRuntimeConfig->rxRefreshRate = 11000;
 364
 365     rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
 366     rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame;
 367
 368     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 369     if (!portConfig) {
 370         return false;
 371     }
 372
 373     fportPort = openSerialPort(portConfig->identifier,
 374         FUNCTION_RX_SERIAL,
 375         fportDataReceive,
 376         NULL,
 377         FPORT_BAUDRATE,
 378         MODE_RXTX,
 379         FPORT_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
 380     );
 381
 382     if (fportPort) {
 383 #if defined(USE_TELEMETRY_SMARTPORT)
 384         telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport);
 385 #endif
 386
 387         if (rssiSource == RSSI_SOURCE_NONE) {
 388             rssiSource = RSSI_SOURCE_RX_PROTOCOL;
 389         }
 390     }
 391
 392     return fportPort != NULL;
 393 }
 394 #endif