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