src/main/telemetry/crsf.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 <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_TELEMETRY_CRSF
  28
  29 #include "build/atomic.h"
  30 #include "build/build_config.h"
  31 #include "build/version.h"
  32
  33 #include "cms/cms.h"
  34
  35 #include "config/feature.h"
  36
  37 #include "config/config.h"
  38 #include "common/crc.h"
  39 #include "common/maths.h"
  40 #include "common/printf.h"
  41 #include "common/streambuf.h"
  42 #include "common/utils.h"
  43
  44 #include "drivers/nvic.h"
  45
  46 #include "fc/rc_modes.h"
  47 #include "fc/runtime_config.h"
  48
  49 #include "flight/imu.h"
  50 #include "flight/position.h"
  51
  52 #include "io/displayport_crsf.h"
  53 #include "io/gps.h"
  54 #include "io/serial.h"
  55
  56 #include "pg/pg.h"
  57 #include "pg/pg_ids.h"
  58
  59 #include "rx/crsf.h"
  60 #include "rx/crsf_protocol.h"
  61
  62 #include "sensors/battery.h"
  63 #include "sensors/sensors.h"
  64
  65 #include "telemetry/telemetry.h"
  66 #include "telemetry/msp_shared.h"
  67
  68 #include "crsf.h"
  69
  70
  71 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
  72 #define CRSF_DEVICEINFO_VERSION             0x01
  73 #define CRSF_DEVICEINFO_PARAMETER_COUNT     0
  74
  75 #define CRSF_MSP_BUFFER_SIZE 96
  76 #define CRSF_MSP_LENGTH_OFFSET 1
  77
  78 static bool crsfTelemetryEnabled;
  79 static bool deviceInfoReplyPending;
  80 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
  81
  82 #if defined(USE_MSP_OVER_TELEMETRY)
  83 typedef struct mspBuffer_s {
  84     uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
  85     int len;
  86 } mspBuffer_t;
  87
  88 static mspBuffer_t mspRxBuffer;
  89
  90 #if defined(USE_CRSF_V3)
  91 static bool isCrsfV3Running = false;
  92 typedef struct {
  93     uint8_t hasPendingReply:1;
  94     uint8_t isNewSpeedValid:1;
  95     uint8_t portID:3;
  96     uint8_t index;
  97     uint32_t confirmationTime;
  98 } crsfSpeedControl_s;
  99
 100 static crsfSpeedControl_s crsfSpeed = {0};
 101
 102 bool checkCrsfCustomizedSpeed(void)
 103 {
 104     return crsfSpeed.index < BAUD_COUNT ? true : false;
 105 }
 106
 107 uint32_t getCrsfDesiredSpeed(void)
 108 {
 109     return checkCrsfCustomizedSpeed() ? baudRates[crsfSpeed.index] : CRSF_BAUDRATE;
 110 }
 111
 112 void setCrsfDefaultSpeed(void)
 113 {
 114     crsfSpeed.hasPendingReply = false;
 115     crsfSpeed.isNewSpeedValid = false;
 116     crsfSpeed.confirmationTime = 0;
 117     crsfSpeed.index = BAUD_COUNT;
 118     isCrsfV3Running = false;
 119     crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
 120 }
 121 #endif
 122
 123 void initCrsfMspBuffer(void)
 124 {
 125     mspRxBuffer.len = 0;
 126 }
 127
 128 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
 129 {
 130     if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
 131         return false;
 132     } else {
 133         uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
 134         *p++ = frameLength;
 135         memcpy(p, frameStart, frameLength);
 136         mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
 137         return true;
 138     }
 139 }
 140
 141 bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
 142 {
 143     static bool replyPending = false;
 144     if (replyPending) {
 145         replyPending = sendMspReply(payloadSize, responseFn);
 146         return replyPending;
 147     }
 148     if (!mspRxBuffer.len) {
 149         return false;
 150     }
 151     int pos = 0;
 152     while (true) {
 153         const int mspFrameLength = mspRxBuffer.bytes[pos];
 154         if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
 155             replyPending |= sendMspReply(payloadSize, responseFn);
 156         }
 157         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
 158         ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
 159             if (pos >= mspRxBuffer.len) {
 160                 mspRxBuffer.len = 0;
 161                 return replyPending;
 162             }
 163         }
 164     }
 165     return replyPending;
 166 }
 167 #endif
 168
 169 static void crsfInitializeFrame(sbuf_t *dst)
 170 {
 171     dst->ptr = crsfFrame;
 172     dst->end = ARRAYEND(crsfFrame);
 173
 174     sbufWriteU8(dst, CRSF_SYNC_BYTE);
 175 }
 176
 177 static void crsfFinalize(sbuf_t *dst)
 178 {
 179     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 180     sbufSwitchToReader(dst, crsfFrame);
 181     // write the telemetry frame to the receiver.
 182     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 183 }
 184
 185 /*
 186 CRSF frame has the structure:
 187 <Device address> <Frame length> <Type> <Payload> <CRC>
 188 Device address: (uint8_t)
 189 Frame length:   length in  bytes including Type (uint8_t)
 190 Type:           (uint8_t)
 191 CRC:            (uint8_t), crc of <Type> and <Payload>
 192 */
 193
 194 /*
 195 0x02 GPS
 196 Payload:
 197 int32_t     Latitude ( degree / 10`000`000 )
 198 int32_t     Longitude (degree / 10`000`000 )
 199 uint16_t    Groundspeed ( km/h / 10 )
 200 uint16_t    GPS heading ( degree / 100 )
 201 uint16      Altitude ( meter 1000m offset )
 202 uint8_t     Satellites in use ( counter )
 203 */
 204 void crsfFrameGps(sbuf_t *dst)
 205 {
 206     // use sbufWrite since CRC does not include frame length
 207     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 208     sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
 209     sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
 210     sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
 211     sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
 212     sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
 213     const uint16_t altitude = (constrain(getEstimatedAltitudeCm(), 0 * 100, 5000 * 100) / 100) + 1000; // constrain altitude from 0 to 5,000m
 214     sbufWriteU16BigEndian(dst, altitude);
 215     sbufWriteU8(dst, gpsSol.numSat);
 216 }
 217
 218 /*
 219 0x08 Battery sensor
 220 Payload:
 221 uint16_t    Voltage ( mV * 100 )
 222 uint16_t    Current ( mA * 100 )
 223 uint24_t    Fuel ( drawn mAh )
 224 uint8_t     Battery remaining ( percent )
 225 */
 226 void crsfFrameBatterySensor(sbuf_t *dst)
 227 {
 228     // use sbufWrite since CRC does not include frame length
 229     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 230     sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
 231     if (telemetryConfig()->report_cell_voltage) {
 232         sbufWriteU16BigEndian(dst, (getBatteryAverageCellVoltage() + 5) / 10); // vbat is in units of 0.01V
 233     } else {
 234         sbufWriteU16BigEndian(dst, getLegacyBatteryVoltage());
 235     }
 236     sbufWriteU16BigEndian(dst, getAmperage() / 10);
 237     const uint32_t mAhDrawn = getMAhDrawn();
 238     const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
 239     sbufWriteU8(dst, (mAhDrawn >> 16));
 240     sbufWriteU8(dst, (mAhDrawn >> 8));
 241     sbufWriteU8(dst, (uint8_t)mAhDrawn);
 242     sbufWriteU8(dst, batteryRemainingPercentage);
 243 }
 244
 245 typedef enum {
 246     CRSF_ACTIVE_ANTENNA1 = 0,
 247     CRSF_ACTIVE_ANTENNA2 = 1
 248 } crsfActiveAntenna_e;
 249
 250 typedef enum {
 251     CRSF_RF_MODE_4_HZ = 0,
 252     CRSF_RF_MODE_50_HZ = 1,
 253     CRSF_RF_MODE_150_HZ = 2
 254 } crsrRfMode_e;
 255
 256 typedef enum {
 257     CRSF_RF_POWER_0_mW = 0,
 258     CRSF_RF_POWER_10_mW = 1,
 259     CRSF_RF_POWER_25_mW = 2,
 260     CRSF_RF_POWER_100_mW = 3,
 261     CRSF_RF_POWER_500_mW = 4,
 262     CRSF_RF_POWER_1000_mW = 5,
 263     CRSF_RF_POWER_2000_mW = 6,
 264     CRSF_RF_POWER_250_mW = 7,
 265     CRSF_RF_POWER_50_mW = 8
 266 } crsrRfPower_e;
 267
 268 /*
 269 0x1E Attitude
 270 Payload:
 271 int16_t     Pitch angle ( rad / 10000 )
 272 int16_t     Roll angle ( rad / 10000 )
 273 int16_t     Yaw angle ( rad / 10000 )
 274 */
 275
 276 // convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
 277 static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
 278 {
 279     while (angle_decidegree > 1800) {
 280         angle_decidegree -= 3600;
 281     }
 282     while (angle_decidegree < -1800) {
 283         angle_decidegree += 3600;
 284     }
 285     return (int16_t)(RAD * 1000.0f * angle_decidegree);
 286 }
 287
 288 // fill dst buffer with crsf-attitude telemetry frame
 289 void crsfFrameAttitude(sbuf_t *dst)
 290 {
 291      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 292      sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
 293      sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.pitch));
 294      sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.roll));
 295      sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.yaw));
 296 }
 297
 298 /*
 299 0x21 Flight mode text based
 300 Payload:
 301 char[]      Flight mode ( Null terminated string )
 302 */
 303 void crsfFrameFlightMode(sbuf_t *dst)
 304 {
 305     // write zero for frame length, since we don't know it yet
 306     uint8_t *lengthPtr = sbufPtr(dst);
 307     sbufWriteU8(dst, 0);
 308     sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 309
 310     // Acro is the default mode
 311     const char *flightMode = "ACRO";
 312
 313     // Modes that are only relevant when disarmed
 314     if (!ARMING_FLAG(ARMED) && isArmingDisabled()) {
 315         flightMode = "!ERR";
 316     } else
 317
 318 #if defined(USE_GPS)
 319     if (!ARMING_FLAG(ARMED) && featureIsEnabled(FEATURE_GPS) && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
 320         flightMode = "WAIT"; // Waiting for GPS lock
 321     } else
 322 #endif
 323
 324     // Flight modes in decreasing order of importance
 325     if (FLIGHT_MODE(FAILSAFE_MODE)) {
 326         flightMode = "!FS!";
 327     } else if (FLIGHT_MODE(GPS_RESCUE_MODE)) {
 328         flightMode = "RTH";
 329     } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
 330         flightMode = "MANU";
 331     } else if (FLIGHT_MODE(ANGLE_MODE)) {
 332         flightMode = "STAB";
 333     } else if (FLIGHT_MODE(HORIZON_MODE)) {
 334         flightMode = "HOR";
 335     } else if (airmodeIsEnabled()) {
 336         flightMode = "AIR";
 337     }
 338
 339     sbufWriteString(dst, flightMode);
 340     if (!ARMING_FLAG(ARMED)) {
 341         sbufWriteU8(dst, '*');
 342     }
 343     sbufWriteU8(dst, '\0');     // zero-terminate string
 344     // write in the frame length
 345     *lengthPtr = sbufPtr(dst) - lengthPtr;
 346 }
 347
 348 /*
 349 0x29 Device Info
 350 Payload:
 351 uint8_t     Destination
 352 uint8_t     Origin
 353 char[]      Device Name ( Null terminated string )
 354 uint32_t    Null Bytes
 355 uint32_t    Null Bytes
 356 uint32_t    Null Bytes
 357 uint8_t     255 (Max MSP Parameter)
 358 uint8_t     0x01 (Parameter version 1)
 359 */
 360 void crsfFrameDeviceInfo(sbuf_t *dst)
 361 {
 362     char buff[30];
 363     tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
 364
 365     uint8_t *lengthPtr = sbufPtr(dst);
 366     sbufWriteU8(dst, 0);
 367     sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
 368     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 369     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 370     sbufWriteStringWithZeroTerminator(dst, buff);
 371     for (unsigned int ii = 0; ii < 12; ii++) {
 372         sbufWriteU8(dst, 0x00);
 373     }
 374     sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
 375     sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
 376     *lengthPtr = sbufPtr(dst) - lengthPtr;
 377 }
 378
 379
 380 #if defined(USE_CRSF_V3)
 381 void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
 382 {
 383     uint8_t *lengthPtr = sbufPtr(dst);
 384     sbufWriteU8(dst, 0);
 385     sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
 386     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
 387     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 388     sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL);
 389     sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE);
 390     sbufWriteU8(dst, crsfSpeed.portID);
 391     sbufWriteU8(dst, reply);
 392     crc8_poly_0xba_sbuf_append(dst, &lengthPtr[1]);
 393     *lengthPtr = sbufPtr(dst) - lengthPtr;
 394 }
 395
 396 static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart)
 397 {
 398     uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
 399     uint8_t ii = 0;
 400     for (ii = 0; ii < BAUD_COUNT; ++ii) {
 401         if (newBaudrate == baudRates[ii]) {
 402             break;
 403         }
 404     }
 405     crsfSpeed.portID = frameStart[1];
 406     crsfSpeed.index = ii;
 407 }
 408
 409 void crsfScheduleSpeedNegotiationResponse(void)
 410 {
 411     crsfSpeed.hasPendingReply = true;
 412     crsfSpeed.isNewSpeedValid = false;
 413 }
 414
 415 void speedNegotiationProcess(uint32_t currentTime)
 416 {
 417     if (!featureIsEnabled(FEATURE_TELEMETRY) && getCrsfDesiredSpeed() == CRSF_BAUDRATE) {
 418         // to notify the RX to fall back to default baud rate by sending device info frame if telemetry is disabled
 419         sbuf_t crsfPayloadBuf;
 420         sbuf_t *dst = &crsfPayloadBuf;
 421         crsfInitializeFrame(dst);
 422         crsfFrameDeviceInfo(dst);
 423         crsfFinalize(dst);
 424         crsfRxSendTelemetryData();
 425     } else {
 426         if (crsfSpeed.hasPendingReply) {
 427             bool found = crsfSpeed.index < BAUD_COUNT ? true : false;
 428             sbuf_t crsfSpeedNegotiationBuf;
 429             sbuf_t *dst = &crsfSpeedNegotiationBuf;
 430             crsfInitializeFrame(dst);
 431             crsfFrameSpeedNegotiationResponse(dst, found);
 432             crsfFinalize(dst);
 433             crsfRxSendTelemetryData();
 434             crsfSpeed.hasPendingReply = false;
 435             crsfSpeed.isNewSpeedValid = true;
 436             crsfSpeed.confirmationTime = currentTime;
 437             return;
 438         } else if (crsfSpeed.isNewSpeedValid) {
 439             if (currentTime - crsfSpeed.confirmationTime >= 4000) {
 440                 // delay 4ms before applying the new baudrate
 441                 crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
 442                 crsfSpeed.isNewSpeedValid = false;
 443                 isCrsfV3Running = true;
 444                 return;
 445             }
 446         }
 447     }
 448 }
 449 #endif
 450
 451 #if defined(USE_CRSF_CMS_TELEMETRY)
 452 #define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH   50
 453 #define CRSF_DISPLAYPORT_BATCH_MAX          0x3F
 454 #define CRSF_DISPLAYPORT_FIRST_CHUNK_MASK   0x80
 455 #define CRSF_DISPLAYPORT_LAST_CHUNK_MASK    0x40
 456 #define CRSF_DISPLAYPORT_SANITIZE_MASK      0x60
 457 #define CRSF_RLE_CHAR_REPEATED_MASK         0x80
 458 #define CRSF_RLE_MAX_RUN_LENGTH             256
 459 #define CRSF_RLE_BATCH_SIZE                 2
 460
 461 static uint16_t getRunLength(const void *start, const void *end)
 462 {
 463     uint8_t *cursor = (uint8_t*)start;
 464     uint8_t c = *cursor;
 465     size_t runLength = 0;
 466     for (; cursor != end; cursor++) {
 467         if (*cursor == c) {
 468             runLength++;
 469         } else {
 470             break;
 471         }
 472     }
 473     return runLength;
 474 }
 475
 476 static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
 477 {
 478     const uint8_t *destEnd = sbufPtr(dest) + maxDestLen;
 479     while (sbufBytesRemaining(source) && (sbufPtr(dest) < destEnd)) {
 480         const uint8_t destRemaining = destEnd - sbufPtr(dest);
 481         const uint8_t *srcPtr = sbufPtr(source);
 482         const uint16_t runLength = getRunLength(srcPtr, source->end);
 483         uint8_t c = *srcPtr;
 484         if (runLength > 1) {
 485             c |=  CRSF_RLE_CHAR_REPEATED_MASK;
 486             const uint8_t fullBatches = (runLength / CRSF_RLE_MAX_RUN_LENGTH);
 487             const uint8_t remainder = (runLength % CRSF_RLE_MAX_RUN_LENGTH);
 488             const uint8_t totalBatches = fullBatches + (remainder) ? 1 : 0;
 489             if (destRemaining >= totalBatches * CRSF_RLE_BATCH_SIZE) {
 490                 for (unsigned int i = 1; i <= totalBatches; i++) {
 491                     const uint8_t batchLength = (i < totalBatches) ? CRSF_RLE_MAX_RUN_LENGTH : remainder;
 492                     sbufWriteU8(dest, c);
 493                     sbufWriteU8(dest, batchLength);
 494                 }
 495                 sbufAdvance(source, runLength);
 496             } else {
 497                 break;
 498             }
 499         } else if (destRemaining >= runLength) {
 500             sbufWriteU8(dest, c);
 501             sbufAdvance(source, runLength);
 502         }
 503     }
 504 }
 505
 506 static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId, uint8_t idx)
 507 {
 508     uint8_t *lengthPtr = sbufPtr(dst);
 509     sbufWriteU8(dst, 0);
 510     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
 511     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 512     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 513     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_UPDATE);
 514     uint8_t *metaPtr = sbufPtr(dst);
 515     sbufWriteU8(dst, batchId);
 516     sbufWriteU8(dst, idx);
 517     cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH);
 518     if (idx == 0) {
 519         *metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
 520     }
 521     if (!sbufBytesRemaining(src)) {
 522         *metaPtr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
 523     }
 524     *lengthPtr = sbufPtr(dst) - lengthPtr;
 525 }
 526
 527 static void crsfFrameDisplayPortClear(sbuf_t *dst)
 528 {
 529     uint8_t *lengthPtr = sbufPtr(dst);
 530     sbufWriteU8(dst, CRSF_DISPLAY_PORT_COLS_MAX + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
 531     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
 532     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 533     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 534     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_CLEAR);
 535     *lengthPtr = sbufPtr(dst) - lengthPtr;
 536 }
 537
 538 #endif
 539
 540 // schedule array to decide how often each type of frame is sent
 541 typedef enum {
 542     CRSF_FRAME_START_INDEX = 0,
 543     CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
 544     CRSF_FRAME_BATTERY_SENSOR_INDEX,
 545     CRSF_FRAME_FLIGHT_MODE_INDEX,
 546     CRSF_FRAME_GPS_INDEX,
 547     CRSF_SCHEDULE_COUNT_MAX
 548 } crsfFrameTypeIndex_e;
 549
 550 static uint8_t crsfScheduleCount;
 551 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 552
 553 #if defined(USE_MSP_OVER_TELEMETRY)
 554
 555 static bool mspReplyPending;
 556
 557 void crsfScheduleMspResponse(void)
 558 {
 559     mspReplyPending = true;
 560 }
 561
 562 void crsfSendMspResponse(uint8_t *payload)
 563 {
 564     sbuf_t crsfPayloadBuf;
 565     sbuf_t *dst = &crsfPayloadBuf;
 566
 567     crsfInitializeFrame(dst);
 568     sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
 569     sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP);
 570     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 571     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 572     sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
 573     crsfFinalize(dst);
 574 }
 575 #endif
 576
 577 static void processCrsf(void)
 578 {
 579     static uint8_t crsfScheduleIndex = 0;
 580
 581     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 582
 583     sbuf_t crsfPayloadBuf;
 584     sbuf_t *dst = &crsfPayloadBuf;
 585
 586     if (currentSchedule & BIT(CRSF_FRAME_ATTITUDE_INDEX)) {
 587         crsfInitializeFrame(dst);
 588         crsfFrameAttitude(dst);
 589         crsfFinalize(dst);
 590     }
 591     if (currentSchedule & BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
 592         crsfInitializeFrame(dst);
 593         crsfFrameBatterySensor(dst);
 594         crsfFinalize(dst);
 595     }
 596
 597     if (currentSchedule & BIT(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
 598         crsfInitializeFrame(dst);
 599         crsfFrameFlightMode(dst);
 600         crsfFinalize(dst);
 601     }
 602 #ifdef USE_GPS
 603     if (currentSchedule & BIT(CRSF_FRAME_GPS_INDEX)) {
 604         crsfInitializeFrame(dst);
 605         crsfFrameGps(dst);
 606         crsfFinalize(dst);
 607     }
 608 #endif
 609     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 610 }
 611
 612 void crsfScheduleDeviceInfoResponse(void)
 613 {
 614     deviceInfoReplyPending = true;
 615 }
 616
 617 void initCrsfTelemetry(void)
 618 {
 619     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
 620     // and feature is enabled, if so, set CRSF telemetry enabled
 621     crsfTelemetryEnabled = crsfRxIsActive();
 622
 623     if (!crsfTelemetryEnabled) {
 624         return;
 625     }
 626
 627     deviceInfoReplyPending = false;
 628 #if defined(USE_MSP_OVER_TELEMETRY)
 629     mspReplyPending = false;
 630 #endif
 631
 632     int index = 0;
 633     if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
 634         crsfSchedule[index++] = BIT(CRSF_FRAME_ATTITUDE_INDEX);
 635     }
 636     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 637         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 638         crsfSchedule[index++] = BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
 639     }
 640     if (telemetryIsSensorEnabled(SENSOR_MODE)) {
 641         crsfSchedule[index++] = BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
 642     }
 643 #ifdef USE_GPS
 644     if (featureIsEnabled(FEATURE_GPS)
 645        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 646         crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
 647     }
 648 #endif
 649     crsfScheduleCount = (uint8_t)index;
 650
 651 #if defined(USE_CRSF_CMS_TELEMETRY)
 652     crsfDisplayportRegister();
 653 #endif
 654 }
 655
 656 bool checkCrsfTelemetryState(void)
 657 {
 658     return crsfTelemetryEnabled;
 659 }
 660
 661 #if defined(USE_CRSF_CMS_TELEMETRY)
 662 void crsfProcessDisplayPortCmd(uint8_t *frameStart)
 663 {
 664     uint8_t cmd = *frameStart;
 665     switch (cmd) {
 666     case CRSF_DISPLAYPORT_SUBCMD_OPEN: ;
 667         const uint8_t rows = *(frameStart + CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET);
 668         const uint8_t cols = *(frameStart + CRSF_DISPLAYPORT_OPEN_COLS_OFFSET);
 669         crsfDisplayPortSetDimensions(rows, cols);
 670         crsfDisplayPortMenuOpen();
 671         break;
 672     case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
 673         crsfDisplayPortMenuExit();
 674         break;
 675     case CRSF_DISPLAYPORT_SUBCMD_POLL:
 676         crsfDisplayPortRefresh();
 677         break;
 678     default:
 679         break;
 680     }
 681
 682 }
 683
 684 #endif
 685
 686 #if defined(USE_CRSF_V3)
 687 void crsfProcessCommand(uint8_t *frameStart)
 688 {
 689     uint8_t cmd = *frameStart;
 690     uint8_t subCmd = frameStart[1];
 691     switch (cmd) {
 692     case CRSF_COMMAND_SUBCMD_GENERAL:
 693         switch (subCmd) {
 694         case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
 695             crsfProcessSpeedNegotiationCmd(&frameStart[1]);
 696             crsfScheduleSpeedNegotiationResponse();
 697             break;
 698         default:
 699             break;
 700         }
 701         break;
 702     default:
 703         break;
 704     }
 705 }
 706 #endif
 707
 708 /*
 709  * Called periodically by the scheduler
 710  */
 711 void handleCrsfTelemetry(timeUs_t currentTimeUs)
 712 {
 713     static uint32_t crsfLastCycleTime;
 714
 715     if (!crsfTelemetryEnabled) {
 716         return;
 717     }
 718     // Give the receiver a chance to send any outstanding telemetry data.
 719     // This needs to be done at high frequency, to enable the RX to send the telemetry frame
 720     // in between the RX frames.
 721     crsfRxSendTelemetryData();
 722
 723     // Send ad-hoc response frames as soon as possible
 724 #if defined(USE_MSP_OVER_TELEMETRY)
 725     if (mspReplyPending) {
 726         mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
 727         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 728         return;
 729     }
 730 #endif
 731
 732     if (deviceInfoReplyPending) {
 733         sbuf_t crsfPayloadBuf;
 734         sbuf_t *dst = &crsfPayloadBuf;
 735         crsfInitializeFrame(dst);
 736         crsfFrameDeviceInfo(dst);
 737         crsfFinalize(dst);
 738         deviceInfoReplyPending = false;
 739         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 740         return;
 741     }
 742
 743 #if defined(USE_CRSF_CMS_TELEMETRY)
 744     if (crsfDisplayPortScreen()->reset) {
 745         crsfDisplayPortScreen()->reset = false;
 746         sbuf_t crsfDisplayPortBuf;
 747         sbuf_t *dst = &crsfDisplayPortBuf;
 748         crsfInitializeFrame(dst);
 749         crsfFrameDisplayPortClear(dst);
 750         crsfFinalize(dst);
 751         crsfLastCycleTime = currentTimeUs;
 752         return;
 753     }
 754     static uint8_t displayPortBatchId = 0;
 755     if (crsfDisplayPortIsReady() && crsfDisplayPortScreen()->updated) {
 756         crsfDisplayPortScreen()->updated = false;
 757         uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
 758         uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
 759         uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
 760         sbuf_t displayPortSbuf;
 761         sbuf_t *src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
 762         sbuf_t crsfDisplayPortBuf;
 763         sbuf_t *dst = &crsfDisplayPortBuf;
 764         displayPortBatchId = (displayPortBatchId  + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
 765         uint8_t i = 0;
 766         while (sbufBytesRemaining(src)) {
 767             crsfInitializeFrame(dst);
 768             crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
 769             crsfFinalize(dst);
 770             crsfRxSendTelemetryData();
 771             i++;
 772         }
 773         crsfLastCycleTime = currentTimeUs;
 774         return;
 775     }
 776 #endif
 777
 778     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
 779     // Spread out scheduled frames evenly so each frame is sent at the same frequency.
 780     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
 781         crsfLastCycleTime = currentTimeUs;
 782         processCrsf();
 783     }
 784 }
 785
 786 #if defined(UNIT_TEST)
 787 static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
 788 {
 789     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 790     sbufSwitchToReader(dst, crsfFrame);
 791     const int frameSize = sbufBytesRemaining(dst);
 792     for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
 793         frame[ii] = sbufReadU8(dst);
 794     }
 795     return frameSize;
 796 }
 797
 798 STATIC_UNIT_TESTED int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
 799 {
 800     sbuf_t crsfFrameBuf;
 801     sbuf_t *sbuf = &crsfFrameBuf;
 802
 803     crsfInitializeFrame(sbuf);
 804     switch (frameType) {
 805     default:
 806     case CRSF_FRAMETYPE_ATTITUDE:
 807         crsfFrameAttitude(sbuf);
 808         break;
 809     case CRSF_FRAMETYPE_BATTERY_SENSOR:
 810         crsfFrameBatterySensor(sbuf);
 811         break;
 812     case CRSF_FRAMETYPE_FLIGHT_MODE:
 813         crsfFrameFlightMode(sbuf);
 814         break;
 815 #if defined(USE_GPS)
 816     case CRSF_FRAMETYPE_GPS:
 817         crsfFrameGps(sbuf);
 818         break;
 819 #endif
 820     }
 821     const int frameSize = crsfFinalizeBuf(sbuf, frame);
 822     return frameSize;
 823 }
 824 #endif
 825 #endif