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