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