src/main/telemetry/crsf.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #if defined(USE_TELEMETRY) && defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
  25
  26 #include "build/atomic.h"
  27 #include "build/build_config.h"
  28 #include "build/version.h"
  29
  30 #include "common/axis.h"
  31 #include "common/crc.h"
  32 #include "common/streambuf.h"
  33 #include "common/time.h"
  34 #include "common/utils.h"
  35 #include "common/printf.h"
  36
  37 #include "config/feature.h"
  38
  39 #include "drivers/serial.h"
  40 #include "drivers/time.h"
  41 #include "drivers/nvic.h"
  42
  43 #include "fc/config.h"
  44 #include "fc/rc_controls.h"
  45 #include "fc/rc_modes.h"
  46 #include "fc/runtime_config.h"
  47
  48 #include "flight/imu.h"
  49
  50 #include "io/gps.h"
  51 #include "io/serial.h"
  52
  53 #include "navigation/navigation.h"
  54
  55 #include "rx/crsf.h"
  56 #include "rx/rx.h"
  57
  58 #include "sensors/battery.h"
  59 #include "sensors/sensors.h"
  60
  61 #include "telemetry/crsf.h"
  62 #include "telemetry/telemetry.h"
  63 #include "telemetry/msp_shared.h"
  64
  65
  66 #define CRSF_CYCLETIME_US                   100000  // 100ms, 10 Hz
  67 #define CRSF_DEVICEINFO_VERSION             0x01
  68 // According to TBS: "CRSF over serial should always use a sync byte at the beginning of each frame.
  69 // To get better performance it's recommended to use the sync byte 0xC8 to get better performance"
  70 //
  71 // Digitalentity: Using frame address byte as a sync field looks somewhat hacky to me, but seems it's needed to get CRSF working properly
  72 #define CRSF_DEVICEINFO_PARAMETER_COUNT     0
  73
  74 #define CRSF_MSP_BUFFER_SIZE 96
  75 #define CRSF_MSP_LENGTH_OFFSET 1
  76
  77 static uint8_t crsfCrc;
  78 static bool crsfTelemetryEnabled;
  79 static bool deviceInfoReplyPending;
  80 static uint8_t crsfFrame[CRSF_FRAME_SIZE_MAX];
  81
  82 #if defined(USE_MSP_OVER_TELEMETRY)
  83 typedef struct mspBuffer_s {
  84     uint8_t bytes[CRSF_MSP_BUFFER_SIZE];
  85     int len;
  86 } mspBuffer_t;
  87
  88 static mspBuffer_t mspRxBuffer;
  89
  90 void initCrsfMspBuffer(void)
  91 {
  92     mspRxBuffer.len = 0;
  93 }
  94
  95 bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
  96 {
  97     if (mspRxBuffer.len + CRSF_MSP_LENGTH_OFFSET + frameLength > CRSF_MSP_BUFFER_SIZE) {
  98         return false;
  99     } else {
 100         uint8_t *p = mspRxBuffer.bytes + mspRxBuffer.len;
 101         *p++ = frameLength;
 102         memcpy(p, frameStart, frameLength);
 103         mspRxBuffer.len += CRSF_MSP_LENGTH_OFFSET + frameLength;
 104         return true;
 105     }
 106 }
 107
 108 bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
 109 {
 110     bool requestHandled = false;
 111     if (!mspRxBuffer.len) {
 112         return false;
 113     }
 114     int pos = 0;
 115     while (true) {
 116         const int mspFrameLength = mspRxBuffer.bytes[pos];
 117         if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength)) {
 118             requestHandled |= sendMspReply(payloadSize, responseFn);
 119         }
 120         pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
 121         ATOMIC_BLOCK(NVIC_PRIO_SERIALUART) {
 122             if (pos >= mspRxBuffer.len) {
 123                 mspRxBuffer.len = 0;
 124                 return requestHandled;
 125             }
 126         }
 127     }
 128     return requestHandled;
 129 }
 130 #endif
 131
 132 static void crsfInitializeFrame(sbuf_t *dst)
 133 {
 134     crsfCrc = 0;
 135     dst->ptr = crsfFrame;
 136     dst->end = ARRAYEND(crsfFrame);
 137
 138     sbufWriteU8(dst, CRSF_TELEMETRY_SYNC_BYTE);
 139 }
 140
 141 static void crsfSerialize8(sbuf_t *dst, uint8_t v)
 142 {
 143     sbufWriteU8(dst, v);
 144     crsfCrc = crc8_dvb_s2(crsfCrc, v);
 145 }
 146
 147 static void crsfSerialize16(sbuf_t *dst, uint16_t v)
 148 {
 149     // Use BigEndian format
 150     crsfSerialize8(dst,  (v >> 8));
 151     crsfSerialize8(dst, (uint8_t)v);
 152 }
 153
 154 static void crsfSerialize32(sbuf_t *dst, uint32_t v)
 155 {
 156     // Use BigEndian format
 157     crsfSerialize8(dst, (v >> 24));
 158     crsfSerialize8(dst, (v >> 16));
 159     crsfSerialize8(dst, (v >> 8));
 160     crsfSerialize8(dst, (uint8_t)v);
 161 }
 162
 163 static void crsfSerializeData(sbuf_t *dst, const uint8_t *data, int len)
 164 {
 165     for (int ii = 0; ii< len; ++ii) {
 166         crsfSerialize8(dst, data[ii]);
 167     }
 168 }
 169
 170 static void crsfFinalize(sbuf_t *dst)
 171 {
 172     sbufWriteU8(dst, crsfCrc);
 173     sbufSwitchToReader(dst, crsfFrame);
 174     // write the telemetry frame to the receiver.
 175     crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 176 }
 177
 178 static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame)
 179 {
 180     sbufWriteU8(dst, crsfCrc);
 181     sbufSwitchToReader(dst, crsfFrame);
 182     const int frameSize = sbufBytesRemaining(dst);
 183     for (int ii = 0; sbufBytesRemaining(dst); ++ii) {
 184         frame[ii] = sbufReadU8(dst);
 185     }
 186     return frameSize;
 187 }
 188
 189 /*
 190 CRSF frame has the structure:
 191 <Device address> <Frame length> <Type> <Payload> <CRC>
 192 Device address: (uint8_t)
 193 Frame length:   length in  bytes including Type (uint8_t)
 194 Type:           (uint8_t)
 195 CRC:            (uint8_t), crc of <Type> and <Payload>
 196 */
 197
 198 /*
 199 0x02 GPS
 200 Payload:
 201 int32_t     Latitude ( degree / 10`000`000 )
 202 int32_t     Longitude (degree / 10`000`000 )
 203 uint16_t    Groundspeed ( km/h / 10 )
 204 uint16_t    GPS heading ( degree / 100 )
 205 uint16      Altitude ( meter 1000m offset )
 206 uint8_t     Satellites in use ( counter )
 207 */
 208 static void crsfFrameGps(sbuf_t *dst)
 209 {
 210     // use sbufWrite since CRC does not include frame length
 211     sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 212     crsfSerialize8(dst, CRSF_FRAMETYPE_GPS);
 213     crsfSerialize32(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
 214     crsfSerialize32(dst, gpsSol.llh.lon);
 215     crsfSerialize16(dst, (gpsSol.groundSpeed * 36 + 50) / 100); // gpsSol.groundSpeed is in cm/s
 216     crsfSerialize16(dst, DECIDEGREES_TO_CENTIDEGREES(gpsSol.groundCourse)); // gpsSol.groundCourse is 0.1 degrees, need 0.01 deg
 217     const uint16_t altitude = (getEstimatedActualPosition(Z) / 100) + 1000;
 218     crsfSerialize16(dst, altitude);
 219     crsfSerialize8(dst, gpsSol.numSat);
 220 }
 221
 222 /*
 223 0x07 Vario sensor
 224 Payload:
 225 int16      Vertical speed ( cm/s )
 226 */
 227 static void crsfFrameVarioSensor(sbuf_t *dst)
 228 {
 229     // use sbufWrite since CRC does not include frame length
 230     sbufWriteU8(dst, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 231     crsfSerialize8(dst, CRSF_FRAMETYPE_VARIO_SENSOR);
 232     crsfSerialize16(dst, lrintf(getEstimatedActualVelocity(Z)));
 233 }
 234
 235 /*
 236 0x08 Battery sensor
 237 Payload:
 238 uint16_t    Voltage ( mV * 100 )
 239 uint16_t    Current ( mA * 100 )
 240 uint24_t    Capacity ( mAh )
 241 uint8_t     Battery remaining ( percent )
 242 */
 243 static void crsfFrameBatterySensor(sbuf_t *dst)
 244 {
 245     // use sbufWrite since CRC does not include frame length
 246     sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 247     crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR);
 248     if (telemetryConfig()->report_cell_voltage) {
 249         crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10);
 250     } else {
 251         crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V
 252     }
 253     crsfSerialize16(dst, getAmperage() / 10);
 254     const uint8_t batteryRemainingPercentage = calculateBatteryPercentage();
 255     crsfSerialize8(dst, (getMAhDrawn() >> 16));
 256     crsfSerialize8(dst, (getMAhDrawn() >> 8));
 257     crsfSerialize8(dst, (uint8_t)getMAhDrawn());
 258     crsfSerialize8(dst, batteryRemainingPercentage);
 259 }
 260
 261 typedef enum {
 262     CRSF_ACTIVE_ANTENNA1 = 0,
 263     CRSF_ACTIVE_ANTENNA2 = 1
 264 } crsfActiveAntenna_e;
 265
 266 typedef enum {
 267     CRSF_RF_MODE_4_HZ = 0,
 268     CRSF_RF_MODE_50_HZ = 1,
 269     CRSF_RF_MODE_150_HZ = 2
 270 } crsrRfMode_e;
 271
 272 typedef enum {
 273     CRSF_RF_POWER_0_mW = 0,
 274     CRSF_RF_POWER_10_mW = 1,
 275     CRSF_RF_POWER_25_mW = 2,
 276     CRSF_RF_POWER_100_mW = 3,
 277     CRSF_RF_POWER_500_mW = 4,
 278     CRSF_RF_POWER_1000_mW = 5,
 279     CRSF_RF_POWER_2000_mW = 6,
 280     CRSF_RF_POWER_250_mW = 7
 281 } crsrRfPower_e;
 282
 283 /*
 284 0x1E Attitude
 285 Payload:
 286 int16_t     Pitch angle ( rad / 10000 )
 287 int16_t     Roll angle ( rad / 10000 )
 288 int16_t     Yaw angle ( rad / 10000 )
 289 */
 290
 291 // convert andgle in decidegree to radians/10000 with reducing angle to +/-180 degree range
 292 static int16_t decidegrees2Radians10000(int16_t angle_decidegree)
 293 {
 294     while (angle_decidegree > 1800) {
 295         angle_decidegree -= 3600;
 296     }
 297     while (angle_decidegree < -1800) {
 298         angle_decidegree += 3600;
 299     }
 300     return (int16_t)(RAD * 1000.0f * angle_decidegree);
 301 }
 302
 303 static void crsfFrameAttitude(sbuf_t *dst)
 304 {
 305      sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
 306      crsfSerialize8(dst, CRSF_FRAMETYPE_ATTITUDE);
 307      crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.pitch));
 308      crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.roll));
 309      crsfSerialize16(dst, decidegrees2Radians10000(attitude.values.yaw));
 310 }
 311
 312 /*
 313 0x21 Flight mode text based
 314 Payload:
 315 char[]      Flight mode ( Nullterminated string )
 316 */
 317 static void crsfFrameFlightMode(sbuf_t *dst)
 318 {
 319     // just do "OK" for the moment as a placeholder
 320     // write zero for frame length, since we don't know it yet
 321     uint8_t *lengthPtr = sbufPtr(dst);
 322     sbufWriteU8(dst, 0);
 323     crsfSerialize8(dst, CRSF_FRAMETYPE_FLIGHT_MODE);
 324
 325     // use same logic as OSD, so telemetry displays same flight text as OSD when armed
 326     const char *flightMode = "OK";
 327     if (ARMING_FLAG(ARMED)) {
 328         if (STATE(AIRMODE_ACTIVE)) {
 329             flightMode = "AIR";
 330         } else {
 331             flightMode = "ACRO";
 332         }
 333         if (FLIGHT_MODE(FAILSAFE_MODE)) {
 334             flightMode = "!FS!";
 335         } else if (IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE)) {
 336             flightMode = "HRST";
 337         } else if (FLIGHT_MODE(MANUAL_MODE)) {
 338             flightMode = "MANU";
 339         } else if (FLIGHT_MODE(NAV_RTH_MODE)) {
 340             flightMode = "RTH";
 341         } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) {
 342             flightMode = "HOLD";
 343         } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
 344             flightMode = "CRUZ";
 345         } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
 346             flightMode = "CRSH";
 347         } else if (FLIGHT_MODE(NAV_WP_MODE)) {
 348             flightMode = "WP";
 349         } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
 350             flightMode = "AH";
 351         } else if (FLIGHT_MODE(ANGLE_MODE)) {
 352             flightMode = "ANGL";
 353         } else if (FLIGHT_MODE(HORIZON_MODE)) {
 354             flightMode = "HOR";
 355         } else if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
 356             flightMode = "ANGH";
 357 #ifdef USE_FW_AUTOLAND
 358         } else if (FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 359             flightMode = "LAND";
 360 #endif
 361         }
 362 #ifdef USE_GPS
 363     } else if (feature(FEATURE_GPS) && navConfig()->general.flags.extra_arming_safety && (!STATE(GPS_FIX) || !STATE(GPS_FIX_HOME))) {
 364         flightMode = "WAIT"; // Waiting for GPS lock
 365 #endif
 366     } else if (isArmingDisabled()) {
 367         flightMode = "!ERR";
 368     }
 369
 370     crsfSerializeData(dst, (const uint8_t*)flightMode, strlen(flightMode));
 371     crsfSerialize8(dst, 0); // zero terminator for string
 372     // write in the length
 373     *lengthPtr = sbufPtr(dst) - lengthPtr;
 374 }
 375
 376 /*
 377 0x29 Device Info
 378 Payload:
 379 uint8_t     Destination
 380 uint8_t     Origin
 381 char[]      Device Name ( Null terminated string )
 382 uint32_t    Null Bytes
 383 uint32_t    Null Bytes
 384 uint32_t    Null Bytes
 385 uint8_t     255 (Max MSP Parameter)
 386 uint8_t     0x01 (Parameter version 1)
 387 */
 388 static void crsfFrameDeviceInfo(sbuf_t *dst)
 389 {
 390     char buff[30];
 391     tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, TARGET_BOARD_IDENTIFIER);
 392
 393     uint8_t *lengthPtr = sbufPtr(dst);
 394     sbufWriteU8(dst, 0);
 395     crsfSerialize8(dst, CRSF_FRAMETYPE_DEVICE_INFO);
 396     crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 397     crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 398     crsfSerializeData(dst, (const uint8_t*)buff, strlen(buff));
 399     crsfSerialize8(dst, 0); // zero terminator for string
 400         for (unsigned int ii=0; ii<12; ii++) {
 401         crsfSerialize8(dst, 0x00);
 402     }
 403     crsfSerialize8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
 404     crsfSerialize8(dst, CRSF_DEVICEINFO_VERSION);
 405     *lengthPtr = sbufPtr(dst) - lengthPtr;
 406 }
 407
 408 #define BV(x)  (1 << (x)) // bit value
 409
 410 // schedule array to decide how often each type of frame is sent
 411 typedef enum {
 412     CRSF_FRAME_START_INDEX = 0,
 413     CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX,
 414     CRSF_FRAME_BATTERY_SENSOR_INDEX,
 415     CRSF_FRAME_FLIGHT_MODE_INDEX,
 416     CRSF_FRAME_GPS_INDEX,
 417     CRSF_FRAME_VARIO_SENSOR_INDEX,
 418     CRSF_SCHEDULE_COUNT_MAX
 419 } crsfFrameTypeIndex_e;
 420
 421 static uint8_t crsfScheduleCount;
 422 static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
 423
 424 #if defined(USE_MSP_OVER_TELEMETRY)
 425
 426 static bool mspReplyPending;
 427
 428 void crsfScheduleMspResponse(void)
 429 {
 430     mspReplyPending = true;
 431 }
 432
 433 void crsfSendMspResponse(uint8_t *payload)
 434 {
 435     sbuf_t crsfPayloadBuf;
 436     sbuf_t *dst = &crsfPayloadBuf;
 437
 438     crsfInitializeFrame(dst);
 439     sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC);
 440     crsfSerialize8(dst, CRSF_FRAMETYPE_MSP_RESP);
 441     crsfSerialize8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
 442     crsfSerialize8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
 443     crsfSerializeData(dst, (const uint8_t*)payload, CRSF_FRAME_TX_MSP_FRAME_SIZE);
 444     crsfFinalize(dst);
 445 }
 446 #endif
 447
 448 static void processCrsf(void)
 449 {
 450     static uint8_t crsfScheduleIndex = 0;
 451     const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
 452
 453     sbuf_t crsfPayloadBuf;
 454     sbuf_t *dst = &crsfPayloadBuf;
 455
 456     if (currentSchedule & BV(CRSF_FRAME_ATTITUDE_INDEX)) {
 457         crsfInitializeFrame(dst);
 458         crsfFrameAttitude(dst);
 459         crsfFinalize(dst);
 460     }
 461     if (currentSchedule & BV(CRSF_FRAME_BATTERY_SENSOR_INDEX)) {
 462         crsfInitializeFrame(dst);
 463         crsfFrameBatterySensor(dst);
 464         crsfFinalize(dst);
 465     }
 466     if (currentSchedule & BV(CRSF_FRAME_FLIGHT_MODE_INDEX)) {
 467         crsfInitializeFrame(dst);
 468         crsfFrameFlightMode(dst);
 469         crsfFinalize(dst);
 470     }
 471 #ifdef USE_GPS
 472     if (currentSchedule & BV(CRSF_FRAME_GPS_INDEX)) {
 473         crsfInitializeFrame(dst);
 474         crsfFrameGps(dst);
 475         crsfFinalize(dst);
 476     }
 477 #endif
 478 #if defined(USE_BARO) || defined(USE_GPS)
 479     if (currentSchedule & BV(CRSF_FRAME_VARIO_SENSOR_INDEX)) {
 480         crsfInitializeFrame(dst);
 481         crsfFrameVarioSensor(dst);
 482         crsfFinalize(dst);
 483     }
 484 #endif
 485     crsfScheduleIndex = (crsfScheduleIndex + 1) % crsfScheduleCount;
 486 }
 487
 488 void crsfScheduleDeviceInfoResponse(void)
 489 {
 490     deviceInfoReplyPending = true;
 491 }
 492
 493 void initCrsfTelemetry(void)
 494 {
 495     // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
 496     // and feature is enabled, if so, set CRSF telemetry enabled
 497     crsfTelemetryEnabled = crsfRxIsActive();
 498
 499     deviceInfoReplyPending = false;
 500 #if defined(USE_MSP_OVER_TELEMETRY)
 501     mspReplyPending = false;
 502 #endif
 503
 504     int index = 0;
 505     crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX);
 506     crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
 507     crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
 508 #ifdef USE_GPS
 509     if (feature(FEATURE_GPS)) {
 510         crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
 511     }
 512 #endif
 513 #if defined(USE_BARO) || defined(USE_GPS)
 514     if (sensors(SENSOR_BARO) || (STATE(FIXED_WING_LEGACY) && feature(FEATURE_GPS))) {
 515         crsfSchedule[index++] = BV(CRSF_FRAME_VARIO_SENSOR_INDEX);
 516     }
 517 #endif
 518     crsfScheduleCount = (uint8_t)index;
 519 }
 520
 521 bool checkCrsfTelemetryState(void)
 522 {
 523     return crsfTelemetryEnabled;
 524 }
 525
 526 /*
 527  * Called periodically by the scheduler
 528  */
 529 void handleCrsfTelemetry(timeUs_t currentTimeUs)
 530 {
 531     static uint32_t crsfLastCycleTime;
 532
 533     if (!crsfTelemetryEnabled) {
 534         return;
 535     }
 536     // Give the receiver a chance to send any outstanding telemetry data.
 537     // This needs to be done at high frequency, to enable the RX to send the telemetry frame
 538     // in between the RX frames.
 539     crsfRxSendTelemetryData();
 540
 541     // Send ad-hoc response frames as soon as possible
 542 #if defined(USE_MSP_OVER_TELEMETRY)
 543     if (mspReplyPending) {
 544         mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
 545         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 546         return;
 547     }
 548 #endif
 549
 550     if (deviceInfoReplyPending) {
 551         sbuf_t crsfPayloadBuf;
 552         sbuf_t *dst = &crsfPayloadBuf;
 553         crsfInitializeFrame(dst);
 554         crsfFrameDeviceInfo(dst);
 555         crsfFinalize(dst);
 556         deviceInfoReplyPending = false;
 557         crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
 558         return;
 559     }
 560
 561     // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
 562     // Spread out scheduled frames evenly so each frame is sent at the same frequency.
 563     if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) {
 564         crsfLastCycleTime = currentTimeUs;
 565         processCrsf();
 566     }
 567 }
 568
 569 int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType)
 570 {
 571     sbuf_t crsfFrameBuf;
 572     sbuf_t *sbuf = &crsfFrameBuf;
 573
 574     crsfInitializeFrame(sbuf);
 575     switch (frameType) {
 576     default:
 577     case CRSF_FRAMETYPE_ATTITUDE:
 578         crsfFrameAttitude(sbuf);
 579         break;
 580     case CRSF_FRAMETYPE_BATTERY_SENSOR:
 581         crsfFrameBatterySensor(sbuf);
 582         break;
 583     case CRSF_FRAMETYPE_FLIGHT_MODE:
 584         crsfFrameFlightMode(sbuf);
 585         break;
 586 #if defined(USE_GPS)
 587     case CRSF_FRAMETYPE_GPS:
 588         crsfFrameGps(sbuf);
 589         break;
 590 #endif
 591     case CRSF_FRAMETYPE_VARIO_SENSOR:
 592         crsfFrameVarioSensor(sbuf);
 593         break;
 594     }
 595     const int frameSize = crsfFinalizeBuf(sbuf, frame);
 596     return frameSize;
 597 }
 598 #endif