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