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