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