src/main/telemetry/smartport.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  * SmartPort Telemetry implementation by frank26080115
  23  * see https://github.com/frank26080115/cleanflight/wiki/Using-Smart-Port
  24  */
  25 #include <stdbool.h>
  26 #include <stdint.h>
  27 #include <stdlib.h>
  28 #include <string.h>
  29 #include <math.h>
  30
  31 #include "platform.h"
  32
  33 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SMARTPORT)
  34
  35 #include "common/axis.h"
  36 #include "common/color.h"
  37 #include "common/maths.h"
  38 #include "common/utils.h"
  39
  40 #include "config/feature.h"
  41 #include "pg/pg.h"
  42 #include "pg/pg_ids.h"
  43 #include "pg/rx.h"
  44
  45 #include "drivers/accgyro/accgyro.h"
  46 #include "drivers/compass/compass.h"
  47 #include "drivers/sensor.h"
  48 #include "drivers/time.h"
  49
  50 #include "fc/config.h"
  51 #include "fc/controlrate_profile.h"
  52 #include "fc/rc_controls.h"
  53 #include "fc/runtime_config.h"
  54
  55 #include "flight/position.h"
  56 #include "flight/failsafe.h"
  57 #include "flight/imu.h"
  58 #include "flight/mixer.h"
  59 #include "flight/pid.h"
  60
  61 #include "interface/msp.h"
  62
  63 #include "io/beeper.h"
  64 #include "io/motors.h"
  65 #include "io/gps.h"
  66 #include "io/serial.h"
  67
  68 #include "sensors/boardalignment.h"
  69 #include "sensors/sensors.h"
  70 #include "sensors/battery.h"
  71 #include "sensors/acceleration.h"
  72 #include "sensors/barometer.h"
  73 #include "sensors/compass.h"
  74 #include "sensors/esc_sensor.h"
  75 #include "sensors/gyro.h"
  76
  77 #include "rx/rx.h"
  78
  79 #include "telemetry/telemetry.h"
  80 #include "telemetry/smartport.h"
  81 #include "telemetry/msp_shared.h"
  82
  83 #define SMARTPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500
  84
  85 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky_hub.h
  86 enum
  87 {
  88     FSSP_DATAID_SPEED      = 0x0830 ,
  89     FSSP_DATAID_VFAS       = 0x0210 ,
  90     FSSP_DATAID_VFAS1      = 0x0211 ,
  91     FSSP_DATAID_VFAS2      = 0x0212 ,
  92     FSSP_DATAID_VFAS3      = 0x0213 ,
  93     FSSP_DATAID_VFAS4      = 0x0214 ,
  94     FSSP_DATAID_VFAS5      = 0x0215 ,
  95     FSSP_DATAID_VFAS6      = 0x0216 ,
  96     FSSP_DATAID_VFAS7      = 0x0217 ,
  97     FSSP_DATAID_VFAS8      = 0x0218 ,
  98     FSSP_DATAID_CURRENT    = 0x0200 ,
  99     FSSP_DATAID_CURRENT1   = 0x0201 ,
 100     FSSP_DATAID_CURRENT2   = 0x0202 ,
 101     FSSP_DATAID_CURRENT3   = 0x0203 ,
 102     FSSP_DATAID_CURRENT4   = 0x0204 ,
 103     FSSP_DATAID_CURRENT5   = 0x0205 ,
 104     FSSP_DATAID_CURRENT6   = 0x0206 ,
 105     FSSP_DATAID_CURRENT7   = 0x0207 ,
 106     FSSP_DATAID_CURRENT8   = 0x0208 ,
 107     FSSP_DATAID_RPM        = 0x0500 ,
 108     FSSP_DATAID_RPM1       = 0x0501 ,
 109     FSSP_DATAID_RPM2       = 0x0502 ,
 110     FSSP_DATAID_RPM3       = 0x0503 ,
 111     FSSP_DATAID_RPM4       = 0x0504 ,
 112     FSSP_DATAID_RPM5       = 0x0505 ,
 113     FSSP_DATAID_RPM6       = 0x0506 ,
 114     FSSP_DATAID_RPM7       = 0x0507 ,
 115     FSSP_DATAID_RPM8       = 0x0508 ,
 116     FSSP_DATAID_ALTITUDE   = 0x0100 ,
 117     FSSP_DATAID_FUEL       = 0x0600 ,
 118     FSSP_DATAID_ADC1       = 0xF102 ,
 119     FSSP_DATAID_ADC2       = 0xF103 ,
 120     FSSP_DATAID_LATLONG    = 0x0800 ,
 121     FSSP_DATAID_CAP_USED   = 0x0600 ,
 122     FSSP_DATAID_VARIO      = 0x0110 ,
 123     FSSP_DATAID_CELLS      = 0x0300 ,
 124     FSSP_DATAID_CELLS_LAST = 0x030F ,
 125     FSSP_DATAID_HEADING    = 0x0840 ,
 126     FSSP_DATAID_ACCX       = 0x0700 ,
 127     FSSP_DATAID_ACCY       = 0x0710 ,
 128     FSSP_DATAID_ACCZ       = 0x0720 ,
 129     FSSP_DATAID_T1         = 0x0400 ,
 130     FSSP_DATAID_T2         = 0x0410 ,
 131     FSSP_DATAID_HOME_DIST  = 0x0420 ,
 132     FSSP_DATAID_GPS_ALT    = 0x0820 ,
 133     FSSP_DATAID_ASPD       = 0x0A00 ,
 134     FSSP_DATAID_TEMP       = 0x0B70 ,
 135     FSSP_DATAID_TEMP1      = 0x0B71 ,
 136     FSSP_DATAID_TEMP2      = 0x0B72 ,
 137     FSSP_DATAID_TEMP3      = 0x0B73 ,
 138     FSSP_DATAID_TEMP4      = 0x0B74 ,
 139     FSSP_DATAID_TEMP5      = 0x0B75 ,
 140     FSSP_DATAID_TEMP6      = 0x0B76 ,
 141     FSSP_DATAID_TEMP7      = 0x0B77 ,
 142     FSSP_DATAID_TEMP8      = 0x0B78 ,
 143     FSSP_DATAID_A3         = 0x0900 ,
 144     FSSP_DATAID_A4         = 0x0910
 145 };
 146
 147 // if adding more sensors then increase this value
 148 #define MAX_DATAIDS 17
 149
 150 static uint16_t frSkyDataIdTable[MAX_DATAIDS];
 151
 152 #ifdef USE_ESC_SENSOR
 153 // number of sensors to send between sending the ESC sensors
 154 #define ESC_SENSOR_PERIOD 7
 155
 156 static uint16_t frSkyEscDataIdTable[] = {
 157     FSSP_DATAID_CURRENT   ,
 158     FSSP_DATAID_RPM       ,
 159     FSSP_DATAID_VFAS      ,
 160     FSSP_DATAID_TEMP
 161 };
 162 #endif
 163
 164 typedef struct frSkyTableInfo_s {
 165     uint16_t * table;
 166     uint8_t size;
 167     uint8_t index;
 168 } frSkyTableInfo_t;
 169
 170 static frSkyTableInfo_t frSkyDataIdTableInfo = { frSkyDataIdTable, 0, 0 };
 171 #ifdef USE_ESC_SENSOR
 172 #define ESC_DATAID_COUNT ( sizeof(frSkyEscDataIdTable) / sizeof(uint16_t) )
 173
 174 static frSkyTableInfo_t frSkyEscDataIdTableInfo = {frSkyEscDataIdTable, ESC_DATAID_COUNT, 0};
 175 #endif
 176
 177 #define SMARTPORT_BAUD 57600
 178 #define SMARTPORT_UART_MODE MODE_RXTX
 179 #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
 180
 181 static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
 182 static serialPortConfig_t *portConfig;
 183
 184 static portSharing_e smartPortPortSharing;
 185
 186 enum
 187 {
 188     TELEMETRY_STATE_UNINITIALIZED,
 189     TELEMETRY_STATE_INITIALIZED_SERIAL,
 190     TELEMETRY_STATE_INITIALIZED_EXTERNAL,
 191 };
 192
 193 static uint8_t telemetryState = TELEMETRY_STATE_UNINITIALIZED;
 194
 195 typedef struct smartPortFrame_s {
 196     uint8_t  sensorId;
 197     smartPortPayload_t payload;
 198     uint8_t  crc;
 199 } __attribute__((packed)) smartPortFrame_t;
 200
 201 #define SMARTPORT_MSP_PAYLOAD_SIZE (sizeof(smartPortPayload_t) - sizeof(uint8_t))
 202
 203 static smartPortWriteFrameFn *smartPortWriteFrame;
 204
 205 #if defined(USE_MSP_OVER_TELEMETRY)
 206 static bool smartPortMspReplyPending = false;
 207 #endif
 208
 209 smartPortPayload_t *smartPortDataReceive(uint16_t c, bool *clearToSend, smartPortCheckQueueEmptyFn *checkQueueEmpty, bool useChecksum)
 210 {
 211     static uint8_t rxBuffer[sizeof(smartPortPayload_t)];
 212     static uint8_t smartPortRxBytes = 0;
 213     static bool skipUntilStart = true;
 214     static bool awaitingSensorId = false;
 215     static bool byteStuffing = false;
 216     static uint16_t checksum = 0;
 217
 218     if (c == FSSP_START_STOP) {
 219         *clearToSend = false;
 220         smartPortRxBytes = 0;
 221         awaitingSensorId = true;
 222         skipUntilStart = false;
 223
 224         return NULL;
 225     } else if (skipUntilStart) {
 226         return NULL;
 227     }
 228
 229     if (awaitingSensorId) {
 230         awaitingSensorId = false;
 231         if ((c == FSSP_SENSOR_ID1) && checkQueueEmpty()) {
 232             // our slot is starting, no need to decode more
 233             *clearToSend = true;
 234             skipUntilStart = true;
 235         } else if (c == FSSP_SENSOR_ID2) {
 236             checksum = 0;
 237         } else {
 238             skipUntilStart = true;
 239         }
 240     } else {
 241         if (c == FSSP_DLE) {
 242             byteStuffing = true;
 243
 244             return NULL;
 245         } else if (byteStuffing) {
 246             c ^= FSSP_DLE_XOR;
 247             byteStuffing = false;
 248         }
 249
 250         if (smartPortRxBytes < sizeof(smartPortPayload_t)) {
 251             rxBuffer[smartPortRxBytes++] = (uint8_t)c;
 252             checksum += c;
 253
 254             if (!useChecksum && (smartPortRxBytes == sizeof(smartPortPayload_t))) {
 255                 skipUntilStart = true;
 256
 257                 return (smartPortPayload_t *)&rxBuffer;
 258             }
 259         } else {
 260             skipUntilStart = true;
 261
 262             checksum += c;
 263             checksum = (checksum & 0xFF) + (checksum >> 8);
 264             if (checksum == 0xFF) {
 265
 266                 return (smartPortPayload_t *)&rxBuffer;
 267             }
 268         }
 269     }
 270
 271     return NULL;
 272 }
 273
 274 void smartPortSendByte(uint8_t c, uint16_t *checksum, serialPort_t *port)
 275 {
 276     // smart port escape sequence
 277     if (c == FSSP_DLE || c == FSSP_START_STOP) {
 278         serialWrite(port, FSSP_DLE);
 279         serialWrite(port, c ^ FSSP_DLE_XOR);
 280     } else {
 281         serialWrite(port, c);
 282     }
 283
 284     if (checksum != NULL) {
 285         *checksum += c;
 286     }
 287 }
 288
 289 bool smartPortPayloadContainsMSP(const smartPortPayload_t *payload)
 290 {
 291     return payload->frameId == FSSP_MSPC_FRAME_SMARTPORT || payload->frameId == FSSP_MSPC_FRAME_FPORT;
 292 }
 293
 294
 295 void smartPortWriteFrameSerial(const smartPortPayload_t *payload, serialPort_t *port, uint16_t checksum)
 296 {
 297     uint8_t *data = (uint8_t *)payload;
 298     for (unsigned i = 0; i < sizeof(smartPortPayload_t); i++) {
 299         smartPortSendByte(*data++, &checksum, port);
 300     }
 301     checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
 302     smartPortSendByte((uint8_t)checksum, NULL, port);
 303 }
 304
 305 static void smartPortWriteFrameInternal(const smartPortPayload_t *payload)
 306 {
 307     smartPortWriteFrameSerial(payload, smartPortSerialPort, 0);
 308 }
 309
 310 static void smartPortSendPackage(uint16_t id, uint32_t val)
 311 {
 312     smartPortPayload_t payload;
 313     payload.frameId = FSSP_DATA_FRAME;
 314     payload.valueId = id;
 315     payload.data = val;
 316
 317     smartPortWriteFrame(&payload);
 318 }
 319
 320 #ifdef USE_ESC_SENSOR
 321 static bool reportExtendedEscSensors(void) {
 322     return feature(FEATURE_ESC_SENSOR) && telemetryConfig()->smartport_use_extra_sensors;
 323 }
 324 #endif
 325
 326 #define ADD_SENSOR(dataId) frSkyDataIdTableInfo.table[frSkyDataIdTableInfo.index++] = dataId
 327
 328 static void initSmartPortSensors(void)
 329 {
 330     frSkyDataIdTableInfo.index = 0;
 331
 332     ADD_SENSOR(FSSP_DATAID_T1);
 333     ADD_SENSOR(FSSP_DATAID_T2);
 334
 335     if (isBatteryVoltageConfigured()) {
 336 #ifdef USE_ESC_SENSOR
 337         if (!reportExtendedEscSensors())
 338 #endif
 339         {
 340             ADD_SENSOR(FSSP_DATAID_VFAS);
 341         }
 342
 343         ADD_SENSOR(FSSP_DATAID_A4);
 344     }
 345
 346     if (isAmperageConfigured()) {
 347 #ifdef USE_ESC_SENSOR
 348         if (!reportExtendedEscSensors())
 349 #endif
 350         {
 351             ADD_SENSOR(FSSP_DATAID_CURRENT);
 352         }
 353
 354         ADD_SENSOR(FSSP_DATAID_FUEL);
 355     }
 356
 357     if (sensors(SENSOR_ACC)) {
 358         ADD_SENSOR(FSSP_DATAID_HEADING);
 359         ADD_SENSOR(FSSP_DATAID_ACCX);
 360         ADD_SENSOR(FSSP_DATAID_ACCY);
 361         ADD_SENSOR(FSSP_DATAID_ACCZ);
 362     }
 363
 364     if (sensors(SENSOR_BARO)) {
 365         ADD_SENSOR(FSSP_DATAID_ALTITUDE);
 366         ADD_SENSOR(FSSP_DATAID_VARIO);
 367     }
 368
 369 #ifdef USE_GPS
 370     if (feature(FEATURE_GPS)) {
 371         ADD_SENSOR(FSSP_DATAID_SPEED);
 372         ADD_SENSOR(FSSP_DATAID_LATLONG);
 373         ADD_SENSOR(FSSP_DATAID_LATLONG); // twice (one for lat, one for long)
 374         ADD_SENSOR(FSSP_DATAID_HOME_DIST);
 375         ADD_SENSOR(FSSP_DATAID_GPS_ALT);
 376     }
 377 #endif
 378
 379     frSkyDataIdTableInfo.size = frSkyDataIdTableInfo.index;
 380     frSkyDataIdTableInfo.index = 0;
 381
 382 #ifdef USE_ESC_SENSOR
 383     if (reportExtendedEscSensors()) {
 384         frSkyEscDataIdTableInfo.size = ESC_DATAID_COUNT;
 385     } else {
 386         frSkyEscDataIdTableInfo.size = 0;
 387     }
 388 #endif
 389 }
 390
 391 bool initSmartPortTelemetry(void)
 392 {
 393     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 394         portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
 395         if (portConfig) {
 396             smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
 397
 398             smartPortWriteFrame = smartPortWriteFrameInternal;
 399
 400             initSmartPortSensors();
 401
 402             telemetryState = TELEMETRY_STATE_INITIALIZED_SERIAL;
 403         }
 404
 405         return true;
 406     }
 407
 408     return false;
 409 }
 410
 411 bool initSmartPortTelemetryExternal(smartPortWriteFrameFn *smartPortWriteFrameExternal)
 412 {
 413     if (telemetryState == TELEMETRY_STATE_UNINITIALIZED) {
 414         smartPortWriteFrame = smartPortWriteFrameExternal;
 415
 416         initSmartPortSensors();
 417
 418         telemetryState = TELEMETRY_STATE_INITIALIZED_EXTERNAL;
 419
 420         return true;
 421     }
 422
 423     return false;
 424 }
 425
 426 static void freeSmartPortTelemetryPort(void)
 427 {
 428     closeSerialPort(smartPortSerialPort);
 429     smartPortSerialPort = NULL;
 430 }
 431
 432 static void configureSmartPortTelemetryPort(void)
 433 {
 434     if (portConfig) {
 435         portOptions_e portOptions = (telemetryConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 436
 437         smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
 438     }
 439 }
 440
 441 void checkSmartPortTelemetryState(void)
 442 {
 443     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL) {
 444         bool enableSerialTelemetry = telemetryDetermineEnabledState(smartPortPortSharing);
 445
 446         if (enableSerialTelemetry && !smartPortSerialPort) {
 447             configureSmartPortTelemetryPort();
 448         } else if (!enableSerialTelemetry && smartPortSerialPort) {
 449             freeSmartPortTelemetryPort();
 450         }
 451     }
 452 }
 453
 454 #if defined(USE_MSP_OVER_TELEMETRY)
 455 static void smartPortSendMspResponse(uint8_t *data) {
 456     smartPortPayload_t payload;
 457     payload.frameId = FSSP_MSPS_FRAME;
 458     memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE);
 459
 460     smartPortWriteFrame(&payload);
 461 }
 462 #endif
 463
 464 void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
 465 {
 466     static uint8_t smartPortIdCycleCnt = 0;
 467     static uint8_t t1Cnt = 0;
 468     static uint8_t t2Cnt = 0;
 469 #ifdef USE_ESC_SENSOR
 470     static uint8_t smartPortIdOffset = 0;
 471 #endif
 472
 473 #if defined(USE_MSP_OVER_TELEMETRY)
 474     if (payload && smartPortPayloadContainsMSP(payload)) {
 475         // Do not check the physical ID here again
 476         // unless we start receiving other sensors' packets
 477         // Pass only the payload: skip frameId
 478          uint8_t *frameStart = (uint8_t *)&payload->valueId;
 479          smartPortMspReplyPending = handleMspFrame(frameStart, SMARTPORT_MSP_PAYLOAD_SIZE);
 480     }
 481 #else
 482     UNUSED(payload);
 483 #endif
 484
 485     bool doRun = true;
 486     while (doRun && *clearToSend) {
 487         // 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.
 488         if (requestTimeout) {
 489             if (millis() >= *requestTimeout) {
 490                 *clearToSend = false;
 491
 492                 return;
 493             }
 494         } else {
 495             doRun = false;
 496         }
 497
 498 #if defined(USE_MSP_OVER_TELEMETRY)
 499         if (smartPortMspReplyPending) {
 500             smartPortMspReplyPending = sendMspReply(SMARTPORT_MSP_PAYLOAD_SIZE, &smartPortSendMspResponse);
 501             *clearToSend = false;
 502
 503             return;
 504         }
 505 #endif
 506
 507         // we can send back any data we want, our tables keep track of the order and frequency of each data type we send
 508         frSkyTableInfo_t * tableInfo = &frSkyDataIdTableInfo;
 509
 510 #ifdef USE_ESC_SENSOR
 511         if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
 512             // send ESC sensors
 513             tableInfo = &frSkyEscDataIdTableInfo;
 514             if (tableInfo->index == tableInfo->size) { // end of ESC table, return to other sensors
 515                 tableInfo->index = 0;
 516                 smartPortIdCycleCnt = 0;
 517                 smartPortIdOffset++;
 518                 if (smartPortIdOffset == getMotorCount() + 1) { // each motor and ESC_SENSOR_COMBINED
 519                     smartPortIdOffset = 0;
 520                 }
 521             }
 522         }
 523         if (smartPortIdCycleCnt < ESC_SENSOR_PERIOD) {
 524             // send other sensors
 525             tableInfo = &frSkyDataIdTableInfo;
 526 #endif
 527             if (tableInfo->index == tableInfo->size) { // end of table reached, loop back
 528                 tableInfo->index = 0;
 529             }
 530 #ifdef USE_ESC_SENSOR
 531         }
 532 #endif
 533         uint16_t id = tableInfo->table[tableInfo->index];
 534 #ifdef USE_ESC_SENSOR
 535         if (smartPortIdCycleCnt >= ESC_SENSOR_PERIOD) {
 536             id += smartPortIdOffset;
 537         }
 538 #endif
 539         smartPortIdCycleCnt++;
 540         tableInfo->index++;
 541
 542         int32_t tmpi;
 543         uint32_t tmp2 = 0;
 544         uint16_t vfasVoltage;
 545         uint8_t cellCount;
 546
 547 #ifdef USE_ESC_SENSOR
 548         escSensorData_t *escData;
 549 #endif
 550
 551         switch (id) {
 552             case FSSP_DATAID_VFAS       :
 553                 vfasVoltage = getBatteryVoltage();
 554                 if (telemetryConfig()->report_cell_voltage) {
 555                     cellCount = getBatteryCellCount();
 556                     vfasVoltage = cellCount ? getBatteryVoltage() / cellCount : 0;
 557                 }
 558                 smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
 559                 *clearToSend = false;
 560                 break;
 561 #ifdef USE_ESC_SENSOR
 562             case FSSP_DATAID_VFAS1      :
 563             case FSSP_DATAID_VFAS2      :
 564             case FSSP_DATAID_VFAS3      :
 565             case FSSP_DATAID_VFAS4      :
 566             case FSSP_DATAID_VFAS5      :
 567             case FSSP_DATAID_VFAS6      :
 568             case FSSP_DATAID_VFAS7      :
 569             case FSSP_DATAID_VFAS8      :
 570                 escData = getEscSensorData(id - FSSP_DATAID_VFAS1);
 571                 if (escData != NULL) {
 572                     smartPortSendPackage(id, escData->voltage);
 573                     *clearToSend = false;
 574                 }
 575                 break;
 576 #endif
 577             case FSSP_DATAID_CURRENT    :
 578                 smartPortSendPackage(id, getAmperage() / 10); // given in 10mA steps, unknown requested unit
 579                 *clearToSend = false;
 580                 break;
 581 #ifdef USE_ESC_SENSOR
 582             case FSSP_DATAID_CURRENT1   :
 583             case FSSP_DATAID_CURRENT2   :
 584             case FSSP_DATAID_CURRENT3   :
 585             case FSSP_DATAID_CURRENT4   :
 586             case FSSP_DATAID_CURRENT5   :
 587             case FSSP_DATAID_CURRENT6   :
 588             case FSSP_DATAID_CURRENT7   :
 589             case FSSP_DATAID_CURRENT8   :
 590                 escData = getEscSensorData(id - FSSP_DATAID_CURRENT1);
 591                 if (escData != NULL) {
 592                     smartPortSendPackage(id, escData->current);
 593                     *clearToSend = false;
 594                 }
 595                 break;
 596             case FSSP_DATAID_RPM        :
 597                 escData = getEscSensorData(ESC_SENSOR_COMBINED);
 598                 if (escData != NULL) {
 599                     smartPortSendPackage(id, calcEscRpm(escData->rpm));
 600                     *clearToSend = false;
 601                 }
 602                 break;
 603             case FSSP_DATAID_RPM1       :
 604             case FSSP_DATAID_RPM2       :
 605             case FSSP_DATAID_RPM3       :
 606             case FSSP_DATAID_RPM4       :
 607             case FSSP_DATAID_RPM5       :
 608             case FSSP_DATAID_RPM6       :
 609             case FSSP_DATAID_RPM7       :
 610             case FSSP_DATAID_RPM8       :
 611                 escData = getEscSensorData(id - FSSP_DATAID_RPM1);
 612                 if (escData != NULL) {
 613                     smartPortSendPackage(id, calcEscRpm(escData->rpm));
 614                     *clearToSend = false;
 615                 }
 616                 break;
 617             case FSSP_DATAID_TEMP        :
 618                 escData = getEscSensorData(ESC_SENSOR_COMBINED);
 619                 if (escData != NULL) {
 620                     smartPortSendPackage(id, escData->temperature);
 621                     *clearToSend = false;
 622                 }
 623                 break;
 624             case FSSP_DATAID_TEMP1      :
 625             case FSSP_DATAID_TEMP2      :
 626             case FSSP_DATAID_TEMP3      :
 627             case FSSP_DATAID_TEMP4      :
 628             case FSSP_DATAID_TEMP5      :
 629             case FSSP_DATAID_TEMP6      :
 630             case FSSP_DATAID_TEMP7      :
 631             case FSSP_DATAID_TEMP8      :
 632                 escData = getEscSensorData(id - FSSP_DATAID_TEMP1);
 633                 if (escData != NULL) {
 634                     smartPortSendPackage(id, escData->temperature);
 635                     *clearToSend = false;
 636                 }
 637                 break;
 638 #endif
 639             case FSSP_DATAID_ALTITUDE   :
 640                 smartPortSendPackage(id, getEstimatedAltitude()); // unknown given unit, requested 100 = 1 meter
 641                 *clearToSend = false;
 642                 break;
 643             case FSSP_DATAID_FUEL       :
 644                 smartPortSendPackage(id, getMAhDrawn()); // given in mAh, unknown requested unit
 645                 *clearToSend = false;
 646                 break;
 647             case FSSP_DATAID_VARIO      :
 648                 smartPortSendPackage(id, getEstimatedVario()); // unknown given unit but requested in 100 = 1m/s
 649                 *clearToSend = false;
 650                 break;
 651             case FSSP_DATAID_HEADING    :
 652                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
 653                 *clearToSend = false;
 654                 break;
 655             case FSSP_DATAID_ACCX       :
 656                 smartPortSendPackage(id, lrintf(100 * acc.accADC[X] / acc.dev.acc_1G)); // Multiply by 100 to show as x.xx g on Taranis
 657                 *clearToSend = false;
 658                 break;
 659             case FSSP_DATAID_ACCY       :
 660                 smartPortSendPackage(id, lrintf(100 * acc.accADC[Y] / acc.dev.acc_1G));
 661                 *clearToSend = false;
 662                 break;
 663             case FSSP_DATAID_ACCZ       :
 664                 smartPortSendPackage(id, lrintf(100 * acc.accADC[Z] / acc.dev.acc_1G));
 665                 *clearToSend = false;
 666                 break;
 667             case FSSP_DATAID_T1         :
 668                 // we send all the flags as decimal digits for easy reading
 669
 670                 // the t1Cnt simply allows the telemetry view to show at least some changes
 671                 t1Cnt++;
 672                 if (t1Cnt == 4) {
 673                     t1Cnt = 1;
 674                 }
 675                 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
 676                 // the Taranis seems to be able to fit 5 digits on the screen
 677                 // the Taranis seems to consider this number a signed 16 bit integer
 678
 679                 if (!isArmingDisabled()) {
 680                     tmpi += 1;
 681                 } else {
 682                     tmpi += 2;
 683                 }
 684                 if (ARMING_FLAG(ARMED)) {
 685                     tmpi += 4;
 686                 }
 687
 688                 if (FLIGHT_MODE(ANGLE_MODE)) {
 689                     tmpi += 10;
 690                 }
 691                 if (FLIGHT_MODE(HORIZON_MODE)) {
 692                     tmpi += 20;
 693                 }
 694                 if (FLIGHT_MODE(PASSTHRU_MODE)) {
 695                     tmpi += 40;
 696                 }
 697
 698                 if (FLIGHT_MODE(MAG_MODE)) {
 699                     tmpi += 100;
 700                 }
 701                 if (FLIGHT_MODE(BARO_MODE)) {
 702                     tmpi += 200;
 703                 }
 704
 705                 if (FLIGHT_MODE(GPS_HOLD_MODE)) {
 706                     tmpi += 1000;
 707                 }
 708                 if (FLIGHT_MODE(GPS_HOME_MODE)) {
 709                     tmpi += 2000;
 710                 }
 711                 if (FLIGHT_MODE(HEADFREE_MODE)) {
 712                     tmpi += 4000;
 713                 }
 714
 715                 smartPortSendPackage(id, (uint32_t)tmpi);
 716                 *clearToSend = false;
 717                 break;
 718             case FSSP_DATAID_T2         :
 719 #ifdef USE_GPS
 720                 if (sensors(SENSOR_GPS)) {
 721                     // provide GPS lock status
 722                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
 723                     *clearToSend = false;
 724                 } else if (feature(FEATURE_GPS)) {
 725                     smartPortSendPackage(id, 0);
 726                     *clearToSend = false;
 727                 } else
 728 #endif
 729                 if (telemetryConfig()->pidValuesAsTelemetry) {
 730                     switch (t2Cnt) {
 731                         case 0:
 732                             tmp2 = currentPidProfile->pid[PID_ROLL].P;
 733                             tmp2 += (currentPidProfile->pid[PID_PITCH].P<<8);
 734                             tmp2 += (currentPidProfile->pid[PID_YAW].P<<16);
 735                         break;
 736                         case 1:
 737                             tmp2 = currentPidProfile->pid[PID_ROLL].I;
 738                             tmp2 += (currentPidProfile->pid[PID_PITCH].I<<8);
 739                             tmp2 += (currentPidProfile->pid[PID_YAW].I<<16);
 740                         break;
 741                         case 2:
 742                             tmp2 = currentPidProfile->pid[PID_ROLL].D;
 743                             tmp2 += (currentPidProfile->pid[PID_PITCH].D<<8);
 744                             tmp2 += (currentPidProfile->pid[PID_YAW].D<<16);
 745                         break;
 746                         case 3:
 747                             tmp2 = currentControlRateProfile->rates[FD_ROLL];
 748                             tmp2 += (currentControlRateProfile->rates[FD_PITCH]<<8);
 749                             tmp2 += (currentControlRateProfile->rates[FD_YAW]<<16);
 750                         break;
 751                     }
 752                     tmp2 += t2Cnt<<24;
 753                     t2Cnt++;
 754                     if (t2Cnt == 4) {
 755                         t2Cnt = 0;
 756                     }
 757                     smartPortSendPackage(id, tmp2);
 758                     *clearToSend = false;
 759                 }
 760                 break;
 761 #ifdef USE_GPS
 762             case FSSP_DATAID_SPEED      :
 763                 if (STATE(GPS_FIX)) {
 764                     //convert to knots: 1cm/s = 0.0194384449 knots
 765                     //Speed should be sent in knots/1000 (GPS speed is in cm/s)
 766                     uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
 767                     smartPortSendPackage(id, tmpui);
 768                     *clearToSend = false;
 769                 }
 770                 break;
 771             case FSSP_DATAID_LATLONG    :
 772                 if (STATE(GPS_FIX)) {
 773                     uint32_t tmpui = 0;
 774                     // the same ID is sent twice, one for longitude, one for latitude
 775                     // the MSB of the sent uint32_t helps FrSky keep track
 776                     // the even/odd bit of our counter helps us keep track
 777                     if (tableInfo->index & 1) {
 778                         tmpui = abs(gpsSol.llh.lon);  // now we have unsigned value and one bit to spare
 779                         tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
 780                         if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
 781                     }
 782                     else {
 783                         tmpui = abs(gpsSol.llh.lat);  // now we have unsigned value and one bit to spare
 784                         tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
 785                         if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
 786                     }
 787                     smartPortSendPackage(id, tmpui);
 788                     *clearToSend = false;
 789                 }
 790                 break;
 791             case FSSP_DATAID_HOME_DIST  :
 792                 if (STATE(GPS_FIX)) {
 793                     smartPortSendPackage(id, GPS_distanceToHome);
 794                      *clearToSend = false;
 795                 }
 796                 break;
 797             case FSSP_DATAID_GPS_ALT    :
 798                 if (STATE(GPS_FIX)) {
 799                     smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
 800                     *clearToSend = false;
 801                 }
 802                 break;
 803 #endif
 804             case FSSP_DATAID_A4         :
 805                 cellCount = getBatteryCellCount();
 806                 vfasVoltage = cellCount ? (getBatteryVoltage() * 10 / cellCount) : 0; // given in 0.1V, convert to volts
 807                 smartPortSendPackage(id, vfasVoltage);
 808                 *clearToSend = false;
 809                 break;
 810             default:
 811                 break;
 812                 // if nothing is sent, hasRequest isn't cleared, we already incremented the counter, just loop back to the start
 813         }
 814     }
 815 }
 816
 817 static bool serialCheckQueueEmpty(void)
 818 {
 819     return (serialRxBytesWaiting(smartPortSerialPort) == 0);
 820 }
 821
 822 void handleSmartPortTelemetry(void)
 823 {
 824     const uint32_t requestTimeout = millis() + SMARTPORT_SERVICE_TIMEOUT_MS;
 825
 826     if (telemetryState == TELEMETRY_STATE_INITIALIZED_SERIAL && smartPortSerialPort) {
 827         smartPortPayload_t *payload = NULL;
 828         bool clearToSend = false;
 829         while (serialRxBytesWaiting(smartPortSerialPort) > 0 && !payload) {
 830             uint8_t c = serialRead(smartPortSerialPort);
 831             payload = smartPortDataReceive(c, &clearToSend, serialCheckQueueEmpty, true);
 832         }
 833
 834         processSmartPortTelemetry(payload, &clearToSend, &requestTimeout);
 835     }
 836 }
 837 #endif