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