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