src/main/telemetry/srxl.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 <math.h>
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #if defined(USE_TELEMETRY_SRXL)
  30
  31 #include "build/version.h"
  32
  33 #include "cms/cms.h"
  34
  35 #include "common/crc.h"
  36 #include "common/streambuf.h"
  37 #include "common/utils.h"
  38
  39 #include "config/config.h"
  40 #include "config/feature.h"
  41
  42 #include "drivers/dshot.h"
  43 #include "drivers/vtx_common.h"
  44
  45 #include "fc/rc_controls.h"
  46 #include "fc/runtime_config.h"
  47
  48 #include "flight/imu.h"
  49 #include "flight/mixer.h"
  50
  51 #include "io/displayport_srxl.h"
  52 #include "io/gps.h"
  53 #include "io/serial.h"
  54 #include "io/vtx_smartaudio.h"
  55 #include "io/vtx_tramp.h"
  56
  57 #include "pg/rx.h"
  58 #include "pg/motor.h"
  59
  60 #include "rx/rx.h"
  61 #include "rx/spektrum.h"
  62 #include "io/spektrum_vtx_control.h"
  63 #include "rx/srxl2.h"
  64
  65 #include "sensors/adcinternal.h"
  66 #include "sensors/battery.h"
  67 #include "sensors/esc_sensor.h"
  68
  69 #include "telemetry/telemetry.h"
  70
  71 #include "srxl.h"
  72
  73 #define SRXL_ADDRESS_FIRST          0xA5
  74 #define SRXL_ADDRESS_SECOND         0x80
  75 #define SRXL_PACKET_LENGTH          0x15
  76
  77 #define SRXL_FRAMETYPE_TELE_QOS     0x7F
  78 #define SRXL_FRAMETYPE_TELE_RPM     0x7E
  79 #define SRXL_FRAMETYPE_POWERBOX     0x0A
  80 #define SRXL_FRAMETYPE_TELE_FP_MAH  0x34
  81 #define TELE_DEVICE_VTX             0x0D   // Video Transmitter Status
  82 #define SRXL_FRAMETYPE_SID          0x00
  83 #define SRXL_FRAMETYPE_GPS_LOC      0x16   // GPS Location Data (Eagle Tree)
  84 #define SRXL_FRAMETYPE_GPS_STAT     0x17
  85
  86 static bool srxlTelemetryEnabled;
  87 static bool srxl2 = false;
  88 static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
  89
  90 static void srxlInitializeFrame(sbuf_t *dst)
  91 {
  92     if (srxl2) {
  93 #if defined(USE_SERIALRX_SRXL2)
  94       srxl2InitializeFrame(dst);
  95 #endif
  96     } else {
  97         dst->ptr = srxlFrame;
  98         dst->end = ARRAYEND(srxlFrame);
  99
 100         sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
 101         sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
 102         sbufWriteU8(dst, SRXL_PACKET_LENGTH);
 103     }
 104 }
 105
 106 static void srxlFinalize(sbuf_t *dst)
 107 {
 108     if (srxl2) {
 109 #if defined(USE_SERIALRX_SRXL2)
 110       srxl2FinalizeFrame(dst);
 111 #endif
 112     } else {
 113         crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
 114         sbufSwitchToReader(dst, srxlFrame);
 115         // write the telemetry frame to the receiver.
 116         srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
 117     }
 118 }
 119
 120 /*
 121 SRXL frame has the structure:
 122 <0xA5><0x80><Length><16-byte telemetry packet><2 Byte CRC of payload>
 123 The <Length> shall be 0x15 (length of the 16-byte telemetry packet + overhead).
 124 */
 125
 126 /*
 127 typedef struct
 128 {
 129     UINT8 identifier; // Source device = 0x7F
 130     UINT8 sID; // Secondary ID
 131     UINT16 A;
 132     UINT16 B;
 133     UINT16 L;
 134     UINT16 R;
 135     UINT16 F;
 136     UINT16 H;
 137     UINT16 rxVoltage; // Volts, 0.01V increments
 138 } STRU_TELE_QOS;
 139 */
 140
 141 #define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14
 142 #define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff
 143
 144 bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
 145 {
 146     UNUSED(currentTimeUs);
 147
 148     sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
 149     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 150
 151     sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder
 152
 153     // Mandatory frame, send it unconditionally.
 154     return true;
 155 }
 156
 157 /*
 158 typedef struct
 159 {
 160     UINT8 identifier; // Source device = 0x7E
 161     UINT8 sID; // Secondary ID
 162     UINT16 microseconds; // microseconds between pulse leading edges
 163     UINT16 volts; // 0.01V increments
 164     INT16 temperature; // degrees F
 165     INT8 dBm_A, // Average signal for A antenna in dBm
 166     INT8 dBm_B; // Average signal for B antenna in dBm.
 167     // If only 1 antenna, set B = A
 168     UINT16 spare[2];
 169     UINT16 fastbootUptime; // bit 15 = fastboot flag.  Bits 0-14= uptime in seconds.  0x0000 --> no data
 170 } STRU_TELE_RPM;
 171 */
 172
 173 #define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8
 174 #define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0x00
 175
 176 #define SPEKTRUM_RPM_UNUSED 0xffff
 177 #define SPEKTRUM_TEMP_UNUSED 0x7fff
 178 #define MICROSEC_PER_MINUTE 60000000
 179
 180 //Original range of 1 - 65534 uSec gives an RPM range of 915 - 60000000rpm, 60MegaRPM
 181 #define SPEKTRUM_MIN_RPM 999      // Min RPM to show the user, indicating RPM is really below 999
 182 #define SPEKTRUM_MAX_RPM 60000000
 183
 184 uint16_t getMotorAveragePeriod(void)
 185 {
 186
 187 #if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY)
 188     uint32_t rpm = 0;
 189     uint16_t period_us = SPEKTRUM_RPM_UNUSED;
 190
 191 #if defined( USE_ESC_SENSOR_TELEMETRY)
 192     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
 193     if (escData != NULL) {
 194         rpm = escData->rpm;
 195     }
 196 #endif
 197
 198 #if defined(USE_DSHOT_TELEMETRY)
 199     // Calculate this way when no rpm from esc data
 200     if (useDshotTelemetry && rpm == 0) {
 201         rpm = lrintf(getDshotRpmAverage());
 202     }
 203 #endif
 204
 205     if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) {
 206         period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds
 207     } else {
 208         period_us = MICROSEC_PER_MINUTE / SPEKTRUM_MIN_RPM;
 209     }
 210
 211     return period_us;
 212 #else
 213     return SPEKTRUM_RPM_UNUSED;
 214 #endif
 215 }
 216
 217 bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
 218 {
 219     int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
 220 #if defined(USE_ADC_INTERNAL)
 221     coreTemp = getCoreTemperatureCelsius();
 222     coreTemp = coreTemp * 9 / 5 + 32; // C -> F
 223 #endif
 224
 225     UNUSED(currentTimeUs);
 226
 227     sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
 228     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 229     sbufWriteU16BigEndian(dst, getMotorAveragePeriod());    // pulse leading edges
 230     if (telemetryConfig()->report_cell_voltage) {
 231         sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
 232     } else {
 233         sbufWriteU16BigEndian(dst, getBatteryVoltage());   // vbat is in units of 0.01V
 234     }
 235     sbufWriteU16BigEndian(dst, coreTemp);                   // temperature
 236     sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
 237
 238     // Mandatory frame, send it unconditionally.
 239     return true;
 240 }
 241
 242 #if defined(USE_GPS)
 243
 244 // From Frsky implementation
 245 static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
 246 {
 247     int32_t absgps, deg, min;
 248     absgps = abs(mwiigps);
 249     deg = absgps / GPS_DEGREES_DIVIDER;
 250     absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;     // absgps = Minutes left * 10^7
 251     min = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
 252     result->dddmm = deg * 100 + min;
 253     result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
 254 }
 255
 256 // BCD conversion
 257 static uint32_t dec2bcd(uint16_t dec)
 258 {
 259     uint32_t result = 0;
 260     uint8_t counter = 0;
 261
 262     while (dec) {
 263         result |= (dec % 10) << counter * 4;
 264         counter++;
 265         dec /= 10;
 266     }
 267     return result;
 268 }
 269
 270 /*
 271 typedef struct
 272 {
 273     UINT8    identifier;    // Source device = 0x16
 274     UINT8    sID;           // Secondary ID
 275     UINT16   altitudeLow;   // BCD, meters, format 3.1 (Low order of altitude)
 276     UINT32   latitude;      // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
 277     UINT32   longitude;     // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
 278     UINT16   course;        // BCD, 3.1
 279     UINT8    PDOP;          // BCD, format 1.1
 280     UINT8    GPSflags;      // see definitions below
 281 } STRU_TELE_GPS_LOC;
 282 */
 283
 284 // GPS flags definitions
 285 #define GPS_FLAGS_IS_NORTH_BIT              0x01
 286 #define GPS_FLAGS_IS_EAST_BIT               0x02
 287 #define GPS_FLAGS_LONGITUDE_GREATER_99_BIT  0x04
 288 #define GPS_FLAGS_GPS_FIX_VALID_BIT         0x08
 289 #define GPS_FLAGS_GPS_DATA_RECEIVED_BIT     0x10
 290 #define GPS_FLAGS_3D_FIX_BIT                0x20
 291 #define GPS_FLAGS_NEGATIVE_ALT_BIT          0x80
 292
 293 bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
 294 {
 295     UNUSED(currentTimeUs);
 296     gpsCoordinateDDDMMmmmm_t coordinate;
 297     uint32_t latitudeBcd, longitudeBcd, altitudeLo;
 298     uint16_t altitudeLoBcd, groundCourseBcd, pdop;
 299     uint8_t pdopBcd, gpsFlags;
 300
 301     if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
 302         return false;
 303     }
 304
 305     // lattitude
 306     GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate);
 307     latitudeBcd  = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
 308
 309     // longitude
 310     GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate);
 311     longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
 312
 313     // altitude (low order)
 314     altitudeLo = abs(gpsSol.llh.altCm) / 10;
 315     altitudeLoBcd = dec2bcd(altitudeLo % 100000);
 316
 317     // Ground course
 318     groundCourseBcd = dec2bcd(gpsSol.groundCourse);
 319
 320     // PDOP
 321     pdop = gpsSol.dop.pdop / 10;
 322     pdop = (pdop > 99) ? 99 : pdop;
 323     pdopBcd = dec2bcd(pdop);
 324
 325     // flags
 326     gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT;
 327     gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0;
 328     gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0;
 329     gpsFlags |= (gpsSol.llh.altCm < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0;
 330     gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0;
 331
 332     // SRXL frame
 333     sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC);
 334     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 335     sbufWriteU16(dst, altitudeLoBcd);
 336     sbufWriteU32(dst, latitudeBcd);
 337     sbufWriteU32(dst, longitudeBcd);
 338     sbufWriteU16(dst, groundCourseBcd);
 339     sbufWriteU8(dst, pdopBcd);
 340     sbufWriteU8(dst, gpsFlags);
 341
 342     return true;
 343 }
 344
 345 /*
 346 typedef struct
 347 {
 348    UINT8   identifier;                      // Source device = 0x17
 349    UINT8   sID;                             // Secondary ID
 350    UINT16  speed;                           // BCD, knots, format 3.1
 351    UINT32  UTC;                             // BCD, format HH:MM:SS.S, format 6.1
 352    UINT8   numSats;                         // BCD, 0-99
 353    UINT8   altitudeHigh;                    // BCD, meters, format 2.0 (High bits alt)
 354 } STRU_TELE_GPS_STAT;
 355 */
 356
 357 #define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff
 358 #define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6
 359 #define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF
 360
 361 bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
 362 {
 363     UNUSED(currentTimeUs);
 364     uint32_t timeBcd;
 365     uint16_t speedKnotsBcd, speedTmp;
 366     uint8_t numSatBcd, altitudeHighBcd;
 367     bool timeProvided = false;
 368
 369     if (!featureIsEnabled(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < GPS_MIN_SAT_COUNT) {
 370         return false;
 371     }
 372
 373     // Number of sats and altitude (high bits)
 374     numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat);
 375     altitudeHighBcd = dec2bcd(gpsSol.llh.altCm / 100000);
 376
 377     // Speed (knots)
 378     speedTmp = gpsSol.groundSpeed * 1944 / 1000;
 379     speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp);
 380
 381 #ifdef USE_RTC_TIME
 382     dateTime_t dt;
 383     // RTC
 384     if (rtcHasTime()) {
 385         rtcGetDateTime(&dt);
 386         timeBcd = dec2bcd(dt.hours);
 387         timeBcd = timeBcd << 8;
 388         timeBcd = timeBcd | dec2bcd(dt.minutes);
 389         timeBcd = timeBcd << 8;
 390         timeBcd = timeBcd | dec2bcd(dt.seconds);
 391         timeBcd = timeBcd << 4;
 392         timeBcd = timeBcd | dec2bcd(dt.millis / 100);
 393         timeProvided = true;
 394     }
 395 #endif
 396     timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN;
 397
 398     // SRXL frame
 399     sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT);
 400     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 401     sbufWriteU16(dst, speedKnotsBcd);
 402     sbufWriteU32(dst, timeBcd);
 403     sbufWriteU8(dst, numSatBcd);
 404     sbufWriteU8(dst, altitudeHighBcd);
 405     sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT);
 406
 407     return true;
 408 }
 409
 410 #endif
 411
 412 /*
 413 typedef struct
 414 {
 415     UINT8   identifier;     // Source device = 0x34
 416     UINT8   sID;            // Secondary ID
 417     INT16   current_A;      // Instantaneous current, 0.1A (0-3276.8A)
 418     INT16   chargeUsed_A;   // Integrated mAh used, 1mAh (0-32.766Ah)
 419     UINT16  temp_A;         // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
 420     INT16   current_B;      // Instantaneous current, 0.1A (0-3276.8A)
 421     INT16   chargeUsed_B;   // Integrated mAh used, 1mAh (0-32.766Ah)
 422     UINT16  temp_B;         // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
 423     UINT16  spare;          // Not used
 424 } STRU_TELE_FP_MAH;
 425 */
 426 #define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2
 427 #define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff
 428
 429 #define SPEKTRUM_AMPS_UNUSED 0x7fff
 430 #define SPEKTRUM_AMPH_UNUSED 0x7fff
 431
 432 #define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
 433
 434 bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
 435 {
 436     uint16_t amps = getAmperage() / 10;
 437     uint16_t mah  = getMAhDrawn();
 438     static uint16_t sentAmps;
 439     static uint16_t sentMah;
 440     static timeUs_t lastTimeSentFPmAh = 0;
 441
 442     timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
 443
 444     if ( amps != sentAmps ||
 445          mah != sentMah ||
 446          keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
 447
 448         sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
 449         sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 450         sbufWriteU16(dst, amps);
 451         sbufWriteU16(dst, mah);
 452         sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED);            // temp A
 453         sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED);            // Amps B
 454         sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED);            // mAH B
 455         sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED);            // temp B
 456
 457         sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT);
 458
 459         sentAmps = amps;
 460         sentMah = mah;
 461         lastTimeSentFPmAh = currentTimeUs;
 462         return true;
 463     }
 464     return false;
 465 }
 466
 467 #if defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_CMS)
 468
 469 // Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
 470
 471 #define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C)     // Text Generator
 472 #define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9)   // Text Generator ROWS
 473 #define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13)  // Text Generator COLS
 474
 475 /*
 476 typedef struct
 477 {
 478     UINT8       identifier;
 479     UINT8       sID;               // Secondary ID
 480     UINT8       lineNumber;        // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
 481     char        text[13];          // 0-terminated text when < 13 chars
 482 } STRU_SPEKTRUM_SRXL_TEXTGEN;
 483 */
 484
 485 #if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
 486 static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
 487 static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
 488 #else
 489 static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
 490 static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
 491 #endif
 492
 493 //**************************************************************************
 494 // API Running in external client task context. E.g. in the CMS task
 495 int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
 496 {
 497     if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
 498       // Only update and force a tm transmision if something has actually changed.
 499         if (srxlTextBuff[row][col] != c) {
 500           srxlTextBuff[row][col] = c;
 501           lineSent[row] = false;
 502         }
 503     }
 504     return 0;
 505 }
 506 //**************************************************************************
 507
 508 bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
 509 {
 510     UNUSED(currentTimeUs);
 511     static uint8_t lineNo = 0;
 512     int lineCount = 0;
 513
 514     // Skip already sent lines...
 515     while (lineSent[lineNo] &&
 516            lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
 517         lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
 518         lineCount++;
 519     }
 520
 521     sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
 522     sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 523     sbufWriteU8(dst, lineNo);
 524     sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
 525
 526     lineSent[lineNo] = true;
 527     lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
 528
 529     // Always send something, Always one user frame after the two mandatory frames
 530     // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
 531     // too keep the "Waltz" sequence intact.
 532     return true;
 533 }
 534 #endif
 535
 536 #if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
 537
 538 static uint8_t vtxDeviceType;
 539
 540 static void collectVtxTmData(spektrumVtx_t * vtx)
 541 {
 542     const vtxDevice_t *vtxDevice = vtxCommonDevice();
 543     vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
 544
 545     // Collect all data from VTX, if VTX is ready
 546     unsigned vtxStatus;
 547     if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
 548            vtxCommonGetStatus(vtxDevice, &vtxStatus) &&
 549            vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
 550         {
 551             vtx->band    = 0;
 552             vtx->channel = 0;
 553             vtx->power   = 0;
 554             vtx->pitMode = 0;
 555         } else {
 556             vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0;
 557         }
 558
 559     vtx->powerValue = 0;
 560 #ifdef USE_SPEKTRUM_REGION_CODES
 561     vtx->region = SpektrumRegion;
 562 #else
 563     vtx->region = SPEKTRUM_VTX_REGION_NONE;
 564 #endif
 565 }
 566
 567 // Reverse lookup, device power index to Spektrum power range index.
 568 static void convertVtxPower(spektrumVtx_t * vtx)
 569     {
 570         uint8_t const * powerIndexTable = NULL;
 571
 572         vtxCommonLookupPowerValue(vtxCommonDevice(), vtx->power, &vtx->powerValue);
 573         switch (vtxDeviceType) {
 574
 575 #if defined(USE_VTX_TRAMP)
 576         case VTXDEV_TRAMP:
 577             powerIndexTable = vtxTrampPi;
 578             break;
 579 #endif
 580 #if defined(USE_VTX_SMARTAUDIO)
 581         case VTXDEV_SMARTAUDIO:
 582             powerIndexTable = vtxSaPi;
 583             break;
 584 #endif
 585 #if defined(USE_VTX_RTC6705)
 586         case VTXDEV_RTC6705:
 587             powerIndexTable = vtxRTC6705Pi;
 588             break;
 589 #endif
 590
 591         case VTXDEV_UNKNOWN:
 592         case VTXDEV_UNSUPPORTED:
 593         default:
 594           break;
 595
 596         }
 597
 598         if (powerIndexTable != NULL) {
 599             for (int i = 0; i < SPEKTRUM_VTX_POWER_COUNT; i++)
 600                 if (powerIndexTable[i] >= vtx->power) {
 601                     vtx->power = i;                                    // Translate device power index to Spektrum power index.
 602                     break;
 603                 }
 604         }
 605     }
 606
 607 static void convertVtxTmData(spektrumVtx_t * vtx)
 608 {
 609     // Convert from internal band indexes to Spektrum indexes
 610     for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) {
 611         if (spek2commonBand[i] == vtx->band) {
 612             vtx->band = i;
 613             break;
 614         }
 615     }
 616
 617     // De-bump channel no 1 based interally, 0-based in Spektrum.
 618     vtx->channel--;
 619
 620     // Convert Power index to Spektrum ranges, different per brand.
 621     convertVtxPower(vtx);
 622 }
 623
 624 /*
 625 typedef struct
 626 {
 627     UINT8       identifier;
 628     UINT8       sID;      // Secondary ID
 629     UINT8       band;     // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved)
 630     UINT8       channel;  // VTX Channel (0-7)
 631     UINT8       pit;      // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings
 632     UINT8       power;    // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
 633     UINT16      powerDec; // VTX Power as a decimal 1mw/unit
 634     UINT8       region;   // Region (0 = USA, 1 = EU, 0xFF = N/A)
 635     UINT8       rfu[7];   // reserved
 636 } STRU_TELE_VTX;
 637 */
 638
 639 #define STRU_TELE_VTX_EMPTY_COUNT 7
 640 #define STRU_TELE_VTX_EMPTY_VALUE 0xff
 641
 642 #define VTX_KEEPALIVE_TIME_OUT 2000000 // uS
 643
 644 static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs)
 645 {
 646     static timeUs_t lastTimeSentVtx = 0;
 647     static spektrumVtx_t vtxSent;
 648
 649     spektrumVtx_t vtx;
 650     collectVtxTmData(&vtx);
 651
 652     if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) {
 653         convertVtxTmData(&vtx);
 654
 655         if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) ||
 656             ((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) {
 657             // Fill in the VTX tm structure
 658             sbufWriteU8(dst, TELE_DEVICE_VTX);
 659             sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
 660             sbufWriteU8(dst,  vtx.band);
 661             sbufWriteU8(dst,  vtx.channel);
 662             sbufWriteU8(dst,  vtx.pitMode);
 663             sbufWriteU8(dst,  vtx.power);
 664             sbufWriteU16(dst, vtx.powerValue);
 665             sbufWriteU8(dst,  vtx.region);
 666
 667             sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT);
 668
 669             memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t));
 670             lastTimeSentVtx = currentTimeUs;
 671             return true;
 672         }
 673     }
 674     return false;
 675 }
 676 #endif // USE_SPEKTRUM_VTX_TELEMETRY && USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON
 677
 678 // Schedule array to decide how often each type of frame is sent
 679 // The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
 680 // The user frame type is cycled for each set.
 681 // Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
 682
 683 #define SRXL_SCHEDULE_MANDATORY_COUNT  2 // Mandatory QOS and RPM sensors
 684
 685 #define SRXL_FP_MAH_COUNT   1
 686
 687 #if defined(USE_GPS)
 688 #define SRXL_GPS_LOC_COUNT  1
 689 #define SRXL_GPS_STAT_COUNT 1
 690 #else
 691 #define SRXL_GPS_LOC_COUNT  0
 692 #define SRXL_GPS_STAT_COUNT 0
 693 #endif
 694
 695 #if defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_CMS)
 696 #define SRXL_SCHEDULE_CMS_COUNT  1
 697 #else
 698 #define SRXL_SCHEDULE_CMS_COUNT  0
 699 #endif
 700
 701 #if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
 702 #define SRXL_VTX_TM_COUNT        1
 703 #else
 704 #define SRXL_VTX_TM_COUNT        0
 705 #endif
 706
 707 #define SRXL_SCHEDULE_USER_COUNT (SRXL_FP_MAH_COUNT + SRXL_SCHEDULE_CMS_COUNT + SRXL_VTX_TM_COUNT + SRXL_GPS_LOC_COUNT + SRXL_GPS_STAT_COUNT)
 708 #define SRXL_SCHEDULE_COUNT_MAX  (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
 709 #define SRXL_TOTAL_COUNT         (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
 710
 711 typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
 712
 713 const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
 714     /* must send srxlFrameQos, Rpm and then alternating items of our own */
 715     srxlFrameQos,
 716     srxlFrameRpm,
 717     srxlFrameFlightPackCurrent,
 718 #if defined(USE_GPS)
 719     srxlFrameGpsStat,
 720     srxlFrameGpsLoc,
 721 #endif
 722 #if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
 723     srxlFrameVTX,
 724 #endif
 725 #if defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_CMS)
 726     srxlFrameText,
 727 #endif
 728 };
 729
 730 static void processSrxl(timeUs_t currentTimeUs)
 731 {
 732     static uint8_t srxlScheduleIndex = 0;
 733     static uint8_t srxlScheduleUserIndex = 0;
 734
 735     sbuf_t srxlPayloadBuf;
 736     sbuf_t *dst = &srxlPayloadBuf;
 737     srxlScheduleFnPtr srxlFnPtr;
 738
 739     if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
 740         srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
 741     } else {
 742         srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
 743         srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
 744
 745 #if defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_CMS)
 746         // Boost CMS performance by sending nothing else but CMS Text frames when in a CMS menu.
 747         // Sideeffect, all other reports are still not sent if user leaves CMS without a proper EXIT.
 748         if (cmsInMenu &&
 749             (pCurrentDisplay == &srxlDisplayPort)) {
 750             srxlFnPtr = srxlFrameText;
 751         }
 752 #endif
 753
 754     }
 755
 756     if (srxlFnPtr) {
 757         srxlInitializeFrame(dst);
 758         if (srxlFnPtr(dst, currentTimeUs)) {
 759             srxlFinalize(dst);
 760         }
 761     }
 762     srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
 763 }
 764
 765 void initSrxlTelemetry(void)
 766 {
 767     // check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
 768     // and feature is enabled, if so, set SRXL telemetry enabled
 769     if (srxlRxIsActive()) {
 770         srxlTelemetryEnabled = true;
 771         srxl2 = false;
 772 #if defined(USE_SERIALRX_SRXL2)
 773     } else if (srxl2RxIsActive()) {
 774         srxlTelemetryEnabled = true;
 775         srxl2 = true;
 776 #endif
 777     } else {
 778         srxlTelemetryEnabled = false;
 779         srxl2 = false;
 780     }
 781
 782 #if defined(USE_SPEKTRUM_CMS_TELEMETRY)
 783     if (srxlTelemetryEnabled) {
 784         srxlDisplayportRegister();
 785     }
 786 #endif
 787  }
 788
 789 bool checkSrxlTelemetryState(void)
 790 {
 791     return srxlTelemetryEnabled;
 792 }
 793
 794 /*
 795  * Called periodically by the scheduler
 796  */
 797 void handleSrxlTelemetry(timeUs_t currentTimeUs)
 798 {
 799   if (srxl2) {
 800 #if defined(USE_SERIALRX_SRXL2)
 801       if (srxl2TelemetryRequested()) {
 802           processSrxl(currentTimeUs);
 803       }
 804 #endif
 805   } else {
 806       if (srxlTelemetryBufferEmpty()) {
 807           processSrxl(currentTimeUs);
 808       }
 809   }
 810 }
 811 #endif