src/main/telemetry/frsky_hub.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 /*
  22  * Initial FrSky Hub Telemetry implementation by silpstream @ rcgroups.
  23  * Addition protocol work by airmamaf @ github.
  24  */
  25
  26 #include <stddef.h>
  27 #include <stdbool.h>
  28 #include <stdint.h>
  29
  30 #include "platform.h"
  31
  32 #if defined(USE_TELEMETRY_FRSKY_HUB)
  33
  34 #include "common/maths.h"
  35 #include "common/axis.h"
  36 #include "common/utils.h"
  37
  38 #include "config/feature.h"
  39 #include "pg/pg.h"
  40 #include "pg/pg_ids.h"
  41 #include "pg/rx.h"
  42
  43 #include "drivers/accgyro/accgyro.h"
  44 #include "drivers/sensor.h"
  45 #include "drivers/serial.h"
  46 #include "drivers/time.h"
  47
  48 #include "config/config.h"
  49 #include "fc/rc_controls.h"
  50 #include "fc/runtime_config.h"
  51
  52 #include "sensors/sensors.h"
  53 #include "sensors/acceleration.h"
  54 #include "sensors/gyro.h"
  55 #include "sensors/barometer.h"
  56 #include "sensors/battery.h"
  57
  58 #include "io/serial.h"
  59 #include "io/gps.h"
  60
  61 #include "flight/mixer.h"
  62 #include "flight/pid.h"
  63 #include "flight/imu.h"
  64 #include "flight/position.h"
  65
  66 #include "rx/rx.h"
  67
  68 #include "telemetry/telemetry.h"
  69
  70 #if defined(USE_ESC_SENSOR_TELEMETRY)
  71 #include "sensors/esc_sensor.h"
  72 #endif
  73
  74 #include "frsky_hub.h"
  75
  76 static serialPort_t *frSkyHubPort = NULL;
  77 static const serialPortConfig_t *portConfig = NULL;
  78
  79 #define FRSKY_HUB_BAUDRATE 9600
  80 #define FRSKY_HUB_INITIAL_PORT_MODE MODE_TX
  81
  82 static portSharing_e frSkyHubPortSharing;
  83
  84 static frSkyHubWriteByteFn *frSkyHubWriteByte = NULL;
  85
  86 #define FRSKY_HUB_CYCLETIME_US    125000
  87
  88 #define PROTOCOL_HEADER       0x5E
  89 #define PROTOCOL_TAIL         0x5E
  90
  91 // Data Ids  (bp = before decimal point; af = after decimal point)
  92 // Official data IDs
  93 #define ID_GPS_ALTIDUTE_BP    0x01
  94 #define ID_GPS_ALTIDUTE_AP    0x09
  95 #define ID_TEMPRATURE1        0x02
  96 #define ID_RPM                0x03
  97 #define ID_FUEL_LEVEL         0x04
  98 #define ID_TEMPRATURE2        0x05
  99 #define ID_VOLT               0x06
 100 #define ID_ALTITUDE_BP        0x10
 101 #define ID_ALTITUDE_AP        0x21
 102 #define ID_GPS_SPEED_BP       0x11
 103 #define ID_GPS_SPEED_AP       0x19
 104 #define ID_LONGITUDE_BP       0x12
 105 #define ID_LONGITUDE_AP       0x1A
 106 #define ID_E_W                0x22
 107 #define ID_LATITUDE_BP        0x13
 108 #define ID_LATITUDE_AP        0x1B
 109 #define ID_N_S                0x23
 110 #define ID_COURSE_BP          0x14
 111 #define ID_COURSE_AP          0x1C
 112 #define ID_DATE_MONTH         0x15
 113 #define ID_YEAR               0x16
 114 #define ID_HOUR_MINUTE        0x17
 115 #define ID_SECOND             0x18
 116 #define ID_ACC_X              0x24
 117 #define ID_ACC_Y              0x25
 118 #define ID_ACC_Z              0x26
 119 #define ID_VOLTAGE_AMP        0x39
 120 #define ID_VOLTAGE_AMP_BP     0x3A
 121 #define ID_VOLTAGE_AMP_AP     0x3B
 122 #define ID_CURRENT            0x28
 123 // User defined data IDs
 124 #define ID_GYRO_X             0x40
 125 #define ID_GYRO_Y             0x41
 126 #define ID_GYRO_Z             0x42
 127
 128 #define ID_VERT_SPEED         0x30 // opentx vario
 129
 130 #define GPS_BAD_QUALITY       300
 131 #define GPS_MAX_HDOP_VAL      9999
 132 #define DELAY_FOR_BARO_INITIALISATION_US 5000000
 133 #define BLADE_NUMBER_DIVIDER  5 // should set 12 blades in Taranis
 134
 135 enum
 136 {
 137     TELEMETRY_STATE_UNINITIALIZED,
 138     TELEMETRY_STATE_INITIALIZED_SERIAL,
 139     TELEMETRY_STATE_INITIALIZED_EXTERNAL,
 140 };
 141
 142 static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
 143
 144 static void serializeFrSkyHub(uint8_t data)
 145 {
 146     // take care of byte stuffing
 147     if (data == 0x5e) {
 148         frSkyHubWriteByte(0x5d);
 149         frSkyHubWriteByte(0x3e);
 150     } else if (data == 0x5d) {
 151         frSkyHubWriteByte(0x5d);
 152         frSkyHubWriteByte(0x3d);
 153     } else{
 154         frSkyHubWriteByte(data);
 155     }
 156 }
 157
 158 static void frSkyHubWriteFrame(const uint8_t id, const int16_t data)
 159 {
 160     frSkyHubWriteByte(PROTOCOL_HEADER);
 161     frSkyHubWriteByte(id);
 162
 163     serializeFrSkyHub((uint8_t)data);
 164     serializeFrSkyHub(data >> 8);
 165 }
 166
 167 static void sendTelemetryTail(void)
 168 {
 169     frSkyHubWriteByte(PROTOCOL_TAIL);
 170 }
 171
 172 static void frSkyHubWriteByteInternal(const char data)
 173  {
 174    serialWrite(frSkyHubPort, data);
 175  }
 176
 177 #if defined(USE_ACC)
 178 static void sendAccel(void)
 179 {
 180     for (unsigned i = 0; i < 3; i++) {
 181         frSkyHubWriteFrame(ID_ACC_X + i, ((int16_t)(acc.accADC[i] * acc.dev.acc_1G_rec) * 1000));
 182     }
 183 }
 184 #endif
 185
 186 static void sendThrottleOrBatterySizeAsRpm(void)
 187 {
 188     int16_t data = 0;
 189 #if defined(USE_ESC_SENSOR_TELEMETRY)
 190     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
 191     if (escData) {
 192         data = escData->dataAge < ESC_DATA_INVALID ? (calcEscRpm(escData->rpm) / 10) : 0;
 193     }
 194 #else
 195     if (ARMING_FLAG(ARMED)) {
 196         const throttleStatus_e throttleStatus = calculateThrottleStatus();
 197         uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER;
 198         if (throttleStatus == THROTTLE_LOW && featureIsEnabled(FEATURE_MOTOR_STOP)) {
 199             throttleForRPM = 0;
 200         }
 201         data = throttleForRPM;
 202     } else {
 203         data = (batteryConfig()->batteryCapacity / BLADE_NUMBER_DIVIDER);
 204     }
 205 #endif
 206
 207     frSkyHubWriteFrame(ID_RPM, data);
 208 }
 209
 210 static void sendTemperature1(void)
 211 {
 212     int16_t data = 0;
 213 #if defined(USE_ESC_SENSOR_TELEMETRY)
 214     escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
 215     if (escData) {
 216         data = escData->dataAge < ESC_DATA_INVALID ? escData->temperature : 0;
 217     }
 218 #elif defined(USE_BARO)
 219     data = (baro.baroTemperature + 50)/ 100; // Airmamaf
 220 #else
 221     data = gyroGetTemperature() / 10;
 222 #endif
 223     frSkyHubWriteFrame(ID_TEMPRATURE1, data);
 224 }
 225
 226 static void sendTime(void)
 227 {
 228     uint32_t seconds = millis() / 1000;
 229     uint8_t minutes = (seconds / 60) % 60;
 230
 231     // if we fly for more than an hour, something's wrong anyway
 232     frSkyHubWriteFrame(ID_HOUR_MINUTE, minutes << 8);
 233     frSkyHubWriteFrame(ID_SECOND, seconds % 60);
 234 }
 235
 236 #if defined(USE_GPS) || defined(USE_MAG)
 237 // Frsky pdf: dddmm.mmmm
 238 // .mmmm is returned in decimal fraction of minutes.
 239 static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
 240 {
 241     int32_t absgps, deg, min;
 242
 243     absgps = ABS(mwiigps);
 244     deg    = absgps / GPS_DEGREES_DIVIDER;
 245     absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;        // absgps = Minutes left * 10^7
 246     min    = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
 247
 248     if (telemetryConfig()->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
 249         result->dddmm = deg * 100 + min;
 250     } else {
 251         result->dddmm = deg * 60 + min;
 252     }
 253
 254     result->mmmm  = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
 255 }
 256
 257 static void sendLatLong(int32_t coord[2])
 258 {
 259     gpsCoordinateDDDMMmmmm_t coordinate;
 260     GPStoDDDMM_MMMM(coord[GPS_LATITUDE], &coordinate);
 261     frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm);
 262     frSkyHubWriteFrame(ID_LATITUDE_AP, coordinate.mmmm);
 263     frSkyHubWriteFrame(ID_N_S, coord[GPS_LATITUDE] < 0 ? 'S' : 'N');
 264
 265     GPStoDDDMM_MMMM(coord[GPS_LONGITUDE], &coordinate);
 266     frSkyHubWriteFrame(ID_LONGITUDE_BP, coordinate.dddmm);
 267     frSkyHubWriteFrame(ID_LONGITUDE_AP, coordinate.mmmm);
 268     frSkyHubWriteFrame(ID_E_W, coord[GPS_LONGITUDE] < 0 ? 'W' : 'E');
 269 }
 270
 271 #if defined(USE_GPS)
 272 static void sendGpsAltitude(void)
 273 {
 274     int32_t altitudeCm = gpsSol.llh.altCm;
 275
 276     // Send real GPS altitude only if it's reliable (there's a GPS fix)
 277     if (!STATE(GPS_FIX)) {
 278         altitudeCm = 0;
 279     }
 280     frSkyHubWriteFrame(ID_GPS_ALTIDUTE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m
 281     frSkyHubWriteFrame(ID_GPS_ALTIDUTE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m
 282 }
 283
 284 static void sendSatalliteSignalQualityAsTemperature2(uint8_t cycleNum)
 285 {
 286     uint16_t satellite = gpsSol.numSat;
 287
 288     if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) { // Every 1s
 289         satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
 290     }
 291     int16_t data;
 292     if (telemetryConfig()->frsky_unit == UNIT_IMPERIAL) {
 293         float tmp = (satellite - 32) / 1.8f;
 294         // Round the value
 295         tmp += (tmp < 0) ? -0.5f : 0.5f;
 296         data = tmp;
 297     } else {
 298         data = satellite;
 299     }
 300     frSkyHubWriteFrame(ID_TEMPRATURE2, data);
 301 }
 302
 303 static void sendSpeed(void)
 304 {
 305     if (!STATE(GPS_FIX)) {
 306         return;
 307     }
 308     // Speed should be sent in knots (GPS speed is in cm/s)
 309     // convert to knots: 1cm/s = 0.0194384449 knots
 310     frSkyHubWriteFrame(ID_GPS_SPEED_BP, gpsSol.groundSpeed * 1944 / 100000);
 311     frSkyHubWriteFrame(ID_GPS_SPEED_AP, (gpsSol.groundSpeed * 1944 / 100) % 100);
 312 }
 313
 314 static void sendFakeLatLong(void)
 315 {
 316     // Heading is only displayed on OpenTX if non-zero lat/long is also sent
 317     int32_t coord[2] = {0,0};
 318
 319     coord[GPS_LATITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER);
 320     coord[GPS_LONGITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER);
 321
 322     sendLatLong(coord);
 323 }
 324
 325 static void sendGPSLatLong(void)
 326 {
 327     static uint8_t gpsFixOccured = 0;
 328     int32_t coord[2] = {0,0};
 329
 330     if (STATE(GPS_FIX) || gpsFixOccured == 1) {
 331         // If we have ever had a fix, send the last known lat/long
 332         gpsFixOccured = 1;
 333         coord[GPS_LATITUDE] = gpsSol.llh.lat;
 334         coord[GPS_LONGITUDE] = gpsSol.llh.lon;
 335         sendLatLong(coord);
 336     } else {
 337         // otherwise send fake lat/long in order to display compass value
 338         sendFakeLatLong();
 339     }
 340 }
 341
 342 #endif
 343 #endif
 344
 345 /*
 346  * Send voltage via ID_VOLT
 347  *
 348  * NOTE: This sends voltage divided by batteryCellCount. To get the real
 349  * battery voltage, you need to multiply the value by batteryCellCount.
 350  */
 351 static void sendVoltageCells(void)
 352 {
 353     static uint16_t currentCell;
 354     uint32_t cellVoltage = 0;
 355     const uint8_t cellCount = getBatteryCellCount();
 356
 357     if (cellCount) {
 358         currentCell %= cellCount;
 359         /*
 360         * Format for Voltage Data for single cells is like this:
 361         *
 362         *  llll llll cccc hhhh
 363         *  l: Low voltage bits
 364         *  h: High voltage bits
 365         *  c: Cell number (starting at 0)
 366         *
 367         * The actual value sent for cell voltage has resolution of 0.002 volts
 368         * Since vbat has resolution of 0.1 volts it has to be multiplied by 50
 369         */
 370         cellVoltage = ((uint32_t)getBatteryVoltage() * 100 + cellCount) / (cellCount * 2);
 371     } else {
 372         currentCell = 0;
 373     }
 374
 375     // Cell number is at bit 9-12
 376     uint16_t data = (currentCell << 4);
 377
 378     // Lower voltage bits are at bit 0-8
 379     data |= ((cellVoltage & 0x0ff) << 8);
 380
 381     // Higher voltage bits are at bits 13-15
 382     data |= ((cellVoltage & 0xf00) >> 8);
 383
 384     frSkyHubWriteFrame(ID_VOLT, data);
 385
 386     currentCell++;
 387 }
 388
 389 /*
 390  * Send voltage with ID_VOLTAGE_AMP
 391  */
 392 static void sendVoltageAmp(void)
 393 {
 394     uint16_t voltage = getLegacyBatteryVoltage();
 395     const uint8_t cellCount = getBatteryCellCount();
 396
 397     if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
 398         // Use new ID 0x39 to send voltage directly in 0.1 volts resolution
 399         if (telemetryConfig()->report_cell_voltage && cellCount) {
 400             voltage /= cellCount;
 401         }
 402         frSkyHubWriteFrame(ID_VOLTAGE_AMP, voltage);
 403     } else {
 404         // send in 0.2 volts resolution
 405         voltage *= 110 / 21;
 406         if (telemetryConfig()->report_cell_voltage && cellCount) {
 407             voltage /= cellCount;
 408         }
 409
 410         frSkyHubWriteFrame(ID_VOLTAGE_AMP_BP, voltage / 100);
 411         frSkyHubWriteFrame(ID_VOLTAGE_AMP_AP, ((voltage % 100) + 5) / 10);
 412     }
 413 }
 414
 415 static void sendAmperage(void)
 416 {
 417     frSkyHubWriteFrame(ID_CURRENT, (uint16_t)(getAmperage() / 10));
 418 }
 419
 420 static void sendFuelLevel(void)
 421 {
 422     int16_t data;
 423     if (batteryConfig()->batteryCapacity > 0) {
 424         data = (uint16_t)calculateBatteryPercentageRemaining();
 425     } else {
 426         data = (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF);
 427     }
 428     frSkyHubWriteFrame(ID_FUEL_LEVEL, data);
 429 }
 430
 431 #if defined(USE_MAG)
 432 static void sendFakeLatLongThatAllowsHeadingDisplay(void)
 433 {
 434     // Heading is only displayed on OpenTX if non-zero lat/long is also sent
 435     int32_t coord[2] = {
 436         1 * GPS_DEGREES_DIVIDER,
 437         1 * GPS_DEGREES_DIVIDER
 438     };
 439
 440     sendLatLong(coord);
 441 }
 442
 443 static void sendHeading(void)
 444 {
 445     frSkyHubWriteFrame(ID_COURSE_BP, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 446     frSkyHubWriteFrame(ID_COURSE_AP, 0);
 447 }
 448 #endif
 449
 450 bool initFrSkyHubTelemetry(void)
 451 {
 452     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 453         portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY_HUB);
 454         if (portConfig) {
 455             frSkyHubPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY_HUB);
 456
 457             frSkyHubWriteByte = frSkyHubWriteByteInternal;
 458
 459             telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
 460         }
 461
 462         return true;
 463     }
 464
 465     return false;
 466 }
 467
 468 bool initFrSkyHubTelemetryExternal(frSkyHubWriteByteFn *frSkyHubWriteByteExternal)
 469 {
 470     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 471         frSkyHubWriteByte = frSkyHubWriteByteExternal;
 472
 473         telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
 474
 475         return true;
 476     }
 477
 478     return false;
 479 }
 480
 481 void freeFrSkyHubTelemetryPort(void)
 482 {
 483     closeSerialPort(frSkyHubPort);
 484     frSkyHubPort = NULL;
 485 }
 486
 487 static void configureFrSkyHubTelemetryPort(void)
 488 {
 489     if (portConfig) {
 490         frSkyHubPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY_HUB, NULL, NULL, FRSKY_HUB_BAUDRATE, FRSKY_HUB_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 491     }
 492 }
 493
 494 void checkFrSkyHubTelemetryState(void)
 495 {
 496     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
 497         if (telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 498             if (frSkyHubPort == NULL && telemetrySharedPort != NULL) {
 499                 frSkyHubPort = telemetrySharedPort;
 500             }
 501         } else {
 502             bool enableSerialTelemetry = telemetryDetermineEnabledState(frSkyHubPortSharing);
 503             if (enableSerialTelemetry && !frSkyHubPort) {
 504                 configureFrSkyHubTelemetryPort();
 505             } else if (!enableSerialTelemetry && frSkyHubPort) {
 506                 freeFrSkyHubTelemetryPort();
 507             }
 508         }
 509     }
 510 }
 511
 512 void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
 513 {
 514     static uint32_t frSkyHubLastCycleTime = 0;
 515     static uint8_t cycleNum = 0;
 516
 517     if (cmpTimeUs(currentTimeUs, frSkyHubLastCycleTime) < FRSKY_HUB_CYCLETIME_US) {
 518         return;
 519     }
 520     frSkyHubLastCycleTime = currentTimeUs;
 521
 522     cycleNum++;
 523
 524 #if defined(USE_ACC)
 525     if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_ACC_X | SENSOR_ACC_Y | SENSOR_ACC_Z)) {
 526         // Sent every 125ms
 527         sendAccel();
 528     }
 529 #endif
 530
 531 #if defined(USE_BARO) || defined(USE_RANGEFINDER) || defined(USE_GPS)
 532     if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER) | sensors(SENSOR_GPS)) {
 533         // Sent every 125ms
 534         // Send vertical speed for opentx. ID_VERT_SPEED
 535         // Unit is cm/s
 536 #ifdef USE_VARIO
 537         if (telemetryIsSensorEnabled(SENSOR_VARIO)) {
 538             frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario());
 539         }
 540 #endif
 541
 542         // Sent every 500ms
 543         if ((cycleNum % 4) == 0 && telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
 544             int32_t altitudeCm = getEstimatedAltitudeCm();
 545
 546             /* Allow 5s to boot correctly othervise send zero to prevent OpenTX
 547              * sensor lost notifications after warm boot. */
 548             if (frSkyHubLastCycleTime < DELAY_FOR_BARO_INITIALISATION_US) {
 549                 altitudeCm = 0;
 550             }
 551
 552             frSkyHubWriteFrame(ID_ALTITUDE_BP, altitudeCm / 100); // meters: integer part, eg. 123 from 123.45m
 553             frSkyHubWriteFrame(ID_ALTITUDE_AP, altitudeCm % 100); // meters: fractional part, eg. 45 from 123.45m
 554         }
 555     }
 556 #endif
 557
 558 #if defined(USE_MAG)
 559     if (sensors(SENSOR_MAG) && telemetryIsSensorEnabled(SENSOR_HEADING)) {
 560         // Sent every 500ms
 561         if ((cycleNum % 4) == 0) {
 562             sendHeading();
 563         }
 564     }
 565 #endif
 566
 567     // Sent every 1s
 568     if ((cycleNum % 8) == 0) {
 569         sendTemperature1();
 570         sendThrottleOrBatterySizeAsRpm();
 571
 572         if (isBatteryVoltageConfigured()) {
 573             if (telemetryIsSensorEnabled(SENSOR_VOLTAGE)) {
 574                 sendVoltageCells();
 575                 sendVoltageAmp();
 576             }
 577
 578             if (isAmperageConfigured()) {
 579                 if (telemetryIsSensorEnabled(SENSOR_CURRENT)) {
 580                     sendAmperage();
 581                 }
 582                 if (telemetryIsSensorEnabled(SENSOR_FUEL)) {
 583                     sendFuelLevel();
 584                 }
 585             }
 586         }
 587
 588 #if defined(USE_GPS)
 589         if (sensors(SENSOR_GPS)) {
 590             if (telemetryIsSensorEnabled(SENSOR_GROUND_SPEED)) {
 591                 sendSpeed();
 592             }
 593             if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) {
 594                 sendGpsAltitude();
 595             }
 596             sendSatalliteSignalQualityAsTemperature2(cycleNum);
 597             if (telemetryIsSensorEnabled(SENSOR_LAT_LONG)) {
 598                 sendGPSLatLong();
 599             }
 600         } else
 601 #endif
 602 #if defined(USE_MAG)
 603         if (sensors(SENSOR_MAG)) {
 604             sendFakeLatLongThatAllowsHeadingDisplay();
 605         }
 606 #else
 607         {}
 608 #endif
 609     }
 610
 611     // Sent every 5s
 612     if (cycleNum == 40) {
 613         cycleNum = 0;
 614         sendTime();
 615     }
 616
 617     sendTelemetryTail();
 618 }
 619
 620 void handleFrSkyHubTelemetry(timeUs_t currentTimeUs)
 621 {
 622     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && frSkyHubPort) {
 623         processFrSkyHubTelemetry(currentTimeUs);
 624     }
 625 }
 626 #endif