src/main/telemetry/crsf.c

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