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/flight.h"
  49 #include "flight/imu.h"
  50 #include "flight/mixer.h"
  51 #include "flight/failsafe.h"
  52 #include "flight/navigation.h"
  53
  54 #include "telemetry/telemetry.h"
  55 #include "telemetry/smartport.h"
  56
  57 #include "config/runtime_config.h"
  58 #include "config/config.h"
  59 #include "config/config_profile.h"
  60 #include "config/config_master.h"
  61
  62 enum
  63 {
  64     SPSTATE_UNINITIALIZED,
  65     SPSTATE_INITIALIZED,
  66     SPSTATE_WORKING,
  67     SPSTATE_TIMEDOUT,
  68     SPSTATE_DEINITIALIZED,
  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; // The 'SmartPort'(tm) Port.
 142 static telemetryConfig_t *telemetryConfig;
 143 static portMode_t previousPortMode;
 144 static uint32_t previousBaudRate;
 145 extern void serialInit(serialConfig_t *); // from main.c // FIXME remove this dependency
 146
 147 char smartPortState = SPSTATE_UNINITIALIZED;
 148 static uint8_t smartPortHasRequest = 0;
 149 static uint8_t smartPortIdCnt = 0;
 150 static uint32_t smartPortLastRequestTime = 0;
 151 static uint32_t smartPortLastServiceTime = 0;
 152
 153 static void smartPortDataReceive(uint16_t c)
 154 {
 155     uint32_t now = millis();
 156
 157     // look for a valid request sequence
 158     static uint8_t lastChar;
 159     if (lastChar == FSSP_START_STOP) {
 160         smartPortState = SPSTATE_WORKING;
 161         smartPortLastRequestTime = now;
 162         if ((c == FSSP_SENSOR_ID1) ||
 163             (c == FSSP_SENSOR_ID2) ||
 164             (c == FSSP_SENSOR_ID3) ||
 165             (c == FSSP_SENSOR_ID4)) {
 166             smartPortHasRequest = 1;
 167             // we only responde to these IDs
 168             // the X4R-SB does send other IDs, we ignore them, but take note of the time
 169         }
 170     }
 171     lastChar = c;
 172 }
 173
 174 static void smartPortSendByte(uint8_t c, uint16_t *crcp)
 175 {
 176     // smart port escape sequence
 177     if (c == 0x7D || c == 0x7E) {
 178         serialWrite(smartPortSerialPort, 0x7D);
 179         c ^= 0x20;
 180     }
 181
 182     serialWrite(smartPortSerialPort, c);
 183
 184     if (crcp == NULL)
 185         return;
 186
 187     uint16_t crc = *crcp;
 188     crc += c;
 189     crc += crc >> 8;
 190     crc &= 0x00FF;
 191     *crcp = crc;
 192 }
 193
 194 static void smartPortSendPackage(uint16_t id, uint32_t val)
 195 {
 196     uint16_t crc = 0;
 197     smartPortSendByte(FSSP_DATA_FRAME, &crc);
 198     uint8_t *u8p = (uint8_t*)&id;
 199     smartPortSendByte(u8p[0], &crc);
 200     smartPortSendByte(u8p[1], &crc);
 201     u8p = (uint8_t*)&val;
 202     smartPortSendByte(u8p[0], &crc);
 203     smartPortSendByte(u8p[1], &crc);
 204     smartPortSendByte(u8p[2], &crc);
 205     smartPortSendByte(u8p[3], &crc);
 206     smartPortSendByte(0xFF - (uint8_t)crc, NULL);
 207
 208     smartPortLastServiceTime = millis();
 209 }
 210
 211 void initSmartPortTelemetry(telemetryConfig_t *initialTelemetryConfig)
 212 {
 213     telemetryConfig = initialTelemetryConfig;
 214 }
 215
 216 void freeSmartPortTelemetryPort(void)
 217 {
 218     if (smartPortState == SPSTATE_UNINITIALIZED)
 219         return;
 220     if (smartPortState == SPSTATE_DEINITIALIZED)
 221         return;
 222
 223     if (isTelemetryPortShared()) {
 224         endSerialPortFunction(smartPortSerialPort, FUNCTION_SMARTPORT_TELEMETRY);
 225         smartPortState = SPSTATE_DEINITIALIZED;
 226         serialInit(&masterConfig.serialConfig);
 227     }
 228     else {
 229         serialSetMode(smartPortSerialPort, previousPortMode);
 230         serialSetBaudRate(smartPortSerialPort, previousBaudRate);
 231         endSerialPortFunction(smartPortSerialPort, FUNCTION_SMARTPORT_TELEMETRY);
 232         smartPortState = SPSTATE_DEINITIALIZED;
 233     }
 234     smartPortSerialPort = NULL;
 235 }
 236
 237 void configureSmartPortTelemetryPort(void)
 238 {
 239     if (smartPortState != SPSTATE_UNINITIALIZED) {
 240         // do not allow reinitialization
 241         return;
 242     }
 243
 244     smartPortSerialPort = findOpenSerialPort(FUNCTION_SMARTPORT_TELEMETRY);
 245     if (smartPortSerialPort) {
 246         previousPortMode = smartPortSerialPort->mode;
 247         previousBaudRate = smartPortSerialPort->baudRate;
 248
 249         //waitForSerialPortToFinishTransmitting(smartPortPort); // FIXME locks up the system
 250
 251         serialSetBaudRate(smartPortSerialPort, SMARTPORT_BAUD);
 252         serialSetMode(smartPortSerialPort, SMARTPORT_UART_MODE);
 253         beginSerialPortFunction(smartPortSerialPort, FUNCTION_SMARTPORT_TELEMETRY);
 254     } else {
 255         smartPortSerialPort = openSerialPort(FUNCTION_SMARTPORT_TELEMETRY, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, telemetryConfig->telemetry_inversion);
 256
 257         if (smartPortSerialPort) {
 258             smartPortState = SPSTATE_INITIALIZED;
 259             previousPortMode = smartPortSerialPort->mode;
 260             previousBaudRate = smartPortSerialPort->baudRate;
 261         }
 262         else {
 263             // failed, resume MSP and CLI
 264             if (isTelemetryPortShared()) {
 265                 smartPortState = SPSTATE_DEINITIALIZED;
 266                 serialInit(&masterConfig.serialConfig);
 267             }
 268         }
 269     }
 270 }
 271
 272 bool canSendSmartPortTelemetry(void)
 273 {
 274     return smartPortState == SPSTATE_INITIALIZED || smartPortState == SPSTATE_WORKING;
 275 }
 276
 277 bool canSmartPortAllowOtherSerial(void)
 278 {
 279     return smartPortState == SPSTATE_DEINITIALIZED;
 280 }
 281
 282 bool isSmartPortTimedOut(void)
 283 {
 284     return smartPortState >= SPSTATE_TIMEDOUT;
 285 }
 286
 287 uint32_t getSmartPortTelemetryProviderBaudRate(void)
 288 {
 289     return SMARTPORT_BAUD;
 290 }
 291
 292 void handleSmartPortTelemetry(void)
 293 {
 294     if (!canSendSmartPortTelemetry())
 295         return;
 296
 297     while (serialTotalBytesWaiting(smartPortSerialPort) > 0) {
 298         uint8_t c = serialRead(smartPortSerialPort);
 299         smartPortDataReceive(c);
 300     }
 301
 302     uint32_t now = millis();
 303
 304     // if timed out, reconfigure the UART back to normal so the GUI or CLI works
 305     if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
 306         smartPortState = SPSTATE_TIMEDOUT;
 307         return;
 308     }
 309
 310     // limit the rate at which we send responses, we don't want to affect flight characteristics
 311     if ((now - smartPortLastServiceTime) < SMARTPORT_SERVICE_DELAY_MS)
 312         return;
 313
 314     if (smartPortHasRequest) {
 315         // we can send back any data we want, our table keeps track of the order and frequency of each data type we send
 316         uint16_t id = frSkyDataIdTable[smartPortIdCnt];
 317         if (id == 0) { // end of table reached, loop back
 318             smartPortIdCnt = 0;
 319             id = frSkyDataIdTable[smartPortIdCnt];
 320         }
 321         smartPortIdCnt++;
 322
 323         int32_t tmpi;
 324         uint32_t tmpui;
 325         static uint8_t t1Cnt = 0;
 326
 327         switch(id) {
 328             case FSSP_DATAID_SPEED      :
 329                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 330                     tmpui = (GPS_speed * 36 + 36 / 2) / 100;
 331                     smartPortSendPackage(id, tmpui); // given in 0.1 m/s, provide in KM/H
 332                     smartPortHasRequest = 0;
 333                 }
 334                 break;
 335             case FSSP_DATAID_VFAS       :
 336                 smartPortSendPackage(id, vbat * 83); // supposedly given in 0.1V, unknown requested unit
 337                 // multiplying by 83 seems to make Taranis read correctly
 338                 smartPortHasRequest = 0;
 339                 break;
 340             case FSSP_DATAID_CURRENT    :
 341                 smartPortSendPackage(id, amperage); // given in 10mA steps, unknown requested unit
 342                 smartPortHasRequest = 0;
 343                 break;
 344             //case FSSP_DATAID_RPM        :
 345             case FSSP_DATAID_ALTITUDE   :
 346                 smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
 347                 smartPortHasRequest = 0;
 348                 break;
 349             case FSSP_DATAID_FUEL       :
 350                 smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
 351                 smartPortHasRequest = 0;
 352                 break;
 353             //case FSSP_DATAID_ADC1       :
 354             //case FSSP_DATAID_ADC2       :
 355             case FSSP_DATAID_LATLONG    :
 356                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 357                     tmpui = 0;
 358                     // the same ID is sent twice, one for longitude, one for latitude
 359                     // the MSB of the sent uint32_t helps FrSky keep track
 360                     // the even/odd bit of our counter helps us keep track
 361                     if (smartPortIdCnt & 1) {
 362                         tmpui = tmpi = GPS_coord[LON];
 363                         if (tmpi < 0) {
 364                             tmpui = -tmpi;
 365                             tmpui |= 0x40000000;
 366                         }
 367                         tmpui |= 0x80000000;
 368                     }
 369                     else {
 370                         tmpui = tmpi = GPS_coord[LAT];
 371                         if (tmpi < 0) {
 372                             tmpui = -tmpi;
 373                             tmpui |= 0x40000000;
 374                         }
 375                     }
 376                     smartPortSendPackage(id, tmpui);
 377                     smartPortHasRequest = 0;
 378                 }
 379                 break;
 380             //case FSSP_DATAID_CAP_USED   :
 381             case FSSP_DATAID_VARIO      :
 382                 smartPortSendPackage(id, vario); // unknown given unit but requested in 100 = 1m/s
 383                 smartPortHasRequest = 0;
 384                 break;
 385             case FSSP_DATAID_HEADING    :
 386                 smartPortSendPackage(id, heading * 100); // given in deg, requested in 10000 = 100 deg
 387                 smartPortHasRequest = 0;
 388                 break;
 389             case FSSP_DATAID_ACCX       :
 390                 smartPortSendPackage(id, accSmooth[X] / 44);
 391                 // unknown input and unknown output unit
 392                 // we can only show 00.00 format, another digit won't display right on Taranis
 393                 // dividing by roughly 44 will give acceleration in G units
 394                 smartPortHasRequest = 0;
 395                 break;
 396             case FSSP_DATAID_ACCY       :
 397                 smartPortSendPackage(id, accSmooth[Y] / 44);
 398                 smartPortHasRequest = 0;
 399                 break;
 400             case FSSP_DATAID_ACCZ       :
 401                 smartPortSendPackage(id, accSmooth[Z] / 44);
 402                 smartPortHasRequest = 0;
 403                 break;
 404             case FSSP_DATAID_T1         :
 405                 // we send all the flags as decimal digits for easy reading
 406
 407                 // the t1Cnt simply allows the telemetry view to show at least some changes
 408                 t1Cnt++;
 409                 if (t1Cnt >= 4) {
 410                     t1Cnt = 1;
 411                 }
 412                 tmpi = t1Cnt * 10000; // start off with at least one digit so the most significant 0 won't be cut off
 413                 // the Taranis seems to be able to fit 5 digits on the screen
 414                 // the Taranis seems to consider this number a signed 16 bit integer
 415
 416                 if (ARMING_FLAG(OK_TO_ARM))
 417                     tmpi += 1;
 418                 if (ARMING_FLAG(PREVENT_ARMING))
 419                     tmpi += 2;
 420                 if (ARMING_FLAG(ARMED))
 421                     tmpi += 4;
 422
 423                 if (FLIGHT_MODE(ANGLE_MODE))
 424                     tmpi += 10;
 425                 if (FLIGHT_MODE(HORIZON_MODE))
 426                     tmpi += 20;
 427                 if (FLIGHT_MODE(AUTOTUNE_MODE))
 428                     tmpi += 40;
 429                 if (FLIGHT_MODE(PASSTHRU_MODE))
 430                     tmpi += 40;
 431
 432                 if (FLIGHT_MODE(MAG_MODE))
 433                     tmpi += 100;
 434                 if (FLIGHT_MODE(BARO_MODE))
 435                     tmpi += 200;
 436                 if (FLIGHT_MODE(SONAR_MODE))
 437                     tmpi += 400;
 438
 439                 if (FLIGHT_MODE(GPS_HOLD_MODE))
 440                     tmpi += 1000;
 441                 if (FLIGHT_MODE(GPS_HOME_MODE))
 442                     tmpi += 2000;
 443                 if (FLIGHT_MODE(HEADFREE_MODE))
 444                     tmpi += 4000;
 445
 446                 smartPortSendPackage(id, (uint32_t)tmpi);
 447                 smartPortHasRequest = 0;
 448                 break;
 449             case FSSP_DATAID_T2         :
 450                 if (sensors(SENSOR_GPS)) {
 451                     // provide GPS lock status
 452                     smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat);
 453                     smartPortHasRequest = 0;
 454                 }
 455                 else {
 456                     smartPortSendPackage(id, 0);
 457                     smartPortHasRequest = 0;
 458                 }
 459                 break;
 460             case FSSP_DATAID_GPS_ALT    :
 461                 if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
 462                     smartPortSendPackage(id, GPS_altitude * 1000); // given in 0.1m , requested in 100 = 1m
 463                     smartPortHasRequest = 0;
 464                 }
 465                 break;
 466             default:
 467                 break;
 468                 // if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just wait for the next loop
 469         }
 470     }
 471 }
 472
 473 #endif