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 #include <math.h>
  25 #include <limits.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_TELEMETRY_CRSF
  30
  31 #include "build/atomic.h"
  32 #include "build/build_config.h"
  33 #include "build/version.h"
  34
  35 #include "cms/cms.h"
  36
  37 #include "config/config.h"
  38 #include "config/feature.h"
  39
  40 #include "common/crc.h"
  41 #include "common/maths.h"
  42 #include "common/printf.h"
  43 #include "common/streambuf.h"
  44 #include "common/time.h"
  45 #include "common/utils.h"
  46
  47 #include "drivers/nvic.h"
  48 #include "drivers/persistent.h"
  49
  50 #include "fc/rc_modes.h"
  51 #include "fc/runtime_config.h"
  52
  53 #include "flight/gps_rescue.h"
  54 #include "flight/imu.h"
  55 #include "flight/position.h"
  56
  57 #include "io/displayport_crsf.h"
  58 #include "io/gps.h"
  59 #include "io/serial.h"
  60
  61 #include "pg/pg.h"
  62 #include "pg/pg_ids.h"
  63
  64 #include "rx/crsf.h"
  65 #include "rx/crsf_protocol.h"
  66
  67 #include "sensors/battery.h"
  68 #include "sensors/sensors.h"
  69 #include "sensors/barometer.h"
  70
  71 #include "telemetry/telemetry.h"
  72 #include "telemetry/msp_shared.h"
  73
  74 #include "crsf.h"
  75
  76 #define CRSF_CYCLETIME_US                   100000 // 100ms, 10 Hz
  77 #define CRSF_DEVICEINFO_VERSION             0x01
  78 #define CRSF_DEVICEINFO_PARAMETER_COUNT     0
  79
  80 #define CRSF_MSP_BUFFER_SIZE 96
  81 #define CRSF_MSP_LENGTH_OFFSET 1
  82
  83 static bool crsfTelemetryEnabled;
  84 static bool deviceInfoReplyPending;
  85 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
  86
  87 #if defined(USE_MSP_OVER_TELEMETRY)
  88 typedef struct mspBuffer_s {
  89     uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
  90     int len;
  91 } mspBuffer_t;
  92
  93 static mspBuffer_t mspRxBuffer;
  94
  95 #if defined(USE_CRSF_V3)
  96
  97 #define CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US 20000 // 20ms
  98
  99 #if defined(USE_CRSF_CMS_TELEMETRY)
 100 #define CRSF_LINK_TYPE_CHECK_US 250000 // 250 ms
 101 #define CRSF_ELRS_DISLAYPORT_CHUNK_INTERVAL_US 75000 // 75 ms
 102
 103 typedef enum {
 104     CRSF_LINK_UNKNOWN,
 105     CRSF_LINK_ELRS,
 106     CRSF_LINK_NOT_ELRS
 107 } crsfLinkType_t;
 108
 109 static crsfLinkType_t crsfLinkType = CRSF_LINK_UNKNOWN;
 110 static timeDelta_t crsfDisplayPortChunkIntervalUs = 0;
 111 #endif
 112
 113 static bool isCrsfV3Running = false;
 114 typedef struct {
 115     uint8_t hasPendingReply:1;
 116     uint8_t isNewSpeedValid:1;
 117     uint8_t portID:3;
 118     uint8_t index;
 119     uint32_t confirmationTime;
 120 } crsfSpeedControl_s;
 121
 122 static crsfSpeedControl_s crsfSpeed = {0};
 123
 124 uint32_t getCrsfCachedBaudrate(void)
 125 {
 126     uint32_t crsfCachedBaudrate = persistentObjectRead(PERSISTENT_OBJECT_SERIALRX_BAUD);
 127     // check if valid first. return default baudrate if not
 128     for (unsigned i = 0; i < BAUD_COUNT; i++) {
 129         if (crsfCachedBaudrate == baudRates[i] && baudRates[i] >= CRSF_BAUDRATE) {
 130             return crsfCachedBaudrate;
 131         }
 132     }
 133     return CRSF_BAUDRATE;
 134 }
 135
 136 bool checkCrsfCustomizedSpeed(void)
 137 {
 138     return crsfSpeed.index < BAUD_COUNT ? true : false;
 139 }
 140
 141 uint32_t getCrsfDesiredSpeed(void)
 142 {
 143     return checkCrsfCustomizedSpeed() ? baudRates[crsfSpeed.index] : CRSF_BAUDRATE;
 144 }
 145
 146 void setCrsfDefaultSpeed(void)
 147 {
 148     crsfSpeed.hasPendingReply = false;
 149     crsfSpeed.isNewSpeedValid = false;
 150     crsfSpeed.confirmationTime = 0;
 151     crsfSpeed.index = BAUD_COUNT;
 152     isCrsfV3Running = false;
 153     crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
 154 }
 155
 156 bool crsfBaudNegotiationInProgress(void)
 157 {
 158     return crsfSpeed.hasPendingReply || crsfSpeed.isNewSpeedValid;
 159 }
 160 #endif
 161
 162 void initCrsfMspBuffer(void)
 163 {
 164     mspRxBuffer.len = 0;
 165 }
 166
 167 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
 168 {
 169     if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
 170         return false;
 171     } else {
 172         uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
 173         *p++ = frameLength;
 174         memcpy(p, frameStart, frameLength);
 175         mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
 176         return true;
 177     }
 178 }
 179
 180 bool handleCrsfMspFrameBuffer(mspResponseFnPtr responseFn)
 181 {
 182     static bool replyPending = false;
 183     if (replyPending) {
 184         if (crsfRxIsTelemetryBufEmpty()) {
 185             replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
 186         }
 187         return replyPending;
 188     }
 189     if (!mspRxBuffer.len) {
 190         return false;
 191     }
 192     int pos = 0;
 193     while (true) {
 194         const uint8_t mspFrameLength = mspRxBuffer.bytes[pos];
 195         if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
 196             if (crsfRxIsTelemetryBufEmpty()) {
 197                 replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, responseFn);
 198             } else {
 199                 replyPending = true;
 200             }
 201         }
 202         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
 203         ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
 204             if (pos >= mspRxBuffer.len) {
 205                 mspRxBuffer.len = 0;
 206                 return replyPending;
 207             }
 208         }
 209     }
 210     return replyPending;
 211 }
 212 #endif
 213
 214 static void crsfInitializeFrame(sbuf_t *dst)
 215 {
 216     dst->ptr = crsfFrame;
 217     dst->end = ARRAYEND(crsfFrame);
 218
 219     sbufWriteU8(dst, CRSF_SYNC_BYTE);
 220 }
 221
 222 static void crsfFinalize(sbuf_t *dst)
 223 {
 224     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
 225     sbufSwitchToReader(dst, crsfFrame);
 226     // write the telemetry frame to the receiver.
 227     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 228 }
 229
 230 /*
 231 CRSF frame has the structure:
 232 <Device address> <Frame length> <Type> <Payload> <CRC>
 233 Device address: (uint8_t)
 234 Frame length:   length in  bytes including Type (uint8_t)
 235 Type:           (uint8_t)
 236 CRC:            (uint8_t), crc of <Type> and <Payload>
 237 */
 238
 239 /*
 240 0x02 GPS
 241 Payload:
 242 int32_t     Latitude ( degree / 10`000`000 )
 243 int32_t     Longitude (degree / 10`000`000 )
 244 uint16_t    Groundspeed ( km/h / 10 )
 245 uint16_t    GPS heading ( degree / 100 )
 246 uint16      Altitude ( meter 1000m offset )
 247 uint8_t     Satellites in use ( counter )
 248 */
 249 void crsfFrameGps(sbuf_t *dst)
 250 {
 251     // use sbufWrite since CRC does not include frame length
 252     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 253     sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
 254     sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
 255     sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
 256     sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
 257     sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
 258     const uint16_t altitude = (constrain(getEstimatedAltitudeCm(), 0 * 100, 5000 * 100) / 100) + 1000; // constrain altitude from 0 to 5,000m
 259     sbufWriteU16BigEndian(dst, altitude);
 260     sbufWriteU8(dst, gpsSol.numSat);
 261 }
 262
 263 /*
 264 0x07 Vario sensor
 265 Payload:
 266 int16_t     Vertical speed ( cm/s )
 267 */
 268 void crsfFrameVarioSensor(sbuf_t *dst)
 269 {
 270     // use sbufWrite since CRC does not include frame length
 271     sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 272     sbufWriteU8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
 273     sbufWriteU16BigEndian(dst, getEstimatedVario()); // vario, cm/s(Z));
 274 }
 275
 276 /*
 277 0x08 Battery sensor
 278 Payload:
 279 uint16_t    Voltage ( mV * 100 )
 280 uint16_t    Current ( mA * 100 )
 281 uint24_t    Fuel ( drawn mAh )
 282 uint8_t     Battery remaining ( percent )
 283 */
 284 void crsfFrameBatterySensor(sbuf_t *dst)
 285 {
 286     // use sbufWrite since CRC does not include frame length
 287     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 288     sbufWriteU8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
 289     if (telemetryConfig()->report_cell_voltage) {
 290         sbufWriteU16BigEndian(dst, (getBatteryAverageCellVoltage() + 5) / 10); // vbat is in units of 0.01V
 291     } else {
 292         sbufWriteU16BigEndian(dst, getLegacyBatteryVoltage());
 293     }
 294     sbufWriteU16BigEndian(dst, getAmperage() / 10);
 295     const uint32_t mAhDrawn = getMAhDrawn();
 296     const uint8_t batteryRemainingPercentage = calculateBatteryPercentageRemaining();
 297     sbufWriteU8(dst, (mAhDrawn >> 16));
 298     sbufWriteU8(dst, (mAhDrawn >> 8));
 299     sbufWriteU8(dst, (uint8_t)mAhDrawn);
 300     sbufWriteU8(dst, batteryRemainingPercentage);
 301 }
 302
 303 #if defined(USE_BARO) && defined(USE_VARIO)
 304 // pack altitude in decimeters into a 16-bit value.
 305 // Due to strange OpenTX behavior of count any 0xFFFF value as incorrect, the maximum sending value is limited to 0xFFFE (32766 meters)
 306 // in order to have both precision and range in 16-bit
 307 // value of altitude is packed with different precision depending on highest-bit value.
 308 // on receiving side:
 309 // if MSB==0, altitude is sent in decimeters as uint16 with -1000m base. So, range is -1000..2276m.
 310 // if MSB==1, altitude is sent in meters with 0 base. So, range is 0..32766m (MSB must be zeroed).
 311 // altitude lower  than -1000m is sent as zero   (should be displayed as "<-1000m" or something).
 312 // altitude higher than 32767m is sent as 0xfffe (should be displayed as ">32766m" or something).
 313 // range from 0 to 2276m might be sent with dm- or m-precision. But this function always use dm-precision.
 314 static inline uint16_t calcAltitudePacked(int32_t altitude_dm)
 315 {
 316     static const int ALT_DM_OFFSET = 10000;
 317     int valDm = altitude_dm + ALT_DM_OFFSET;
 318
 319     if (valDm < 0) return 0;   // too low, return minimum
 320     if (valDm < 0x8000) return valDm;  // 15 bits to return dm value with offset
 321
 322     return MIN((altitude_dm + 5) / 10, 0x7fffe) | 0x8000; // positive 15bit value in meters, with OpenTX limit
 323 }
 324
 325 static inline int8_t calcVerticalSpeedPacked(int16_t verticalSpeed) // Vertical speed in m/s (meters per second)
 326 {
 327     // linearity coefficient.
 328     // Bigger values lead to more linear output i.e., less precise smaller values and more precise big values.
 329     // Decreasing the coefficient increases nonlinearity, i.e., more precise small values and less precise big values.
 330     static const float Kl = 100.0f;
 331
 332     // Range coefficient is calculated as result_max / log(verticalSpeedMax * LinearityCoefficient + 1);
 333     // but it must be set manually (not calculated) for equality of packing and unpacking
 334     static const float Kr = .026f;
 335
 336     int8_t sign = verticalSpeed < 0 ? -1 : 1;
 337     const int result32 = lrintf(log_approx(verticalSpeed * sign / Kl + 1) / Kr) * sign;
 338     int8_t result8 = constrain(result32, SCHAR_MIN, SCHAR_MAX);
 339     return result8;
 340
 341     // for unpacking the following function might be used:
 342     // int unpacked = lrintf((expf(result8 * sign * Kr) - 1) * Kl) * sign;
 343     // lrint might not be used depending on integer or floating output.
 344 }
 345
 346 // pack barometric altitude
 347 static void crsfFrameAltitude(sbuf_t *dst)
 348 {
 349     // use sbufWrite since CRC does not include frame length
 350     sbufWriteU8(dst, CRSF_FRAME_BARO_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 351     sbufWriteU8(dst, CRSF_FRAMETYPE_BARO_ALTITUDE);
 352     sbufWriteU16BigEndian(dst, calcAltitudePacked((baro.altitude + 5) / 10));
 353     sbufWriteU8(dst, calcVerticalSpeedPacked(getEstimatedVario()));
 354 }
 355 #endif
 356
 357 /*
 358 0x0B Heartbeat
 359 Payload:
 360 int16_t    origin_add ( Origin Device address )
 361 */
 362 void crsfFrameHeartbeat(sbuf_t *dst)
 363 {
 364     sbufWriteU8(dst, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 365     sbufWriteU8(dst, CRSF_FRAMETYPE_HEARTBEAT);
 366     sbufWriteU16BigEndian(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 367 }
 368
 369 /*
 370 0x28 Ping
 371 Payload:
 372 int8_t    destination_add ( Destination Device address )
 373 int8_t    origin_add ( Origin Device address )
 374 */
 375 void crsfFramePing(sbuf_t *dst)
 376 {
 377     sbufWriteU8(dst, CRSF_FRAME_DEVICE_PING_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 378     sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_PING);
 379     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
 380     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 381 }
 382
 383 typedef enum {
 384     CRSF_ACTIVE_ANTENNA1 = 0,
 385     CRSF_ACTIVE_ANTENNA2 = 1
 386 } crsfActiveAntenna_e;
 387
 388 typedef enum {
 389     CRSF_RF_MODE_4_HZ = 0,
 390     CRSF_RF_MODE_50_HZ = 1,
 391     CRSF_RF_MODE_150_HZ = 2
 392 } crsrRfMode_e;
 393
 394 typedef enum {
 395     CRSF_RF_POWER_0_mW = 0,
 396     CRSF_RF_POWER_10_mW = 1,
 397     CRSF_RF_POWER_25_mW = 2,
 398     CRSF_RF_POWER_100_mW = 3,
 399     CRSF_RF_POWER_500_mW = 4,
 400     CRSF_RF_POWER_1000_mW = 5,
 401     CRSF_RF_POWER_2000_mW = 6,
 402     CRSF_RF_POWER_250_mW = 7,
 403     CRSF_RF_POWER_50_mW = 8
 404 } crsrRfPower_e;
 405
 406 /*
 407 0x1E Attitude
 408 Payload:
 409 int16_t     Pitch angle ( rad / 10000 )
 410 int16_t     Roll angle ( rad / 10000 )
 411 int16_t     Yaw angle ( rad / 10000 )
 412 */
 413
 414 // convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
 415 static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
 416 {
 417     while (angle_decidegree > 1800) {
 418         angle_decidegree -= 3600;
 419     }
 420     while (angle_decidegree < -1800) {
 421         angle_decidegree += 3600;
 422     }
 423     return (int16_t)(RAD * 1000.0f * angle_decidegree);
 424 }
 425
 426 // fill dst buffer with crsf-attitude telemetry frame
 427 void crsfFrameAttitude(sbuf_t *dst)
 428 {
 429     sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 430     sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE);
 431     sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.pitch));
 432     sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.roll));
 433     sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.yaw));
 434 }
 435
 436 /*
 437 0x21 Flight mode text based
 438 Payload:
 439 char[]      Flight mode ( Null terminated string )
 440 */
 441 void crsfFrameFlightMode(sbuf_t *dst)
 442 {
 443     // write zero for frame length, since we don't know it yet
 444     uint8_t *lengthPtr = sbufPtr(dst);
 445     sbufWriteU8(dst, 0);
 446     sbufWriteU8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 447
 448     // Acro is the default mode
 449     const char *flightMode = "ACRO";
 450
 451     // Flight modes in decreasing order of importance
 452     if (FLIGHT_MODE(FAILSAFE_MODE) || IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
 453         flightMode = "!FS!";
 454     } else if (FLIGHT_MODE(GPS_RESCUE_MODE) || IS_RC_MODE_ACTIVE(BOXGPSRESCUE)) {
 455         flightMode = "RTH";
 456     } else if (FLIGHT_MODE(PASSTHRU_MODE)) {
 457         flightMode = "MANU";
 458     } else if (FLIGHT_MODE(ANGLE_MODE)) {
 459         flightMode = "ANGL";
 460     } else if (FLIGHT_MODE(ALT_HOLD_MODE)) {
 461         flightMode = "ALTH";
 462     } else if (FLIGHT_MODE(HORIZON_MODE)) {
 463         flightMode = "HOR";
 464     } else if (isAirmodeEnabled()) {
 465         flightMode = "AIR";
 466     }
 467
 468     sbufWriteString(dst, flightMode);
 469
 470     if (!ARMING_FLAG(ARMED) && !FLIGHT_MODE(FAILSAFE_MODE)) {
 471         // * = ready to arm
 472         // ! = arming disabled
 473         // ? = GPS rescue disabled
 474         bool isGpsRescueDisabled = false;
 475 #ifdef USE_GPS
 476         isGpsRescueDisabled = featureIsEnabled(FEATURE_GPS) && gpsRescueIsConfigured() && gpsSol.numSat < gpsRescueConfig()->minSats && !STATE(GPS_FIX);
 477 #endif
 478         sbufWriteU8(dst, isArmingDisabled() ? '!' : isGpsRescueDisabled ? '?' : '*');
 479     }
 480
 481     sbufWriteU8(dst, '\0');     // zero-terminate string
 482     // write in the frame length
 483     *lengthPtr = sbufPtr(dst) - lengthPtr;
 484 }
 485
 486 /*
 487 0x29 Device Info
 488 Payload:
 489 uint8_t     Destination
 490 uint8_t     Origin
 491 char[]      Device Name ( Null terminated string )
 492 uint32_t    Null Bytes
 493 uint32_t    Null Bytes
 494 uint32_t    Null Bytes
 495 uint8_t     255 (Max MSP Parameter)
 496 uint8_t     0x01 (Parameter version 1)
 497 */
 498 void crsfFrameDeviceInfo(sbuf_t *dst)
 499 {
 500     char buff[30];
 501     tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
 502
 503     uint8_t *lengthPtr = sbufPtr(dst);
 504     sbufWriteU8(dst, 0);
 505     sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
 506     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 507     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 508     sbufWriteStringWithZeroTerminator(dst, buff);
 509     for (unsigned int ii = 0; ii < 12; ii++) {
 510         sbufWriteU8(dst, 0x00);
 511     }
 512     sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
 513     sbufWriteU8(dst, CRSF_DEVICEINFO_VERSION);
 514     *lengthPtr = sbufPtr(dst) - lengthPtr;
 515 }
 516
 517 #if defined(USE_CRSF_V3)
 518 void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
 519 {
 520     uint8_t *lengthPtr = sbufPtr(dst);
 521     sbufWriteU8(dst, 0);
 522     sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
 523     sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
 524     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 525     sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL);
 526     sbufWriteU8(dst, CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_RESPONSE);
 527     sbufWriteU8(dst, crsfSpeed.portID);
 528     sbufWriteU8(dst, reply);
 529     crc8_poly_0xba_sbuf_append(dst, &lengthPtr[1]);
 530     *lengthPtr = sbufPtr(dst) - lengthPtr;
 531 }
 532
 533 static void crsfProcessSpeedNegotiationCmd(const uint8_t *frameStart)
 534 {
 535     uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
 536     uint8_t ii = 0;
 537     for (ii = 0; ii < BAUD_COUNT; ++ii) {
 538         if (newBaudrate == baudRates[ii]) {
 539             break;
 540         }
 541     }
 542     crsfSpeed.portID = frameStart[1];
 543     crsfSpeed.index = ii;
 544 }
 545
 546 void crsfScheduleSpeedNegotiationResponse(void)
 547 {
 548     crsfSpeed.hasPendingReply = true;
 549     crsfSpeed.isNewSpeedValid = false;
 550 }
 551
 552 void speedNegotiationProcess(timeUs_t currentTimeUs)
 553 {
 554     if (crsfSpeed.hasPendingReply) {
 555         bool found = ((crsfSpeed.index < BAUD_COUNT) && crsfRxUseNegotiatedBaud()) ? true : false;
 556         sbuf_t crsfSpeedNegotiationBuf;
 557         sbuf_t *dst = &crsfSpeedNegotiationBuf;
 558         crsfInitializeFrame(dst);
 559         crsfFrameSpeedNegotiationResponse(dst, found);
 560         crsfRxSendTelemetryData(); // prevent overwriting previous data
 561         crsfFinalize(dst);
 562         crsfRxSendTelemetryData();
 563         crsfSpeed.hasPendingReply = false;
 564         crsfSpeed.isNewSpeedValid = found;
 565         crsfSpeed.confirmationTime = currentTimeUs;
 566     } else if (crsfSpeed.isNewSpeedValid) {
 567         if (cmpTimeUs(currentTimeUs, crsfSpeed.confirmationTime) >= 4000) {
 568             // delay 4ms before applying the new baudrate
 569             crsfRxUpdateBaudrate(getCrsfDesiredSpeed());
 570             crsfSpeed.isNewSpeedValid = false;
 571             isCrsfV3Running = true;
 572         }
 573     } else if (!featureIsEnabled(FEATURE_TELEMETRY) && crsfRxUseNegotiatedBaud()) {
 574         // Send heartbeat if telemetry is disabled to allow RX to detect baud rate mismatches
 575         sbuf_t crsfPayloadBuf;
 576         sbuf_t *dst = &crsfPayloadBuf;
 577         crsfInitializeFrame(dst);
 578         crsfFrameHeartbeat(dst);
 579         crsfRxSendTelemetryData(); // prevent overwriting previous data
 580         crsfFinalize(dst);
 581         crsfRxSendTelemetryData();
 582 #if defined(USE_CRSF_CMS_TELEMETRY)
 583     } else if (crsfLinkType == CRSF_LINK_UNKNOWN) {
 584         static timeUs_t lastPing;
 585
 586         if ((cmpTimeUs(currentTimeUs, lastPing) > CRSF_LINK_TYPE_CHECK_US)) {
 587             // Send a ping, the response to which will be a device info response giving the rx serial number
 588             sbuf_t crsfPayloadBuf;
 589             sbuf_t *dst = &crsfPayloadBuf;
 590             crsfInitializeFrame(dst);
 591             crsfFramePing(dst);
 592             crsfRxSendTelemetryData(); // prevent overwriting previous data
 593             crsfFinalize(dst);
 594             crsfRxSendTelemetryData();
 595
 596             lastPing = currentTimeUs;
 597         }
 598 #endif
 599     }
 600 }
 601 #endif
 602
 603 #if defined(USE_CRSF_CMS_TELEMETRY)
 604 #define CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH   50
 605 #define CRSF_DISPLAYPORT_BATCH_MAX          0x3F
 606 #define CRSF_DISPLAYPORT_FIRST_CHUNK_MASK   0x80
 607 #define CRSF_DISPLAYPORT_LAST_CHUNK_MASK    0x40
 608 #define CRSF_DISPLAYPORT_SANITIZE_MASK      0x60
 609 #define CRSF_RLE_CHAR_REPEATED_MASK         0x80
 610 #define CRSF_RLE_MAX_RUN_LENGTH             256
 611 #define CRSF_RLE_BATCH_SIZE                 2
 612
 613 static uint16_t getRunLength(const void *start, const void *end)
 614 {
 615     uint8_t *cursor = (uint8_t*)start;
 616     uint8_t c = *cursor;
 617     size_t runLength = 0;
 618     for (; cursor != end; cursor++) {
 619         if (*cursor == c) {
 620             runLength++;
 621         } else {
 622             break;
 623         }
 624     }
 625     return runLength;
 626 }
 627
 628 static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
 629 {
 630     const uint8_t *destEnd = sbufPtr(dest) + maxDestLen;
 631     while (sbufBytesRemaining(source) && (sbufPtr(dest) < destEnd)) {
 632         const uint8_t destRemaining = destEnd - sbufPtr(dest);
 633         const uint8_t *srcPtr = sbufPtr(source);
 634         const uint16_t runLength = getRunLength(srcPtr, source->end);
 635         uint8_t c = *srcPtr;
 636         if (runLength > 1) {
 637             c |=  CRSF_RLE_CHAR_REPEATED_MASK;
 638             const uint8_t fullBatches = (runLength / CRSF_RLE_MAX_RUN_LENGTH);
 639             const uint8_t remainder = (runLength % CRSF_RLE_MAX_RUN_LENGTH);
 640             const uint8_t totalBatches = fullBatches + (remainder ? 1 : 0);
 641             if (destRemaining >= totalBatches * CRSF_RLE_BATCH_SIZE) {
 642                 for (unsigned int i = 1; i <= totalBatches; i++) {
 643                     const uint8_t batchLength = (i < totalBatches) ? CRSF_RLE_MAX_RUN_LENGTH : remainder;
 644                     sbufWriteU8(dest, c);
 645                     sbufWriteU8(dest, batchLength);
 646                 }
 647                 sbufAdvance(source, runLength);
 648             } else {
 649                 break;
 650             }
 651         } else if (destRemaining >= runLength) {
 652             sbufWriteU8(dest, c);
 653             sbufAdvance(source, runLength);
 654         }
 655     }
 656 }
 657
 658 static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId, uint8_t idx)
 659 {
 660     uint8_t *lengthPtr = sbufPtr(dst);
 661     sbufWriteU8(dst, 0);
 662     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
 663     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 664     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 665     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_UPDATE);
 666     uint8_t *metaPtr = sbufPtr(dst);
 667     sbufWriteU8(dst, batchId);
 668     sbufWriteU8(dst, idx);
 669     cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH);
 670     if (idx == 0) {
 671         *metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
 672     }
 673     if (!sbufBytesRemaining(src)) {
 674         *metaPtr |= CRSF_DISPLAYPORT_LAST_CHUNK_MASK;
 675     }
 676     *lengthPtr = sbufPtr(dst) - lengthPtr;
 677 }
 678
 679 static void crsfFrameDisplayPortClear(sbuf_t *dst)
 680 {
 681     uint8_t *lengthPtr = sbufPtr(dst);
 682     sbufWriteU8(dst, CRSF_DISPLAY_PORT_COLS_MAX + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
 683     sbufWriteU8(dst, CRSF_FRAMETYPE_DISPLAYPORT_CMD);
 684     sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 685     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 686     sbufWriteU8(dst, CRSF_DISPLAYPORT_SUBCMD_CLEAR);
 687     *lengthPtr = sbufPtr(dst) - lengthPtr;
 688 }
 689
 690 #endif
 691
 692 // schedule array to decide how often each type of frame is sent
 693 typedef enum {
 694     CRSF_FRAME_START_INDEX = 0,
 695     CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
 696     CRSF_FRAME_BARO_ALTITUDE_INDEX,
 697     CRSF_FRAME_BATTERY_SENSOR_INDEX,
 698     CRSF_FRAME_FLIGHT_MODE_INDEX,
 699     CRSF_FRAME_GPS_INDEX,
 700     CRSF_FRAME_VARIO_SENSOR_INDEX,
 701     CRSF_FRAME_HEARTBEAT_INDEX,
 702     CRSF_SCHEDULE_COUNT_MAX
 703 } crsfFrameTypeIndex_e;
 704
 705 static uint8_t crsfScheduleCount;
 706 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 707
 708 #if defined(USE_MSP_OVER_TELEMETRY)
 709
 710 static bool mspReplyPending;
 711 static uint8_t mspRequestOriginID = 0; // origin ID of last msp-over-crsf request. Needed to send response to the origin.
 712
 713 void crsfScheduleMspResponse(uint8_t requestOriginID)
 714 {
 715     mspReplyPending = true;
 716     mspRequestOriginID = requestOriginID;
 717 }
 718
 719 // sends MSP response chunk over CRSF. Must be of type mspResponseFnPtr
 720 static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
 721 {
 722     sbuf_t crsfPayloadBuf;
 723     sbuf_t *dst = &crsfPayloadBuf;
 724
 725     crsfInitializeFrame(dst);
 726     sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
 727     sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
 728     sbufWriteU8(dst, mspRequestOriginID);   // response destination must be the same as request origin in order to response reach proper destination.
 729     sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
 730     sbufWriteData(dst, payload, payloadSize);
 731     crsfFinalize(dst);
 732 }
 733 #endif
 734
 735 static void processCrsf(void)
 736 {
 737     if (!crsfRxIsTelemetryBufEmpty()) {
 738         return; // do nothing if telemetry ouptut buffer is not empty yet.
 739     }
 740
 741     static uint8_t crsfScheduleIndex = 0;
 742
 743     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 744
 745     sbuf_t crsfPayloadBuf;
 746     sbuf_t *dst = &crsfPayloadBuf;
 747
 748     if (currentSchedule & BIT(CRSF_FRAME_ATTITUDE_INDEX)) {
 749         crsfInitializeFrame(dst);
 750         crsfFrameAttitude(dst);
 751         crsfFinalize(dst);
 752     }
 753 #if defined(USE_BARO) && defined(USE_VARIO)
 754     // send barometric altitude
 755     if (currentSchedule & BIT(CRSF_FRAME_BARO_ALTITUDE_INDEX)) {
 756         crsfInitializeFrame(dst);
 757         crsfFrameAltitude(dst);
 758         crsfFinalize(dst);
 759     }
 760 #endif
 761     if (currentSchedule & BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
 762         crsfInitializeFrame(dst);
 763         crsfFrameBatterySensor(dst);
 764         crsfFinalize(dst);
 765     }
 766
 767     if (currentSchedule & BIT(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
 768         crsfInitializeFrame(dst);
 769         crsfFrameFlightMode(dst);
 770         crsfFinalize(dst);
 771     }
 772 #ifdef USE_GPS
 773     if (currentSchedule & BIT(CRSF_FRAME_GPS_INDEX)) {
 774         crsfInitializeFrame(dst);
 775         crsfFrameGps(dst);
 776         crsfFinalize(dst);
 777     }
 778 #endif
 779 #ifdef USE_VARIO
 780     if (currentSchedule & BIT(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
 781         crsfInitializeFrame(dst);
 782         crsfFrameVarioSensor(dst);
 783         crsfFinalize(dst);
 784     }
 785 #endif
 786 #if defined(USE_CRSF_V3)
 787     if (currentSchedule & BIT(CRSF_FRAME_HEARTBEAT_INDEX)) {
 788         crsfInitializeFrame(dst);
 789         crsfFrameHeartbeat(dst);
 790         crsfFinalize(dst);
 791     }
 792 #endif
 793
 794     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 795 }
 796
 797 void crsfScheduleDeviceInfoResponse(void)
 798 {
 799     deviceInfoReplyPending = true;
 800 }
 801
 802 #if defined(USE_CRSF_CMS_TELEMETRY)
 803 void crsfHandleDeviceInfoResponse(uint8_t *payload)
 804 {
 805     // Skip over dst/src address bytes
 806     payload += 2;
 807
 808     // Skip over first string which is the rx model/part number
 809     while (*payload++ != '\0');
 810
 811     // Check the serial number
 812     if (memcmp(payload, "ELRS", 4) == 0) {
 813         crsfLinkType = CRSF_LINK_ELRS;
 814         crsfDisplayPortChunkIntervalUs = CRSF_ELRS_DISLAYPORT_CHUNK_INTERVAL_US;
 815     } else {
 816         crsfLinkType = CRSF_LINK_NOT_ELRS;
 817     }
 818 }
 819 #endif
 820
 821 void initCrsfTelemetry(void)
 822 {
 823     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
 824     // and feature is enabled, if so, set CRSF telemetry enabled
 825     crsfTelemetryEnabled = crsfRxIsActive();
 826
 827     if (!crsfTelemetryEnabled) {
 828         return;
 829     }
 830
 831     deviceInfoReplyPending = false;
 832 #if defined(USE_MSP_OVER_TELEMETRY)
 833     mspReplyPending = false;
 834 #endif
 835
 836     int index = 0;
 837     if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) {
 838         crsfSchedule[index++] = BIT(CRSF_FRAME_ATTITUDE_INDEX);
 839     }
 840 #if defined(USE_BARO) && defined(USE_VARIO)
 841     if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
 842         crsfSchedule[index++] = BIT(CRSF_FRAME_BARO_ALTITUDE_INDEX);
 843     }
 844 #endif
 845     if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE))
 846         || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) {
 847         crsfSchedule[index++] = BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX);
 848     }
 849     if (telemetryIsSensorEnabled(SENSOR_MODE)) {
 850         crsfSchedule[index++] = BIT(CRSF_FRAME_FLIGHT_MODE_INDEX);
 851     }
 852 #ifdef USE_GPS
 853     if (featureIsEnabled(FEATURE_GPS)
 854        && telemetryIsSensorEnabled(SENSOR_ALTITUDE | SENSOR_LAT_LONG | SENSOR_GROUND_SPEED | SENSOR_HEADING)) {
 855         crsfSchedule[index++] = BIT(CRSF_FRAME_GPS_INDEX);
 856     }
 857 #endif
 858 #ifdef USE_VARIO
 859     if ((sensors(SENSOR_BARO) || featureIsEnabled(FEATURE_GPS)) && telemetryIsSensorEnabled(SENSOR_VARIO)) {
 860         crsfSchedule[index++] = BIT(CRSF_FRAME_VARIO_SENSOR_INDEX);
 861     }
 862 #endif
 863
 864 #if defined(USE_CRSF_V3)
 865     while (index < (CRSF_CYCLETIME_US / CRSF_TELEMETRY_FRAME_INTERVAL_MAX_US) && index < CRSF_SCHEDULE_COUNT_MAX) {
 866         // schedule heartbeat to ensure that telemetry/heartbeat frames are sent at minimum 50Hz
 867         crsfSchedule[index++] = BIT(CRSF_FRAME_HEARTBEAT_INDEX);
 868     }
 869 #endif
 870
 871     crsfScheduleCount = (uint8_t)index;
 872
 873 #if defined(USE_CRSF_CMS_TELEMETRY)
 874     crsfDisplayportRegister();
 875 #endif
 876 }
 877
 878 bool checkCrsfTelemetryState(void)
 879 {
 880     return crsfTelemetryEnabled;
 881 }
 882
 883 #if defined(USE_CRSF_CMS_TELEMETRY)
 884 void crsfProcessDisplayPortCmd(uint8_t *frameStart)
 885 {
 886     uint8_t cmd = *frameStart;
 887     switch (cmd) {
 888     case CRSF_DISPLAYPORT_SUBCMD_OPEN: ;
 889         const uint8_t rows = *(frameStart + CRSF_DISPLAYPORT_OPEN_ROWS_OFFSET);
 890         const uint8_t cols = *(frameStart + CRSF_DISPLAYPORT_OPEN_COLS_OFFSET);
 891         crsfDisplayPortSetDimensions(rows, cols);
 892         crsfDisplayPortMenuOpen();
 893         break;
 894     case CRSF_DISPLAYPORT_SUBCMD_CLOSE:
 895         crsfDisplayPortMenuExit();
 896         break;
 897     case CRSF_DISPLAYPORT_SUBCMD_POLL:
 898         crsfDisplayPortRefresh();
 899         break;
 900     default:
 901         break;
 902     }
 903
 904 }
 905
 906 #endif
 907
 908 #if defined(USE_CRSF_V3)
 909 void crsfProcessCommand(uint8_t *frameStart)
 910 {
 911     uint8_t cmd = *frameStart;
 912     uint8_t subCmd = frameStart[1];
 913     switch (cmd) {
 914     case CRSF_COMMAND_SUBCMD_GENERAL:
 915         switch (subCmd) {
 916         case CRSF_COMMAND_SUBCMD_GENERAL_CRSF_SPEED_PROPOSAL:
 917             crsfProcessSpeedNegotiationCmd(&frameStart[1]);
 918             crsfScheduleSpeedNegotiationResponse();
 919             break;
 920         default:
 921             break;
 922         }
 923         break;
 924     default:
 925         break;
 926     }
 927 }
 928 #endif
 929
 930 /*
 931  * Called periodically by the scheduler
 932  */
 933 void handleCrsfTelemetry(timeUs_t currentTimeUs)
 934 {
 935     static uint32_t crsfLastCycleTime;
 936
 937     if (!crsfTelemetryEnabled) {
 938         return;
 939     }
 940
 941 #if defined(USE_CRSF_V3)
 942     if (crsfBaudNegotiationInProgress()) {
 943         return;
 944     }
 945 #endif
 946
 947     // Give the receiver a chance to send any outstanding telemetry data.
 948     // This needs to be done at high frequency, to enable the RX to send the telemetry frame
 949     // in between the RX frames.
 950     crsfRxSendTelemetryData();
 951
 952     // Send ad-hoc response frames as soon as possible
 953 #if defined(USE_MSP_OVER_TELEMETRY)
 954     if (mspReplyPending) {
 955         mspReplyPending = handleCrsfMspFrameBuffer(&crsfSendMspResponse);
 956         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 957         return;
 958     }
 959 #endif
 960
 961     if (deviceInfoReplyPending) {
 962         sbuf_t crsfPayloadBuf;
 963         sbuf_t *dst = &crsfPayloadBuf;
 964         crsfInitializeFrame(dst);
 965         crsfFrameDeviceInfo(dst);
 966         crsfFinalize(dst);
 967         deviceInfoReplyPending = false;
 968         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 969         return;
 970     }
 971
 972 #if defined(USE_CRSF_CMS_TELEMETRY)
 973     if (crsfDisplayPortScreen()->reset) {
 974         crsfDisplayPortScreen()->reset = false;
 975         sbuf_t crsfDisplayPortBuf;
 976         sbuf_t *dst = &crsfDisplayPortBuf;
 977         crsfInitializeFrame(dst);
 978         crsfFrameDisplayPortClear(dst);
 979         crsfFinalize(dst);
 980         crsfLastCycleTime = currentTimeUs;
 981         return;
 982     }
 983
 984     if (crsfDisplayPortIsReady()) {
 985         static uint8_t displayPortBatchId = 0;
 986         static sbuf_t displayPortSbuf;
 987         static sbuf_t *src = NULL;
 988         static uint8_t batchIndex;
 989         static timeUs_t batchLastTimeUs;
 990         sbuf_t crsfDisplayPortBuf;
 991         sbuf_t *dst = &crsfDisplayPortBuf;
 992
 993         if (crsfDisplayPortScreen()->updated) {
 994             crsfDisplayPortScreen()->updated = false;
 995             uint16_t screenSize = crsfDisplayPortScreen()->rows * crsfDisplayPortScreen()->cols;
 996             uint8_t *srcStart = (uint8_t*)crsfDisplayPortScreen()->buffer;
 997             uint8_t *srcEnd = (uint8_t*)(crsfDisplayPortScreen()->buffer + screenSize);
 998             src = sbufInit(&displayPortSbuf, srcStart, srcEnd);
 999             displayPortBatchId = (displayPortBatchId  + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
1000             batchIndex = 0;
1001         }
1002
1003         // Wait between successive chunks of displayport data for CMS menu display to prevent ELRS buffer over-run if necessary
1004         if (src && sbufBytesRemaining(src) &&
1005             (cmpTimeUs(currentTimeUs, batchLastTimeUs) > crsfDisplayPortChunkIntervalUs)) {
1006             crsfInitializeFrame(dst);
1007             crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, batchIndex);
1008             crsfFinalize(dst);
1009             crsfRxSendTelemetryData();
1010             batchIndex++;
1011             batchLastTimeUs = currentTimeUs;
1012
1013             crsfLastCycleTime = currentTimeUs;
1014
1015             return;
1016         }
1017     }
1018 #endif
1019
1020     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
1021     // Spread out scheduled frames evenly so each frame is sent at the same frequency.
1022     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
1023         crsfLastCycleTime = currentTimeUs;
1024         processCrsf();
1025     }
1026 }
1027
1028 #if defined(UNIT_TEST) || defined(USE_RX_EXPRESSLRS)
1029 static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
1030 {
1031     crc8_dvb_s2_sbuf_append(dst, &crsfFrame[2]); // start at byte 2, since CRC does not include device address and frame length
1032     sbufSwitchToReader(dst, crsfFrame);
1033     const int frameSize = sbufBytesRemaining(dst);
1034     for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
1035         frame[ii] = sbufReadU8(dst);
1036     }
1037     return frameSize;
1038 }
1039
1040 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
1041 {
1042     sbuf_t crsfFrameBuf;
1043     sbuf_t *sbuf = &crsfFrameBuf;
1044
1045     crsfInitializeFrame(sbuf);
1046     switch (frameType) {
1047     default:
1048     case CRSF_FRAMETYPE_ATTITUDE:
1049         crsfFrameAttitude(sbuf);
1050         break;
1051     case CRSF_FRAMETYPE_BATTERY_SENSOR:
1052         crsfFrameBatterySensor(sbuf);
1053         break;
1054     case CRSF_FRAMETYPE_FLIGHT_MODE:
1055         crsfFrameFlightMode(sbuf);
1056         break;
1057 #if defined(USE_GPS)
1058     case CRSF_FRAMETYPE_GPS:
1059         crsfFrameGps(sbuf);
1060         break;
1061 #endif
1062 #if defined(USE_VARIO)
1063     case CRSF_FRAMETYPE_VARIO_SENSOR:
1064         crsfFrameVarioSensor(sbuf);
1065         break;
1066 #endif
1067 #if defined(USE_MSP_OVER_TELEMETRY)
1068     case CRSF_FRAMETYPE_DEVICE_INFO:
1069         crsfFrameDeviceInfo(sbuf);
1070         break;
1071 #endif
1072     }
1073     const int frameSize = crsfFinalizeBuf(sbuf, frame);
1074     return frameSize;
1075 }
1076
1077 #if defined(USE_MSP_OVER_TELEMETRY)
1078 int getCrsfMspFrame(uint8_t *frame, uint8_t *payload, const uint8_t payloadSize)
1079 {
1080     sbuf_t crsfFrameBuf;
1081     sbuf_t *sbuf = &crsfFrameBuf;
1082
1083     crsfInitializeFrame(sbuf);
1084     sbufWriteU8(sbuf, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
1085     sbufWriteU8(sbuf, CRSF_FRAMETYPE_MSP_RESP);
1086     sbufWriteU8(sbuf, CRSF_ADDRESS_RADIO_TRANSMITTER);
1087     sbufWriteU8(sbuf, CRSF_ADDRESS_FLIGHT_CONTROLLER);
1088     sbufWriteData(sbuf, payload, payloadSize);
1089     const int frameSize = crsfFinalizeBuf(sbuf, frame);
1090     return frameSize;
1091 }
1092 #endif
1093 #endif
1094 #endif