src/main/telemetry/frsky.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 /*
  19  * Initial FrSky Telemetry implementation by silpstream @ rcgroups.
  20  * Addition protocol work by airmamaf @ github.
  21  */
  22 #include <stdbool.h>
  23 #include <stdint.h>
  24 #include <stdlib.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef TELEMETRY
  29
  30 #include "common/maths.h"
  31 #include "common/axis.h"
  32
  33 #include "drivers/system.h"
  34 #include "drivers/sensor.h"
  35 #include "drivers/accgyro.h"
  36 #include "drivers/gpio.h"
  37 #include "drivers/timer.h"
  38 #include "drivers/serial.h"
  39
  40
  41 #include "sensors/sensors.h"
  42 #include "sensors/acceleration.h"
  43 #include "sensors/gyro.h"
  44 #include "sensors/barometer.h"
  45 #include "sensors/battery.h"
  46
  47 #include "io/serial.h"
  48 #include "io/rc_controls.h"
  49 #include "io/gps.h"
  50
  51 #include "rx/rx.h"
  52
  53 #include "flight/mixer.h"
  54 #include "flight/pid.h"
  55 #include "flight/imu.h"
  56 #include "flight/altitudehold.h"
  57
  58 #include "config/runtime_config.h"
  59 #include "config/config.h"
  60
  61 #include "telemetry/telemetry.h"
  62 #include "telemetry/frsky.h"
  63
  64 static serialPort_t *frskyPort = NULL;
  65 static serialPortConfig_t *portConfig;
  66
  67 #define FRSKY_BAUDRATE 9600
  68 #define FRSKY_INITIAL_PORT_MODE MODE_TX
  69
  70 static telemetryConfig_t *telemetryConfig;
  71 static bool frskyTelemetryEnabled =  false;
  72 static portSharing_e frskyPortSharing;
  73
  74
  75 extern batteryConfig_t *batteryConfig;
  76
  77 extern int16_t telemTemperature1; // FIXME dependency on mw.c
  78
  79 #define CYCLETIME             125
  80
  81 #define PROTOCOL_HEADER       0x5E
  82 #define PROTOCOL_TAIL         0x5E
  83
  84 // Data Ids  (bp = before decimal point; af = after decimal point)
  85 // Official data IDs
  86 #define ID_GPS_ALTIDUTE_BP    0x01
  87 #define ID_GPS_ALTIDUTE_AP    0x09
  88 #define ID_TEMPRATURE1        0x02
  89 #define ID_RPM                0x03
  90 #define ID_FUEL_LEVEL         0x04
  91 #define ID_TEMPRATURE2        0x05
  92 #define ID_VOLT               0x06
  93 #define ID_ALTITUDE_BP        0x10
  94 #define ID_ALTITUDE_AP        0x21
  95 #define ID_GPS_SPEED_BP       0x11
  96 #define ID_GPS_SPEED_AP       0x19
  97 #define ID_LONGITUDE_BP       0x12
  98 #define ID_LONGITUDE_AP       0x1A
  99 #define ID_E_W                0x22
 100 #define ID_LATITUDE_BP        0x13
 101 #define ID_LATITUDE_AP        0x1B
 102 #define ID_N_S                0x23
 103 #define ID_COURSE_BP          0x14
 104 #define ID_COURSE_AP          0x1C
 105 #define ID_DATE_MONTH         0x15
 106 #define ID_YEAR               0x16
 107 #define ID_HOUR_MINUTE        0x17
 108 #define ID_SECOND             0x18
 109 #define ID_ACC_X              0x24
 110 #define ID_ACC_Y              0x25
 111 #define ID_ACC_Z              0x26
 112 #define ID_VOLTAGE_AMP_BP     0x3A
 113 #define ID_VOLTAGE_AMP_AP     0x3B
 114 #define ID_CURRENT            0x28
 115 // User defined data IDs
 116 #define ID_GYRO_X             0x40
 117 #define ID_GYRO_Y             0x41
 118 #define ID_GYRO_Z             0x42
 119
 120 #define ID_VERT_SPEED         0x30 //opentx vario
 121
 122 #define GPS_BAD_QUALITY       300
 123 #define GPS_MAX_HDOP_VAL      9999
 124 #define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s
 125 #define BLADE_NUMBER_DIVIDER  5 // should set 12 blades in Taranis
 126
 127 static uint32_t lastCycleTime = 0;
 128 static uint8_t cycleNum = 0;
 129 static void sendDataHead(uint8_t id)
 130 {
 131     serialWrite(frskyPort, PROTOCOL_HEADER);
 132     serialWrite(frskyPort, id);
 133 }
 134
 135 static void sendTelemetryTail(void)
 136 {
 137     serialWrite(frskyPort, PROTOCOL_TAIL);
 138 }
 139
 140 static void serializeFrsky(uint8_t data)
 141 {
 142     // take care of byte stuffing
 143     if (data == 0x5e) {
 144         serialWrite(frskyPort, 0x5d);
 145         serialWrite(frskyPort, 0x3e);
 146     } else if (data == 0x5d) {
 147         serialWrite(frskyPort, 0x5d);
 148         serialWrite(frskyPort, 0x3d);
 149     } else
 150         serialWrite(frskyPort, data);
 151 }
 152
 153 static void serialize16(int16_t a)
 154 {
 155     uint8_t t;
 156     t = a;
 157     serializeFrsky(t);
 158     t = a >> 8 & 0xff;
 159     serializeFrsky(t);
 160 }
 161
 162 static void sendAccel(void)
 163 {
 164     int i;
 165
 166     for (i = 0; i < 3; i++) {
 167         sendDataHead(ID_ACC_X + i);
 168         serialize16(((float)accSmooth[i] / acc_1G) * 1000);
 169     }
 170 }
 171
 172 static void sendBaro(void)
 173 {
 174     sendDataHead(ID_ALTITUDE_BP);
 175     serialize16(BaroAlt / 100);
 176     sendDataHead(ID_ALTITUDE_AP);
 177     serialize16(ABS(BaroAlt % 100));
 178 }
 179
 180 static void sendGpsAltitude(void)
 181 {
 182     uint16_t altitude = GPS_altitude;
 183     //Send real GPS altitude only if it's reliable (there's a GPS fix)
 184     if (!STATE(GPS_FIX)) {
 185         altitude = 0;
 186     }
 187     sendDataHead(ID_GPS_ALTIDUTE_BP);
 188     serialize16(altitude);
 189     sendDataHead(ID_GPS_ALTIDUTE_AP);
 190     serialize16(0);
 191 }
 192
 193
 194 static void sendThrottleOrBatterySizeAsRpm(void)
 195 {
 196     sendDataHead(ID_RPM);
 197     if (ARMING_FLAG(ARMED)) {
 198         serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);
 199     } else {
 200         serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
 201     }
 202
 203 }
 204
 205 static void sendTemperature1(void)
 206 {
 207     sendDataHead(ID_TEMPRATURE1);
 208 #ifdef BARO
 209     serialize16((baroTemperature + 50)/ 100); //Airmamaf
 210 #else
 211     serialize16(telemTemperature1 / 10);
 212 #endif
 213 }
 214
 215 static void sendSatalliteSignalQualityAsTemperature2(void)
 216 {
 217     uint16_t satellite = GPS_numSat;
 218     if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
 219         satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
 220     }
 221     sendDataHead(ID_TEMPRATURE2);
 222
 223     if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
 224         serialize16(satellite);
 225     } else {
 226         float tmp = (satellite - 32) / 1.8;
 227         //Round the value
 228         tmp += (tmp < 0) ? -0.5f : 0.5f;
 229         serialize16(tmp);
 230     }
 231 }
 232
 233 static void sendSpeed(void)
 234 {
 235     if (!STATE(GPS_FIX)) {
 236         return;
 237     }
 238     //Speed should be sent in m/s (GPS speed is in cm/s)
 239     sendDataHead(ID_GPS_SPEED_BP);
 240     serialize16((GPS_speed * 0.01 + 0.5));
 241     sendDataHead(ID_GPS_SPEED_AP);
 242     serialize16(0); //Not dipslayed
 243 }
 244
 245 static void sendTime(void)
 246 {
 247     uint32_t seconds = millis() / 1000;
 248     uint8_t minutes = (seconds / 60) % 60;
 249
 250     // if we fly for more than an hour, something's wrong anyway
 251     sendDataHead(ID_HOUR_MINUTE);
 252     serialize16(minutes << 8);
 253     sendDataHead(ID_SECOND);
 254     serialize16(seconds % 60);
 255 }
 256
 257 #ifdef GPS
 258 // Frsky pdf: dddmm.mmmm
 259 // .mmmm is returned in decimal fraction of minutes.
 260 static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
 261 {
 262     int32_t absgps, deg, min;
 263     absgps = ABS(mwiigps);
 264     deg    = absgps / GPS_DEGREES_DIVIDER;
 265     absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;        // absgps = Minutes left * 10^7
 266     min    = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
 267
 268     if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
 269         result->dddmm = deg * 100 + min;
 270     } else {
 271         result->dddmm = deg * 60 + min;
 272     }
 273
 274     result->mmmm  = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
 275 }
 276
 277 static void sendGPS(void)
 278 {
 279     int32_t localGPS_coord[2] = {0,0};
 280     // Don't set dummy GPS data, if we already had a GPS fix
 281     // it can be usefull to keep last valid coordinates
 282     static uint8_t gpsFixOccured = 0;
 283
 284     //Dummy data if no 3D fix, this way we can display heading in Taranis
 285     if (STATE(GPS_FIX) || gpsFixOccured == 1) {
 286         localGPS_coord[LAT] = GPS_coord[LAT];
 287         localGPS_coord[LON] = GPS_coord[LON];
 288         gpsFixOccured = 1;
 289     } else {
 290         // Send dummy GPS Data in order to display compass value
 291         localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
 292         localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
 293     }
 294
 295     gpsCoordinateDDDMMmmmm_t coordinate;
 296     GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
 297     sendDataHead(ID_LATITUDE_BP);
 298     serialize16(coordinate.dddmm);
 299     sendDataHead(ID_LATITUDE_AP);
 300     serialize16(coordinate.mmmm);
 301     sendDataHead(ID_N_S);
 302     serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
 303
 304     GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
 305     sendDataHead(ID_LONGITUDE_BP);
 306     serialize16(coordinate.dddmm);
 307     sendDataHead(ID_LONGITUDE_AP);
 308     serialize16(coordinate.mmmm);
 309     sendDataHead(ID_E_W);
 310     serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
 311 }
 312 #endif
 313
 314
 315 /*
 316  * Send vertical speed for opentx. ID_VERT_SPEED
 317  * Unit is cm/s
 318  */
 319 static void sendVario(void)
 320 {
 321     sendDataHead(ID_VERT_SPEED);
 322     serialize16(vario);
 323 }
 324
 325 /*
 326  * Send voltage via ID_VOLT
 327  *
 328  * NOTE: This sends voltage divided by batteryCellCount. To get the real
 329  * battery voltage, you need to multiply the value by batteryCellCount.
 330  */
 331 static void sendVoltage(void)
 332 {
 333     static uint16_t currentCell = 0;
 334     uint16_t cellNumber;
 335     uint32_t cellVoltage;
 336     uint16_t payload;
 337
 338     /*
 339      * Format for Voltage Data for single cells is like this:
 340      *
 341      *  llll llll cccc hhhh
 342      *  l: Low voltage bits
 343      *  h: High voltage bits
 344      *  c: Cell number (starting at 0)
 345      */
 346     cellVoltage = vbat / batteryCellCount;
 347
 348     // Map to 12 bit range
 349     cellVoltage = (cellVoltage * 2100) / 42;
 350
 351     cellNumber = currentCell % batteryCellCount;
 352
 353     // Cell number is at bit 9-12
 354     payload = (cellNumber << 4);
 355
 356     // Lower voltage bits are at bit 0-8
 357     payload |= ((cellVoltage & 0x0ff) << 8);
 358
 359     // Higher voltage bits are at bits 13-15
 360     payload |= ((cellVoltage & 0xf00) >> 8);
 361
 362     sendDataHead(ID_VOLT);
 363     serialize16(payload);
 364
 365     currentCell++;
 366     currentCell %= batteryCellCount;
 367 }
 368
 369 /*
 370  * Send voltage with ID_VOLTAGE_AMP
 371  */
 372 static void sendVoltageAmp(void)
 373 {
 374     uint16_t voltage = (vbat * 110) / 21;
 375
 376     sendDataHead(ID_VOLTAGE_AMP_BP);
 377     serialize16(voltage / 100);
 378     sendDataHead(ID_VOLTAGE_AMP_AP);
 379     serialize16(((voltage % 100) + 5) / 10);
 380 }
 381
 382 static void sendAmperage(void)
 383 {
 384     sendDataHead(ID_CURRENT);
 385     serialize16((uint16_t)(amperage / 10));
 386 }
 387
 388 static void sendFuelLevel(void)
 389 {
 390     sendDataHead(ID_FUEL_LEVEL);
 391
 392     if (batteryConfig->batteryCapacity > 0) {
 393         serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage());
 394     } else {
 395         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
 396     }
 397 }
 398
 399 static void sendHeading(void)
 400 {
 401     sendDataHead(ID_COURSE_BP);
 402     serialize16(heading);
 403     sendDataHead(ID_COURSE_AP);
 404     serialize16(0);
 405 }
 406
 407 void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
 408 {
 409     telemetryConfig = initialTelemetryConfig;
 410     portConfig = findSerialPortConfig(FUNCTION_FRSKY_TELEMETRY);
 411     frskyPortSharing = determinePortSharing(portConfig, FUNCTION_FRSKY_TELEMETRY);
 412 }
 413
 414 void freeFrSkyTelemetryPort(void)
 415 {
 416     closeSerialPort(frskyPort);
 417     frskyPort = NULL;
 418     frskyTelemetryEnabled = false;
 419 }
 420
 421 void configureFrSkyTelemetryPort(void)
 422 {
 423     if (!portConfig) {
 424         return;
 425     }
 426
 427     frskyPort = openSerialPort(portConfig->identifier, FUNCTION_FRSKY_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion);
 428     if (!frskyPort) {
 429         return;
 430     }
 431
 432     frskyTelemetryEnabled = true;
 433 }
 434
 435
 436 bool canSendFrSkyTelemetry(void)
 437 {
 438     return frskyPort && serialTotalBytesWaiting(frskyPort) == 0;
 439 }
 440
 441 bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
 442 {
 443     return currentMillis - lastCycleTime >= CYCLETIME;
 444 }
 445
 446 void checkFrSkyTelemetryState(void)
 447 {
 448     bool newTelemetryEnabledValue = determineNewTelemetryEnabledState(frskyPortSharing);
 449
 450     if (newTelemetryEnabledValue == frskyTelemetryEnabled) {
 451         return;
 452     }
 453
 454     if (newTelemetryEnabledValue)
 455         configureFrSkyTelemetryPort();
 456     else
 457         freeFrSkyTelemetryPort();
 458 }
 459
 460 void handleFrSkyTelemetry(void)
 461 {
 462     if (!frskyTelemetryEnabled) {
 463         return;
 464     }
 465
 466     if (!canSendFrSkyTelemetry()) {
 467         return;
 468     }
 469
 470     uint32_t now = millis();
 471
 472     if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
 473         return;
 474     }
 475
 476     lastCycleTime = now;
 477
 478     cycleNum++;
 479
 480     // Sent every 125ms
 481     sendAccel();
 482     sendVario();
 483     sendTelemetryTail();
 484
 485     if ((cycleNum % 4) == 0) {      // Sent every 500ms
 486         if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
 487             sendBaro();
 488         }
 489         sendHeading();
 490         sendTelemetryTail();
 491     }
 492
 493     if ((cycleNum % 8) == 0) {      // Sent every 1s
 494         sendTemperature1();
 495         sendThrottleOrBatterySizeAsRpm();
 496
 497         if (feature(FEATURE_VBAT)) {
 498             sendVoltage();
 499             sendVoltageAmp();
 500             sendAmperage();
 501             sendFuelLevel();
 502         }
 503
 504 #ifdef GPS
 505         if (sensors(SENSOR_GPS)) {
 506             sendSpeed();
 507             sendGpsAltitude();
 508             sendSatalliteSignalQualityAsTemperature2();
 509         }
 510 #endif
 511
 512         //  Send GPS information to display compass information
 513         if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
 514             sendGPS();
 515         }
 516         sendTelemetryTail();
 517     }
 518
 519     if (cycleNum == 40) {     //Frame 3: Sent every 5s
 520         cycleNum = 0;
 521         sendTime();
 522         sendTelemetryTail();
 523     }
 524 }
 525
 526 #endif