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 <math.h>
   9
  10 #include "platform.h"
  11
  12 #ifdef TELEMETRY
  13
  14 #include "common/axis.h"
  15 #include "common/color.h"
  16 #include "common/maths.h"
  17
  18 #include "drivers/system.h"
  19 #include "drivers/sensor.h"
  20 #include "drivers/accgyro.h"
  21 #include "drivers/compass.h"
  22 #include "drivers/serial.h"
  23 #include "drivers/bus_i2c.h"
  24 #include "drivers/gpio.h"
  25 #include "drivers/timer.h"
  26 #include "drivers/pwm_rx.h"
  27 #include "drivers/adc.h"
  28 #include "drivers/light_led.h"
  29
  30 #include "rx/rx.h"
  31 #include "rx/msp.h"
  32
  33 #include "io/beeper.h"
  34 #include "io/escservo.h"
  35 #include "io/rc_controls.h"
  36 #include "io/gps.h"
  37 #include "io/gimbal.h"
  38 #include "io/serial.h"
  39 #include "io/ledstrip.h"
  40 #include "io/osd.h"
  41 #include "io/vtx.h"
  42
  43 #include "sensors/boardalignment.h"
  44 #include "sensors/sensors.h"
  45 #include "sensors/battery.h"
  46 #include "sensors/acceleration.h"
  47 #include "sensors/barometer.h"
  48 #include "sensors/compass.h"
  49 #include "sensors/gyro.h"
  50
  51 #include "flight/pid.h"
  52 #include "flight/imu.h"
  53 #include "flight/mixer.h"
  54 #include "flight/failsafe.h"
  55 #include "flight/navigation.h"
  56 #include "flight/altitudehold.h"
  57
  58 #include "telemetry/telemetry.h"
  59 #include "telemetry/smartport.h"
  60
  61 #include "config/runtime_config.h"
  62 #include "config/config.h"
  63
  64 enum
  65 {
  66     SPSTATE_UNINITIALIZED,
  67     SPSTATE_INITIALIZED,
  68     SPSTATE_WORKING,
  69     SPSTATE_TIMEDOUT,
  70 };
  71
  72 enum
  73 {
  74     FSSP_START_STOP = 0x7E,
  75     FSSP_DATA_FRAME = 0x10,
  76
  77     // ID of sensor. Must be something that is polled by FrSky RX
  78     FSSP_SENSOR_ID1 = 0x1B,
  79     FSSP_SENSOR_ID2 = 0x0D,
  80     FSSP_SENSOR_ID3 = 0x34,
  81     FSSP_SENSOR_ID4 = 0x67,
  82     // there are 32 ID's polled by smartport master
  83     // remaining 3 bits are crc (according to comments in openTx code)
  84 };
  85
  86 // these data identifiers are obtained from https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h
  87 enum
  88 {
  89     FSSP_DATAID_SPEED      = 0x0830 ,
  90     FSSP_DATAID_VFAS       = 0x0210 ,
  91     FSSP_DATAID_CURRENT    = 0x0200 ,
  92     FSSP_DATAID_RPM        = 0x050F ,
  93     FSSP_DATAID_ALTITUDE   = 0x0100 ,
  94     FSSP_DATAID_FUEL       = 0x0600 ,
  95     FSSP_DATAID_ADC1       = 0xF102 ,
  96     FSSP_DATAID_ADC2       = 0xF103 ,
  97     FSSP_DATAID_LATLONG    = 0x0800 ,
  98     FSSP_DATAID_CAP_USED   = 0x0600 ,
  99     FSSP_DATAID_VARIO      = 0x0110 ,
 100     FSSP_DATAID_CELLS      = 0x0300 ,
 101     FSSP_DATAID_CELLS_LAST = 0x030F ,
 102     FSSP_DATAID_HEADING    = 0x0840 ,
 103     FSSP_DATAID_ACCX       = 0x0700 ,
 104     FSSP_DATAID_ACCY       = 0x0710 ,
 105     FSSP_DATAID_ACCZ       = 0x0720 ,
 106     FSSP_DATAID_T1         = 0x0400 ,
 107     FSSP_DATAID_T2         = 0x0410 ,
 108     FSSP_DATAID_GPS_ALT    = 0x0820 ,
 109     FSSP_DATAID_A3         = 0x0900 ,
 110     FSSP_DATAID_A4         = 0x0910 ,
 111 };
 112
 113 const uint16_t frSkyDataIdTable[] = {
 114     FSSP_DATAID_SPEED     ,
 115     FSSP_DATAID_VFAS      ,
 116     FSSP_DATAID_CURRENT   ,
 117     //FSSP_DATAID_RPM       ,
 118     FSSP_DATAID_ALTITUDE  ,
 119     FSSP_DATAID_FUEL      ,
 120     //FSSP_DATAID_ADC1      ,
 121     //FSSP_DATAID_ADC2      ,
 122     FSSP_DATAID_LATLONG   ,
 123     FSSP_DATAID_LATLONG   , // twice
 124     //FSSP_DATAID_CAP_USED  ,
 125     FSSP_DATAID_VARIO     ,
 126     //FSSP_DATAID_CELLS     ,
 127     //FSSP_DATAID_CELLS_LAST,
 128     FSSP_DATAID_HEADING   ,
 129     FSSP_DATAID_ACCX      ,
 130     FSSP_DATAID_ACCY      ,
 131     FSSP_DATAID_ACCZ      ,
 132     FSSP_DATAID_T1        ,
 133     FSSP_DATAID_T2        ,
 134     FSSP_DATAID_GPS_ALT   ,
 135     FSSP_DATAID_A4        ,
 136     0
 137 };
 138
 139 #define __USE_C99_MATH // for roundf()
 140 #define SMARTPORT_BAUD 57600
 141 #define SMARTPORT_UART_MODE MODE_RXTX
 142 #define SMARTPORT_SERVICE_TIMEOUT_MS 1 // max allowed time to find a value to send
 143 #define SMARTPORT_NOT_CONNECTED_TIMEOUT_MS 7000
 144
 145 static serialPort_t *smartPortSerialPort = NULL; // The 'SmartPort'(tm) Port.
 146 static serialPortConfig_t *portConfig;
 147
 148 static telemetryConfig_t *telemetryConfig;
 149 static bool smartPortTelemetryEnabled =  false;
 150 static portSharing_e smartPortPortSharing;
 151
 152 char smartPortState = SPSTATE_UNINITIALIZED;
 153 static uint8_t smartPortHasRequest = 0;
 154 static uint8_t smartPortIdCnt = 0;
 155 static uint32_t smartPortLastRequestTime = 0;
 156
 157 static void smartPortDataReceive(uint16_t c)
 158 {
 159     uint32_t now = millis();
 160
 161     // look for a valid request sequence
 162     static uint8_t lastChar;
 163     if (lastChar == FSSP_START_STOP) {
 164         smartPortState = SPSTATE_WORKING;
 165         if (c == FSSP_SENSOR_ID1 && (serialRxBytesWaiting(smartPortSerialPort) == 0)) {
 166             smartPortLastRequestTime = now;
 167             smartPortHasRequest = 1;
 168             // we only responde to these IDs
 169             // the X4R-SB does send other IDs, we ignore them, but take note of the time
 170         }
 171     }
 172     lastChar = c;
 173 }
 174
 175 static void smartPortSendByte(uint8_t c, uint16_t *crcp)
 176 {
 177     // smart port escape sequence
 178     if (c == 0x7D || c == 0x7E) {
 179         serialWrite(smartPortSerialPort, 0x7D);
 180         c ^= 0x20;
 181     }
 182
 183     serialWrite(smartPortSerialPort, c);
 184
 185     if (crcp == NULL)
 186         return;
 187
 188     uint16_t crc = *crcp;
 189     crc += c;
 190     crc += crc >> 8;
 191     crc &= 0x00FF;
 192     *crcp = crc;
 193 }
 194
 195 static void smartPortSendPackage(uint16_t id, uint32_t val)
 196 {
 197     uint16_t crc = 0;
 198     smartPortSendByte(FSSP_DATA_FRAME, &crc);
 199     uint8_t *u8p = (uint8_t*)&id;
 200     smartPortSendByte(u8p[0], &crc);
 201     smartPortSendByte(u8p[1], &crc);
 202     u8p = (uint8_t*)&val;
 203     smartPortSendByte(u8p[0], &crc);
 204     smartPortSendByte(u8p[1], &crc);
 205     smartPortSendByte(u8p[2], &crc);
 206     smartPortSendByte(u8p[3], &crc);
 207     smartPortSendByte(0xFF - (uint8_t)crc, NULL);
 208 }
 209
 210 void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
 211 {
 212     telemetryConfig = initialTelemetryConfig;
 213     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT);
 214     smartPortPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_SMARTPORT);
 215 }
 216
 217 void freeSmartPortTelemetryPort(void)
 218 {
 219     closeSerialPort(smartPortSerialPort);
 220     smartPortSerialPort = NULL;
 221
 222     smartPortState = SPSTATE_UNINITIALIZED;
 223     smartPortTelemetryEnabled = false;
 224 }
 225
 226 void configureSmartPortTelemetryPort(void)
 227 {
 228     portOptions_t portOptions;
 229
 230     if (!portConfig) {
 231         return;
 232     }
 233
 234     portOptions = SERIAL_BIDIR;
 235
 236     if (telemetryConfig->telemetry_inversion) {
 237         portOptions |= SERIAL_INVERTED;
 238     }
 239
 240     smartPortSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
 241
 242     if (!smartPortSerialPort) {
 243         return;
 244     }
 245
 246     smartPortState = SPSTATE_INITIALIZED;
 247     smartPortTelemetryEnabled = true;
 248     smartPortLastRequestTime = millis();
 249 }
 250
 251 bool canSendSmartPortTelemetry(void)
 252 {
 253     return smartPortSerialPort && (smartPortState == SPSTATE_INITIALIZED || smartPortState == SPSTATE_WORKING);
 254 }
 255
 256 bool isSmartPortTimedOut(void)
 257 {
 258     return smartPortState >= SPSTATE_TIMEDOUT;
 259 }
 260
 261 void checkSmartPortTelemetryState(void)
 262 {
 263     bool newTelemetryEnabledValue = telemetryDetermineEnabledState(smartPortPortSharing);
 264
 265     if (newTelemetryEnabledValue == smartPortTelemetryEnabled) {
 266         return;
 267     }
 268
 269     if (newTelemetryEnabledValue)
 270         configureSmartPortTelemetryPort();
 271     else
 272         freeSmartPortTelemetryPort();
 273 }
 274
 275 void handleSmartPortTelemetry(void)
 276 {
 277     uint32_t smartPortLastServiceTime = millis();
 278
 279     if (!smartPortTelemetryEnabled) {
 280         return;
 281     }
 282
 283     if (!canSendSmartPortTelemetry()) {
 284         return;
 285     }
 286
 287     while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
 288         uint8_t c = serialRead(smartPortSerialPort);
 289         smartPortDataReceive(c);
 290     }
 291
 292     uint32_t now = millis();
 293
 294     // if timed out, reconfigure the UART back to normal so the GUI or CLI works
 295     if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
 296         smartPortState = SPSTATE_TIMEDOUT;
 297         return;
 298     }
 299
 300     while (smartPortHasRequest) {
 301         // 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.
 302         if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
 303             smartPortHasRequest = 0;
 304             return;
 305         }
 306
 307         // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
 308         uint16_t id = frSkyDataIdTable[smartPortIdCnt];
 309         if (id == 0) { // end of table reached, loop back
 310             smartPortIdCnt = 0;
 311             id = frSkyDataIdTable[smartPortIdCnt];
 312         }
 313         smartPortIdCnt++;
 314
 315         int32_t tmpi;
 316         static uint8_t t1Cnt = 0;
 317
 318         switch(id) {
 319 #ifdef GPS
 320             case FSSP_DATAID_SPEED      :
 321                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 322                     uint32_t tmpui = (GPS_speed * 36 + 36 / 2) / 100;
 323                     smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
 324                     smartPortHasRequest = 0;
 325                 }
 326                 break;
 327 #endif
 328             case FSSP_DATAID_VFAS       :
 329                 if (feature(FEATURE_VBAT)) {
 330                     uint16_t vfasVoltage;
 331                     if (telemetryConfig->frsky_vfas_cell_voltage) {
 332                         vfasVoltage = vbat / batteryCellCount;
 333                     } else {
 334                         vfasVoltage = vbat;
 335                     }
 336                     smartPortSendPackage(id, vfasVoltage * 10); // given in 0.1V, convert to volts
 337                     smartPortHasRequest = 0;
 338                 }
 339                 break;
 340             case FSSP_DATAID_CURRENT    :
 341                 if (feature(FEATURE_CURRENT_METER)) {
 342                     smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
 343                     smartPortHasRequest = 0;
 344                 }
 345                 break;
 346             //case FSSP_DATAID_RPM        :
 347             case FSSP_DATAID_ALTITUDE   :
 348                 if (sensors(SENSOR_BARO)) {
 349                     smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
 350                     smartPortHasRequest = 0;
 351                 }
 352                 break;
 353             case FSSP_DATAID_FUEL       :
 354                 if (feature(FEATURE_CURRENT_METER)) {
 355                     smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
 356                     smartPortHasRequest = 0;
 357                 }
 358                 break;
 359             //case FSSP_DATAID_ADC1       :
 360             //case FSSP_DATAID_ADC2       :
 361 #ifdef GPS
 362             case FSSP_DATAID_LATLONG    :
 363                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 364                     uint32_t tmpui = 0;
 365                     // the same ID is sent twice, one for longitude, one for latitude
 366                     // the MSB of the sent uint32_t helps FrSky keep track
 367                     // the even/odd bit of our counter helps us keep track
 368                     if (smartPortIdCnt & 1) {
 369                         tmpui = abs(GPS_coord[LON]);  // now we have unsigned value and one bit to spare
 370                         tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
 371                         if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
 372                     }
 373                     else {
 374                         tmpui = abs(GPS_coord[LAT]);  // now we have unsigned value and one bit to spare
 375                         tmpui = (tmpui + tmpui / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
 376                         if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
 377                     }
 378                     smartPortSendPackage(id, tmpui);
 379                     smartPortHasRequest = 0;
 380                 }
 381                 break;
 382 #endif
 383             //case FSSP_DATAID_CAP_USED   :
 384             case FSSP_DATAID_VARIO      :
 385                 if (sensors(SENSOR_BARO)) {
 386                     smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
 387                     smartPortHasRequest = 0;
 388                 }
 389                 break;
 390             case FSSP_DATAID_HEADING    :
 391                 smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
 392                 smartPortHasRequest = 0;
 393                 break;
 394             case FSSP_DATAID_ACCX       :
 395                 smartPortSendPackage(id, accSmooth[X] / 44);
 396                 // unknown input and unknown output unit
 397                 // we can only show 00.00 format, another digit won't display right on Taranis
 398                 // dividing by roughly 44 will give acceleration in G units
 399                 smartPortHasRequest = 0;
 400                 break;
 401             case FSSP_DATAID_ACCY       :
 402                 smartPortSendPackage(id, accSmooth[Y] / 44);
 403                 smartPortHasRequest = 0;
 404                 break;
 405             case FSSP_DATAID_ACCZ       :
 406                 smartPortSendPackage(id, accSmooth[Z] / 44);
 407                 smartPortHasRequest = 0;
 408                 break;
 409             case FSSP_DATAID_T1         :
 410                 // we send all the flags as decimal digits for easy reading
 411
 412                 // the t1Cnt simply allows the telemetry view to show at least some changes
 413                 t1Cnt++;
 414                 if (t1Cnt >= 4) {
 415                     t1Cnt = 1;
 416                 }
 417                 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
 418                 // the Taranis seems to be able to fit 5 digits on the screen
 419                 // the Taranis seems to consider this number a signed 16 bit integer
 420
 421                 if (ARMING_FLAG(OK_TO_ARM))
 422                     tmpi += 1;
 423                 if (ARMING_FLAG(PREVENT_ARMING))
 424                     tmpi += 2;
 425                 if (ARMING_FLAG(ARMED))
 426                     tmpi += 4;
 427
 428                 if (FLIGHT_MODE(ANGLE_MODE))
 429                     tmpi += 10;
 430                 if (FLIGHT_MODE(HORIZON_MODE))
 431                     tmpi += 20;
 432                 if (FLIGHT_MODE(UNUSED_MODE))
 433                     tmpi += 40;
 434                 if (FLIGHT_MODE(PASSTHRU_MODE))
 435                     tmpi += 40;
 436
 437                 if (FLIGHT_MODE(MAG_MODE))
 438                     tmpi += 100;
 439                 if (FLIGHT_MODE(BARO_MODE))
 440                     tmpi += 200;
 441                 if (FLIGHT_MODE(SONAR_MODE))
 442                     tmpi += 400;
 443
 444                 if (FLIGHT_MODE(GPS_HOLD_MODE))
 445                     tmpi += 1000;
 446                 if (FLIGHT_MODE(GPS_HOME_MODE))
 447                     tmpi += 2000;
 448                 if (FLIGHT_MODE(HEADFREE_MODE))
 449                     tmpi += 4000;
 450
 451                 smartPortSendPackage(id, (uint32_t)tmpi);
 452                 smartPortHasRequest = 0;
 453                 break;
 454             case FSSP_DATAID_T2         :
 455                 if (sensors(SENSOR_GPS)) {
 456 #ifdef GPS
 457                     // provide GPS lock status
 458                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
 459                     smartPortHasRequest = 0;
 460 #endif
 461                 }
 462                 else if (feature(FEATURE_GPS)) {
 463                     smartPortSendPackage(id, 0);
 464                     smartPortHasRequest = 0;
 465                 }
 466                 break;
 467 #ifdef GPS
 468             case FSSP_DATAID_GPS_ALT    :
 469                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 470                     smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
 471                     smartPortHasRequest = 0;
 472                 }
 473                 break;
 474 #endif
 475             case FSSP_DATAID_A4         :
 476                 if (feature(FEATURE_VBAT)) {
 477                     smartPortSendPackage(id, vbat * 10 / batteryCellCount ); // given in 0.1V, convert to volts
 478                     smartPortHasRequest = 0;
 479                 }
 480                 break;
 481             default:
 482                 break;
 483                 // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
 484         }
 485     }
 486 }
 487
 488 #endif