src/main/telemetry/smartport.c

   1 /*
   2  * SmartPort Telemetry implementation by frank26080115
   3  * see https://github.com/frank26080115/cleanflight/wiki/Using-Smart-Port
   4  */
   5 #include <stdbool.h>
   6 #include <stdint.h>
   7 #include <stdlib.h>
   8 #include <string.h>
   9 #include <math.h>
  10
  11 #include "platform.h"
  12
  13 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
  14
  15 #include "common/axis.h"
  16 #include "common/color.h"
  17 #include "common/maths.h"
  18 #include "common/utils.h"
  19
  20 #include "config/feature.h"
  21 #include "config/parameter_group.h"
  22 #include "config/parameter_group_ids.h"
  23
  24 #include "drivers/accgyro/accgyro.h"
  25 #include "drivers/compass/compass.h"
  26 #include "drivers/sensor.h"
  27 #include "drivers/time.h"
  28
  29 #include "fc/config.h"
  30 #include "fc/controlrate_profile.h"
  31 #include "fc/rc_controls.h"
  32 #include "fc/rc_modes.h"
  33 #include "fc/runtime_config.h"
  34
  35 #include "flight/failsafe.h"
  36 #include "flight/imu.h"
  37 #include "flight/mixer.h"
  38 #include "flight/pid.h"
  39
  40 #include "io/beeper.h"
  41 #include "io/gps.h"
  42 #include "io/serial.h"
  43
  44 #include "navigation/navigation.h"
  45
  46 #include "rx/frsky_crc.h"
  47
  48 #include "sensors/boardalignment.h"
  49 #include "sensors/sensors.h"
  50 #include "sensors/battery.h"
  51 #include "sensors/acceleration.h"
  52 #include "sensors/barometer.h"
  53 #include "sensors/compass.h"
  54 #include "sensors/gyro.h"
  55 #include "sensors/pitotmeter.h"
  56
  57 #include "rx/rx.h"
  58
  59 #include "telemetry/telemetry.h"
  60 #include "telemetry/smartport.h"
  61 #include "telemetry/msp_shared.h"
  62
  63 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/2.3/radio/src/telemetry/frsky.h
  64 enum
  65 {
  66     FSSP_DATAID_SPEED      = 0x0830 ,
  67     FSSP_DATAID_VFAS       = 0x0210 ,
  68     FSSP_DATAID_CURRENT    = 0x0200 ,
  69     FSSP_DATAID_RPM        = 0x050F ,
  70     FSSP_DATAID_ALTITUDE   = 0x0100 ,
  71     FSSP_DATAID_FUEL       = 0x0600 ,
  72     FSSP_DATAID_ADC1       = 0xF102 ,
  73     FSSP_DATAID_ADC2       = 0xF103 ,
  74     FSSP_DATAID_LATLONG    = 0x0800 ,
  75     FSSP_DATAID_CAP_USED   = 0x0600 ,
  76     FSSP_DATAID_VARIO      = 0x0110 ,
  77     FSSP_DATAID_CELLS      = 0x0300 ,
  78     FSSP_DATAID_CELLS_LAST = 0x030F ,
  79     FSSP_DATAID_HEADING    = 0x0840 ,
  80     FSSP_DATAID_FPV        = 0x0450 ,
  81     FSSP_DATAID_PITCH      = 0x0430 ,
  82     FSSP_DATAID_ROLL       = 0x0440 ,
  83     FSSP_DATAID_ACCX       = 0x0700 ,
  84     FSSP_DATAID_ACCY       = 0x0710 ,
  85     FSSP_DATAID_ACCZ       = 0x0720 ,
  86     FSSP_DATAID_T1         = 0x0400 ,
  87     FSSP_DATAID_T2         = 0x0410 ,
  88     FSSP_DATAID_HOME_DIST  = 0x0420 ,
  89     FSSP_DATAID_GPS_ALT    = 0x0820 ,
  90     FSSP_DATAID_ASPD       = 0x0A00 ,
  91     FSSP_DATAID_A3         = 0x0900 ,
  92     FSSP_DATAID_A4         = 0x0910 ,
  93     FSSP_DATAID_AZIMUTH    = 0x0460
  94 };
  95
  96 const uint16_t frSkyDataIdTable[] = {
  97     FSSP_DATAID_SPEED     ,
  98     FSSP_DATAID_VFAS      ,
  99     FSSP_DATAID_CURRENT   ,
 100     //FSSP_DATAID_RPM       ,
 101     FSSP_DATAID_ALTITUDE  ,
 102     FSSP_DATAID_FUEL      ,
 103     //FSSP_DATAID_ADC1      ,
 104     //FSSP_DATAID_ADC2      ,
 105     FSSP_DATAID_LATLONG   ,
 106     FSSP_DATAID_LATLONG   , // twice
 107     //FSSP_DATAID_CAP_USED  ,
 108     FSSP_DATAID_VARIO     ,
 109     //FSSP_DATAID_CELLS     ,
 110     //FSSP_DATAID_CELLS_LAST,
 111     FSSP_DATAID_HEADING   ,
 112     FSSP_DATAID_FPV       ,
 113     FSSP_DATAID_PITCH     ,
 114     FSSP_DATAID_ROLL      ,
 115     FSSP_DATAID_ACCX      ,
 116     FSSP_DATAID_ACCY      ,
 117     FSSP_DATAID_ACCZ      ,
 118     FSSP_DATAID_T1        ,
 119     FSSP_DATAID_T2        ,
 120     FSSP_DATAID_HOME_DIST ,
 121     FSSP_DATAID_GPS_ALT   ,
 122     FSSP_DATAID_ASPD      ,
 123     // FSSP_DATAID_A3        ,
 124     FSSP_DATAID_A4        ,
 125     FSSP_DATAID_AZIMUTH ,
 126     0
 127 };
 128
 129 #define __USE_C99_MATH // for roundf()
 130 #define SMARTPORT_BAUD 57600
 131 #define SMARTPORT_UART_MODE MODE_RXTX
 132 #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
 133
 134 static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
 135 static serialPortConfig_t *portConfig;
 136
 137 static portSharing_e smartPortPortSharing;
 138
 139 enum
 140 {
 141     TELEMETRY_STATE_UNINITIALIZED,
 142     TELEMETRY_STATE_INITIALIZED_SERIAL,
 143     TELEMETRY_STATE_INITIALIZED_EXTERNAL,
 144 };
 145
 146 static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
 147 static uint8_t smartPortIdCnt = 0;
 148
 149 typedef struct smartPortFrame_s {
 150     uint8_t  sensorId;
 151     smartPortPayload_t payload;
 152     uint8_t  crc;
 153 } __attribute__((packed)) smartPortFrame_t;
 154
 155 #define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
 156
 157 static smartPortWriteFrameFn *smartPortWriteFrame;
 158
 159 #if defined(USE_MSP_OVER_TELEMETRY)
 160 static bool smartPortMspReplyPending = false;
 161 #endif
 162
 163 static uint16_t frskyGetFlightMode(void)
 164 {
 165     uint16_t tmpi = 0;
 166
 167     // ones column
 168     if (!isArmingDisabled())
 169         tmpi += 1;
 170     else
 171         tmpi += 2;
 172     if (ARMING_FLAG(ARMED))
 173         tmpi += 4;
 174
 175     // tens column
 176     if (FLIGHT_MODE(ANGLE_MODE))
 177         tmpi += 10;
 178     if (FLIGHT_MODE(HORIZON_MODE))
 179         tmpi += 20;
 180     if (FLIGHT_MODE(MANUAL_MODE))
 181         tmpi += 40;
 182
 183     // hundreds column
 184     if (FLIGHT_MODE(HEADING_MODE))
 185         tmpi += 100;
 186     if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
 187         tmpi += 200;
 188     if (FLIGHT_MODE(NAV_POSHOLD_MODE))
 189         tmpi += 400;
 190
 191     // thousands column
 192     if (FLIGHT_MODE(NAV_RTH_MODE))
 193         tmpi += 1000;
 194     if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow
 195         tmpi += 8000;
 196     else if (FLIGHT_MODE(NAV_WP_MODE))
 197         tmpi += 2000;
 198     else if (FLIGHT_MODE(HEADFREE_MODE))
 199         tmpi += 4000;
 200
 201     // ten thousands column
 202     if (FLIGHT_MODE(FLAPERON))
 203         tmpi += 10000;
 204     if (FLIGHT_MODE(FAILSAFE_MODE))
 205         tmpi += 40000;
 206     else if (FLIGHT_MODE(AUTO_TUNE)) // intentionally reverse order and 'else-if' to prevent 16-bit overflow
 207         tmpi += 20000;
 208
 209     return tmpi;
 210 }
 211
 212 static uint16_t frskyGetGPSState(void)
 213 {
 214     uint16_t tmpi = 0;
 215
 216     // ones and tens columns (# of satellites 0 - 99)
 217     tmpi += constrain(gpsSol.numSat, 0, 99);
 218
 219     // hundreds column (satellite accuracy HDOP: 0 = worst [HDOP > 5.5], 9 = best [HDOP <= 1.0])
 220     tmpi += (9 - constrain((gpsSol.hdop - 51) / 50, 0, 9)) * 100;
 221
 222     // thousands column (GPS fix status)
 223     if (STATE(GPS_FIX))
 224         tmpi += 1000;
 225     if (STATE(GPS_FIX_HOME))
 226         tmpi += 2000;
 227     if (ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXHOMERESET) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE))
 228         tmpi += 4000;
 229
 230     return tmpi;
 231 }
 232
 233 smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
 234 {
 235     static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
 236     static uint8_t smartPortRxBytes = 0;
 237     static bool skipUntilStart = true;
 238     static bool awaitingSensorId = false;
 239     static bool byteStuffing = false;
 240     static uint16_t checksum = 0;
 241
 242     if (c == FSSP_START_STOP) {
 243         *clearToSend = false;
 244         smartPortRxBytes = 0;
 245         awaitingSensorId = true;
 246         skipUntilStart = false;
 247
 248         return NULL;
 249     } else if (skipUntilStart) {
 250         return NULL;
 251     }
 252
 253     if (awaitingSensorId) {
 254         awaitingSensorId = false;
 255         if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
 256             // our slot is starting, no need to decode more
 257             *clearToSend = true;
 258             skipUntilStart = true;
 259         } else if (c == FSSP_SENSOR_ID2) {
 260             checksum = 0;
 261         } else {
 262             skipUntilStart = true;
 263         }
 264     } else {
 265         if (c == FSSP_DLE) {
 266             byteStuffing = true;
 267
 268             return NULL;
 269         } else if (byteStuffing) {
 270             c ^= FSSP_DLE_XOR;
 271             byteStuffing = false;
 272         }
 273
 274         if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
 275             rxBuffer[smartPortRxBytes++] = (uint8_t)c;
 276             checksum += c;
 277
 278             if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
 279                 skipUntilStart = true;
 280
 281                 return (smartPortPayload_t *)&rxBuffer;
 282             }
 283         } else {
 284             skipUntilStart = true;
 285
 286             checksum += c;
 287             checksum = (checksum & 0xFF) + (checksum >> 8);
 288             if (checksum == 0xFF) {
 289                 return (smartPortPayload_t *)&rxBuffer;
 290             }
 291         }
 292     }
 293
 294     return NULL;
 295 }
 296
 297 void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
 298 {
 299     // smart port escape sequence
 300     if (c == FSSP_DLE || c == FSSP_START_STOP) {
 301         serialWrite(port, FSSP_DLE);
 302         serialWrite(port, c ^ FSSP_DLE_XOR);
 303     } else {
 304         serialWrite(port, c);
 305     }
 306
 307     if (checksum != NULL) {
 308         frskyCheckSumStep(checksum, c);
 309     }
 310 }
 311
 312 bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
 313 {
 314     return payload && (payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT);
 315 }
 316
 317 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 318 {
 319     uint8_t *data = (uint8_t *)payload;
 320     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
 321         smartPortSendByte(*data++, &checksum, port);
 322     }
 323     frskyCheckSumFini(&checksum);
 324     smartPortSendByte(checksum, NULL, port);
 325 }
 326
 327 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
 328 {
 329     smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
 330 }
 331
 332 static void smartPortSendPackage(uint16_t id, uint32_t val)
 333 {
 334     smartPortPayload_t payload;
 335     payload.frameId = FSSP_DATA_FRAME;
 336     payload.valueId = id;
 337     payload.data = val;
 338
 339     smartPortWriteFrame(&payload);
 340 }
 341
 342 bool initSmartPortTelemetry(void)
 343 {
 344     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 345         portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
 346         if (portConfig) {
 347             smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
 348
 349             smartPortWriteFrame = smartPortWriteFrameInternal;
 350
 351             telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
 352         }
 353
 354         return true;
 355     }
 356
 357     return false;
 358 }
 359
 360 bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
 361 {
 362     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 363         smartPortWriteFrame = smartPortWriteFrameExternal;
 364
 365         telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
 366
 367         return true;
 368     }
 369
 370     return false;
 371 }
 372
 373 static void freeSmartPortTelemetryPort(void)
 374 {
 375     closeSerialPort(smartPortSerialPort);
 376     smartPortSerialPort = NULL;
 377 }
 378
 379 static void configureSmartPortTelemetryPort(void)
 380 {
 381     if (portConfig) {
 382         portOptions_t portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 383
 384         smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
 385     }
 386 }
 387
 388 void checkSmartPortTelemetryState(void)
 389 {
 390     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
 391         bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
 392
 393         if (enableSerialTelemetry && !smartPortSerialPort) {
 394             configureSmartPortTelemetryPort();
 395         } else if (!enableSerialTelemetry && smartPortSerialPort) {
 396             freeSmartPortTelemetryPort();
 397         }
 398     }
 399 }
 400
 401 #if defined(USE_MSP_OVER_TELEMETRY)
 402 static void smartPortSendMspResponse(uint8_t *data) {
 403     smartPortPayload_t payload;
 404     payload.frameId = FSSP_MSPS_FRAME;
 405     memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
 406
 407     smartPortWriteFrame(&payload);
 408 }
 409 #endif
 410
 411 static bool smartPortShouldSendGPSData(void)
 412 {
 413     // We send GPS data if the GPS is configured and we have a fix
 414     // or the craft has never been armed yet. This way if GPS stops working
 415     // while in flight, the user will easily notice because the sensor will stop
 416     // updating.
 417     return feature(FEATURE_GPS) && (STATE(GPS_FIX)
 418 #ifdef USE_GPS_FIX_ESTIMATION
 419             || STATE(GPS_ESTIMATED_FIX)
 420 #endif
 421         || !ARMING_FLAG(WAS_EVER_ARMED));
 422 }
 423
 424 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
 425 {
 426     if (payload) {
 427         // do not check the physical ID here again
 428         // unless we start receiving other sensors' packets
 429
 430 #if defined(USE_MSP_OVER_TELEMETRY)
 431         if (smartPortPayloadContainsMSP(payload)) {
 432             // Pass only the payload: skip frameId
 433             uint8_t *frameStart = (uint8_t *)&payload->valueId;
 434             smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
 435         }
 436 #endif
 437     }
 438
 439     bool doRun = true;
 440     while (doRun && *clearToSend) {
 441         // Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
 442         if (requestTimeout) {
 443             if (millis() >= *requestTimeout) {
 444                 *clearToSend = false;
 445
 446                 return;
 447             }
 448         } else {
 449             doRun = false;
 450         }
 451
 452 #if defined(USE_MSP_OVER_TELEMETRY)
 453         if (smartPortMspReplyPending) {
 454             smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
 455             *clearToSend = false;
 456
 457             return;
 458         }
 459 #endif
 460
 461         // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
 462         uint16_t id = frSkyDataIdTable[smartPortIdCnt];
 463         if (id == 0) { // end of table reached, loop back
 464             smartPortIdCnt = 0;
 465             id = frSkyDataIdTable[smartPortIdCnt];
 466         }
 467         smartPortIdCnt++;
 468
 469         switch (id) {
 470             case FSSP_DATAID_VFAS       :
 471                 if (isBatteryVoltageConfigured()) {
 472                     uint16_t vfasVoltage = telemetryConfig()->report_cell_voltage ? getBatteryAverageCellVoltage() : getBatteryVoltage();
 473                     smartPortSendPackage(id, vfasVoltage);
 474                     *clearToSend = false;
 475                 }
 476                 break;
 477             case FSSP_DATAID_CURRENT    :
 478                 if (isAmperageConfigured()) {
 479                     smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
 480                     *clearToSend = false;
 481                 }
 482                 break;
 483             //case FSSP_DATAID_RPM        :
 484             case FSSP_DATAID_ALTITUDE   :
 485                 if (sensors(SENSOR_BARO)) {
 486                     smartPortSendPackage(id, getEstimatedActualPosition(Z)); // unknown given unit, requested 100 = 1 meter
 487                     *clearToSend = false;
 488                 }
 489                 break;
 490             case FSSP_DATAID_FUEL       :
 491                 if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
 492                     smartPortSendPackage(id, calculateBatteryPercentage()); // Show remaining battery % if smartport_fuel_percent=ON
 493                     *clearToSend = false;
 494                 } else if (isAmperageConfigured()) {
 495                     smartPortSendPackage(id, (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn()));
 496                     *clearToSend = false;
 497                 }
 498                 break;
 499             //case FSSP_DATAID_ADC1       :
 500             //case FSSP_DATAID_ADC2       :
 501             //case FSSP_DATAID_CAP_USED   :
 502             case FSSP_DATAID_VARIO      :
 503                 if (sensors(SENSOR_BARO)) {
 504                     smartPortSendPackage(id, lrintf(getEstimatedActualVelocity(Z))); // unknown given unit but requested in 100 = 1m/s
 505                     *clearToSend = false;
 506                 }
 507                 break;
 508             case FSSP_DATAID_HEADING    :
 509                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
 510                 *clearToSend = false;
 511                 break;
 512             case FSSP_DATAID_PITCH      :
 513                 if (telemetryConfig()->frsky_pitch_roll) {
 514                     smartPortSendPackage(id, attitude.values.pitch); // given in 10*deg
 515                     *clearToSend = false;
 516                 }
 517                 break;
 518             case FSSP_DATAID_ROLL       :
 519                 if (telemetryConfig()->frsky_pitch_roll) {
 520                     smartPortSendPackage(id, attitude.values.roll); // given in 10*deg
 521                     *clearToSend = false;
 522                 }
 523                 break;
 524             case FSSP_DATAID_ACCX       :
 525                 if (!telemetryConfig()->frsky_pitch_roll) {
 526                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[X]));
 527                     *clearToSend = false;
 528                 }
 529                 break;
 530             case FSSP_DATAID_ACCY       :
 531                 if (!telemetryConfig()->frsky_pitch_roll) {
 532                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[Y]));
 533                     *clearToSend = false;
 534                 }
 535                 break;
 536             case FSSP_DATAID_ACCZ       :
 537                 if (!telemetryConfig()->frsky_pitch_roll) {
 538                     smartPortSendPackage(id, lrintf(100 * acc.accADCf[Z]));
 539                     *clearToSend = false;
 540                 }
 541                 break;
 542             case FSSP_DATAID_T1         :
 543                 {
 544                     smartPortSendPackage(id, frskyGetFlightMode());
 545                     *clearToSend = false;
 546                     break;
 547                 }
 548 #ifdef USE_GPS
 549             case FSSP_DATAID_T2         :
 550                 if (smartPortShouldSendGPSData()) {
 551                     smartPortSendPackage(id, frskyGetGPSState());
 552                     *clearToSend = false;
 553                 }
 554                 break;
 555             case FSSP_DATAID_SPEED      :
 556                 if (smartPortShouldSendGPSData()) {
 557                     //convert to knots: 1cm/s = 0.0194384449 knots
 558                     //Speed should be sent in knots/1000 (GPS speed is in cm/s)
 559                     uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
 560                     smartPortSendPackage(id, tmpui);
 561                     *clearToSend = false;
 562                 }
 563                 break;
 564             case FSSP_DATAID_LATLONG    :
 565                 if (smartPortShouldSendGPSData()) {
 566                     uint32_t tmpui = 0;
 567                     // the same ID is sent twice, one for longitude, one for latitude
 568                     // the MSB of the sent uint32_t helps FrSky keep track
 569                     // the even/odd bit of our counter helps us keep track
 570                     if (smartPortIdCnt & 1) {
 571                         tmpui = abs(gpsSol.llh.lon);  // now we have unsigned value and one bit to spare
 572                         tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
 573                         if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
 574                     }
 575                     else {
 576                         tmpui = abs(gpsSol.llh.lat);  // now we have unsigned value and one bit to spare
 577                         tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
 578                         if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
 579                     }
 580                     smartPortSendPackage(id, tmpui);
 581                     *clearToSend = false;
 582                 }
 583                 break;
 584             case FSSP_DATAID_HOME_DIST  :
 585                 if (smartPortShouldSendGPSData()) {
 586                     smartPortSendPackage(id, GPS_distanceToHome);
 587                      *clearToSend = false;
 588                 }
 589                 break;
 590             case FSSP_DATAID_GPS_ALT    :
 591                 if (smartPortShouldSendGPSData()) {
 592                     smartPortSendPackage(id, gpsSol.llh.alt); // cm
 593                     *clearToSend = false;
 594                 }
 595                 break;
 596             case FSSP_DATAID_FPV       :
 597                 if (smartPortShouldSendGPSData()) {
 598                     smartPortSendPackage(id, gpsSol.groundCourse); // given in 10*deg
 599                     *clearToSend = false;
 600                 }
 601                 break;
 602             case FSSP_DATAID_AZIMUTH    :
 603                 if (smartPortShouldSendGPSData()) {
 604                     int16_t h = GPS_directionToHome;
 605                     if (h < 0) {
 606                         h += 360;
 607                     }
 608                     if(h >= 180)
 609                         h = h - 180;
 610                     else
 611                         h = h + 180;
 612                     smartPortSendPackage(id, h *10); // given in 10*deg
 613                     *clearToSend = false;
 614                 }
 615                 break;
 616 #endif
 617             case FSSP_DATAID_A4         :
 618                 if (isBatteryVoltageConfigured()) {
 619                     smartPortSendPackage(id, getBatteryAverageCellVoltage());
 620                     *clearToSend = false;
 621                 }
 622                 break;
 623             case FSSP_DATAID_ASPD       :
 624 #ifdef USE_PITOT
 625                 if (sensors(SENSOR_PITOT) && pitotIsHealthy()) {
 626                     smartPortSendPackage(id, getAirspeedEstimate() * 0.194384449f); // cm/s to knots*1
 627                     *clearToSend = false;
 628                 }
 629 #endif
 630                 break;
 631             default:
 632                 break;
 633                 // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
 634         }
 635     }
 636 }
 637
 638 static bool serialCheckQueueEmpty(void)
 639 {
 640     return (serialRxBytesWaiting(smartPortSerialPort) == 0);
 641 }
 642
 643 void handleSmartPortTelemetry(void)
 644 {
 645     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
 646         bool clearToSend = false;
 647         smartPortPayload_t *payload = NULL;
 648         const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
 649         while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
 650             uint8_t c = serialRead(smartPortSerialPort);
 651             payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
 652         }
 653
 654         processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
 655     }
 656 }
 657 #endif