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