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