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