src/main/telemetry/frsky_d.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 #include <math.h>
  26
  27 #include "platform.h"
  28
  29 #if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_FRSKY)
  30
  31 #include "common/maths.h"
  32 #include "common/axis.h"
  33
  34 #include "config/feature.h"
  35
  36 #include "drivers/time.h"
  37 #include "drivers/serial.h"
  38
  39 #include "fc/rc_controls.h"
  40 #include "fc/runtime_config.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/rc_modes.h"
  44
  45 #include "flight/mixer.h"
  46 #include "flight/pid.h"
  47 #include "flight/imu.h"
  48
  49 #include "io/gps.h"
  50 #include "io/serial.h"
  51
  52 #include "navigation/navigation.h"
  53
  54 #include "sensors/acceleration.h"
  55 #include "sensors/barometer.h"
  56 #include "sensors/battery.h"
  57 #include "sensors/gyro.h"
  58 #include "sensors/sensors.h"
  59
  60 #include "telemetry/telemetry.h"
  61 #include "telemetry/frsky_d.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 bool frskyTelemetryEnabled =  false;
  71 static portSharing_e frskyPortSharing;
  72
  73 #define CYCLETIME             125
  74
  75 #define PROTOCOL_HEADER       0x5E
  76 #define PROTOCOL_TAIL         0x5E
  77
  78 // Data Ids  (bp = before decimal point; af = after decimal point)
  79 // Official data IDs
  80 #define ID_GPS_ALTIDUTE_BP    0x01
  81 #define ID_GPS_ALTIDUTE_AP    0x09
  82 #define ID_TEMPRATURE1        0x02
  83 #define ID_RPM                0x03
  84 #define ID_FUEL_LEVEL         0x04
  85 #define ID_TEMPRATURE2        0x05
  86 #define ID_VOLT               0x06
  87 #define ID_ALTITUDE_BP        0x10
  88 #define ID_ALTITUDE_AP        0x21
  89 #define ID_GPS_SPEED_BP       0x11
  90 #define ID_GPS_SPEED_AP       0x19
  91 #define ID_LONGITUDE_BP       0x12
  92 #define ID_LONGITUDE_AP       0x1A
  93 #define ID_E_W                0x22
  94 #define ID_LATITUDE_BP        0x13
  95 #define ID_LATITUDE_AP        0x1B
  96 #define ID_N_S                0x23
  97 #define ID_COURSE_BP          0x14
  98 #define ID_COURSE_AP          0x1C
  99 #define ID_DATE_MONTH         0x15
 100 #define ID_YEAR               0x16
 101 #define ID_HOUR_MINUTE        0x17
 102 #define ID_SECOND             0x18
 103 #define ID_ACC_X              0x24
 104 #define ID_ACC_Y              0x25
 105 #define ID_ACC_Z              0x26
 106 #define ID_VOLTAGE_AMP        0x39
 107 #define ID_VOLTAGE_AMP_BP     0x3A
 108 #define ID_VOLTAGE_AMP_AP     0x3B
 109 #define ID_CURRENT            0x28
 110 // User defined data IDs
 111 #define ID_GYRO_X             0x40
 112 #define ID_GYRO_Y             0x41
 113 #define ID_GYRO_Z             0x42
 114 #define ID_HOME_DIST          0x07
 115 #define ID_PITCH              0x08
 116 #define ID_ROLL               0x20
 117
 118 #define ID_VERT_SPEED         0x30 //opentx vario
 119
 120 #define GPS_BAD_QUALITY       300
 121 #define GPS_MAX_HDOP_VAL      9999
 122 #define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s
 123 #define BLADE_NUMBER_DIVIDER  5 // should set 12 blades in Taranis
 124
 125 static uint32_t lastCycleTime = 0;
 126 static uint8_t cycleNum = 0;
 127 static void sendDataHead(uint8_t id)
 128 {
 129     serialWrite(frskyPort, PROTOCOL_HEADER);
 130     serialWrite(frskyPort, id);
 131 }
 132
 133 static void sendTelemetryTail(void)
 134 {
 135     serialWrite(frskyPort, PROTOCOL_TAIL);
 136 }
 137
 138 static void serializeFrsky(uint8_t data)
 139 {
 140     // take care of byte stuffing
 141     if (data == 0x5e) {
 142         serialWrite(frskyPort, 0x5d);
 143         serialWrite(frskyPort, 0x3e);
 144     } else if (data == 0x5d) {
 145         serialWrite(frskyPort, 0x5d);
 146         serialWrite(frskyPort, 0x3d);
 147     } else
 148         serialWrite(frskyPort, data);
 149 }
 150
 151 static void serialize16(int16_t a)
 152 {
 153     uint8_t t;
 154     t = a;
 155     serializeFrsky(t);
 156     t = a >> 8 & 0xff;
 157     serializeFrsky(t);
 158 }
 159
 160 static void sendAccel(void)
 161 {
 162     int i;
 163
 164     for (i = 0; i < 3; i++) {
 165         sendDataHead(ID_ACC_X + i);
 166         serialize16(lrintf(acc.accADCf[i] * 1000));
 167     }
 168 }
 169
 170 static void sendBaro(void)
 171 {
 172     const int32_t alt = getEstimatedActualPosition(Z);
 173     sendDataHead(ID_ALTITUDE_BP);
 174     serialize16(alt / 100);
 175     sendDataHead(ID_ALTITUDE_AP);
 176     serialize16(ABS(alt % 100));
 177 }
 178
 179 #ifdef USE_GPS
 180 static void sendGpsAltitude(void)
 181 {
 182     uint16_t altitude = gpsSol.llh.alt / 100; // meters
 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 #endif
 193
 194 static void sendThrottleOrBatterySizeAsRpm(void)
 195 {
 196     sendDataHead(ID_RPM);
 197     if (ARMING_FLAG(ARMED)) {
 198         uint16_t throttleForRPM = getThrottlePercent() / BLADE_NUMBER_DIVIDER;
 199         serialize16(throttleForRPM);
 200     } else {
 201         serialize16((currentBatteryProfile->capacity.value / BLADE_NUMBER_DIVIDER));
 202     }
 203
 204 }
 205
 206 static void sendFlightModeAsTemperature1(void)
 207 {
 208     sendDataHead(ID_TEMPRATURE1);
 209     serialize16(frskyGetFlightMode());
 210 }
 211
 212 #ifdef USE_GPS
 213 static void sendSatalliteSignalQualityAsTemperature2(void)
 214 {
 215     sendDataHead(ID_TEMPRATURE2);
 216     serialize16(frskyGetGPSState());
 217 }
 218
 219 static void sendSpeed(void)
 220 {
 221     if (!STATE(GPS_FIX)) {
 222         return;
 223     }
 224     //Speed should be sent in knots (GPS speed is in cm/s)
 225     sendDataHead(ID_GPS_SPEED_BP);
 226     //convert to knots: 1cm/s = 0.0194384449 knots
 227     serialize16(gpsSol.groundSpeed * 1944 / 100000);
 228     sendDataHead(ID_GPS_SPEED_AP);
 229     serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
 230 }
 231
 232 static void sendHomeDistance(void)
 233 {
 234     if (!STATE(GPS_FIX)) {
 235         return;
 236     }
 237     sendDataHead(ID_HOME_DIST);
 238     serialize16(GPS_distanceToHome);
 239 }
 240
 241 #endif
 242
 243 static void sendTime(void)
 244 {
 245     uint32_t seconds = millis() / 1000;
 246     uint8_t minutes = (seconds / 60) % 60;
 247
 248     // if we fly for more than an hour, something's wrong anyway
 249     sendDataHead(ID_HOUR_MINUTE);
 250     serialize16(minutes << 8);
 251     sendDataHead(ID_SECOND);
 252     serialize16(seconds % 60);
 253 }
 254
 255 // Frsky pdf: dddmm.mmmm
 256 // .mmmm is returned in decimal fraction of minutes.
 257 static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
 258 {
 259     int32_t absgps, deg, min;
 260     absgps = ABS(mwiigps);
 261     deg    = absgps / GPS_DEGREES_DIVIDER;
 262     absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;        // absgps = Minutes left * 10^7
 263     min    = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
 264
 265     result->dddmm = deg * ((FRSKY_FORMAT_DMS == telemetryConfig()->frsky_coordinate_format) ? (100) : (60)) + min;
 266
 267     result->mmmm  = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
 268 }
 269
 270 static void sendLatLong(int32_t coord[2])
 271 {
 272     gpsCoordinateDDDMMmmmm_t coordinate;
 273     GPStoDDDMM_MMMM(coord[LAT], &coordinate);
 274     sendDataHead(ID_LATITUDE_BP);
 275     serialize16(coordinate.dddmm);
 276     sendDataHead(ID_LATITUDE_AP);
 277     serialize16(coordinate.mmmm);
 278     sendDataHead(ID_N_S);
 279     serialize16(coord[LAT] < 0 ? 'S' : 'N');
 280
 281     GPStoDDDMM_MMMM(coord[LON], &coordinate);
 282     sendDataHead(ID_LONGITUDE_BP);
 283     serialize16(coordinate.dddmm);
 284     sendDataHead(ID_LONGITUDE_AP);
 285     serialize16(coordinate.mmmm);
 286     sendDataHead(ID_E_W);
 287     serialize16(coord[LON] < 0 ? 'W' : 'E');
 288 }
 289
 290 #ifdef USE_GPS
 291 static void sendFakeLatLong(void)
 292 {
 293     // Heading is only displayed on OpenTX if non-zero lat/long is also sent
 294     int32_t coord[2] = {0,0};
 295
 296     coord[LAT] = (telemetryConfig()->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
 297     coord[LON] = (telemetryConfig()->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
 298
 299     sendLatLong(coord);
 300 }
 301 #endif
 302
 303 static void sendFakeLatLongThatAllowsHeadingDisplay(void)
 304 {
 305     // Heading is only displayed on OpenTX if non-zero lat/long is also sent
 306     int32_t coord[2] = {
 307         1 * GPS_DEGREES_DIVIDER,
 308         1 * GPS_DEGREES_DIVIDER
 309     };
 310
 311     sendLatLong(coord);
 312 }
 313
 314 #ifdef USE_GPS
 315 static void sendGPSLatLong(void)
 316 {
 317     static uint8_t gpsFixOccured = 0;
 318     int32_t coord[2] = {0,0};
 319
 320     if (STATE(GPS_FIX) || gpsFixOccured == 1) {
 321         // If we have ever had a fix, send the last known lat/long
 322         gpsFixOccured = 1;
 323         coord[LAT] = gpsSol.llh.lat;
 324         coord[LON] = gpsSol.llh.lon;
 325         sendLatLong(coord);
 326     } else {
 327         // otherwise send fake lat/long in order to display compass value
 328         sendFakeLatLong();
 329     }
 330 }
 331 #endif
 332
 333 /*
 334  * Send vertical speed for opentx. ID_VERT_SPEED
 335  * Unit is cm/s
 336  */
 337 static void sendVario(void)
 338 {
 339     sendDataHead(ID_VERT_SPEED);
 340     serialize16((int16_t)lrintf(getEstimatedActualVelocity(Z)));
 341 }
 342
 343 /*
 344  * Send voltage via ID_VOLT
 345  *
 346  * NOTE: This sends voltage divided by batteryCellCount. To get the real
 347  * battery voltage, you need to multiply the value by batteryCellCount.
 348  */
 349 static void sendVoltage(void)
 350 {
 351     uint32_t cellVoltage;
 352     uint16_t payload;
 353
 354     /*
 355      * Format for Voltage Data for single cells is like this:
 356      *
 357      *  llll llll cccc hhhh
 358      *  l: Low voltage bits
 359      *  h: High voltage bits
 360      *  c: Cell number (starting at 0)
 361      *
 362      * The actual value sent for cell voltage has resolution of 0.002 volts
 363      * Since vbat has resolution of 0.01 volts it has to be multiplied by 5
 364      */
 365     cellVoltage = ((uint32_t)getBatteryVoltage() * 10) / (getBatteryCellCount() * 2);
 366
 367     // Cell number is at bit 9-12 (only uses vbat, so it can't send individual cell voltages, set cell number to 0)
 368     payload = 0;
 369
 370     // Lower voltage bits are at bit 0-8
 371     payload |= ((cellVoltage & 0x0ff) << 8);
 372
 373     // Higher voltage bits are at bits 13-15
 374     payload |= ((cellVoltage & 0xf00) >> 8);
 375
 376     sendDataHead(ID_VOLT);
 377     serialize16(payload);
 378 }
 379
 380 /*
 381  * Send voltage with ID_VOLTAGE_AMP
 382  */
 383 static void sendVoltageAmp(void)
 384 {
 385     uint16_t vbat = getBatteryVoltage();
 386     if (telemetryConfig()->frsky_vfas_precision == FRSKY_VFAS_PRECISION_HIGH) {
 387         /*
 388          * Use new ID 0x39 to send voltage directly in 0.1 volts resolution
 389          */
 390         sendDataHead(ID_VOLTAGE_AMP);
 391         serialize16(vbat / 10);
 392     } else {
 393         uint16_t voltage = (vbat * 11) / 21;
 394         uint16_t vfasVoltage;
 395         if (telemetryConfig()->report_cell_voltage) {
 396             vfasVoltage = voltage / getBatteryCellCount();
 397         } else {
 398             vfasVoltage = voltage;
 399         }
 400         sendDataHead(ID_VOLTAGE_AMP_BP);
 401         serialize16(vfasVoltage / 100);
 402         sendDataHead(ID_VOLTAGE_AMP_AP);
 403         serialize16(((vfasVoltage % 100) + 5) / 10);
 404     }
 405 }
 406
 407 static void sendAmperage(void)
 408 {
 409     sendDataHead(ID_CURRENT);
 410     serialize16((uint16_t)(getAmperage() / 10));
 411 }
 412
 413 static void sendFuelLevel(void)
 414 {
 415     if (telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_PERCENT) {
 416         sendDataHead(ID_FUEL_LEVEL);
 417         serialize16((uint16_t)calculateBatteryPercentage());
 418     } else if (isAmperageConfigured()) {
 419         sendDataHead(ID_FUEL_LEVEL);
 420         serialize16((uint16_t)telemetryConfig()->smartportFuelUnit == SMARTPORT_FUEL_UNIT_MAH ? getMAhDrawn() : getMWhDrawn());
 421     }
 422 }
 423
 424 static void sendHeading(void)
 425 {
 426     sendDataHead(ID_COURSE_BP);
 427     serialize16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 428     sendDataHead(ID_COURSE_AP);
 429     serialize16(0);
 430 }
 431
 432 static void sendPitch(void)
 433 {
 434     sendDataHead(ID_PITCH);
 435     serialize16(attitude.values.pitch);
 436 }
 437
 438 static void sendRoll(void)
 439 {
 440     sendDataHead(ID_ROLL);
 441     serialize16(attitude.values.roll);
 442 }
 443
 444 void initFrSkyTelemetry(void)
 445 {
 446     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_FRSKY);
 447     frskyPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_FRSKY);
 448 }
 449
 450 void freeFrSkyTelemetryPort(void)
 451 {
 452     closeSerialPort(frskyPort);
 453     frskyPort = NULL;
 454     frskyTelemetryEnabled = false;
 455 }
 456
 457 void configureFrSkyTelemetryPort(void)
 458 {
 459     if (!portConfig) {
 460         return;
 461     }
 462
 463     frskyPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_FRSKY, NULL, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 464     if (!frskyPort) {
 465         return;
 466     }
 467
 468     frskyTelemetryEnabled = true;
 469 }
 470
 471 bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
 472 {
 473     return currentMillis - lastCycleTime >= CYCLETIME;
 474 }
 475
 476 void checkFrSkyTelemetryState(void)
 477 {
 478     if (portConfig && telemetryCheckRxPortShared(portConfig)) {
 479         if (!frskyTelemetryEnabled && telemetrySharedPort != NULL) {
 480             frskyPort = telemetrySharedPort;
 481             frskyTelemetryEnabled = true;
 482         }
 483     } else {
 484         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(frskyPortSharing);
 485
 486         if (newTelemetryEnabledValue == frskyTelemetryEnabled) {
 487             return;
 488         }
 489
 490         if (newTelemetryEnabledValue)
 491             configureFrSkyTelemetryPort();
 492         else
 493             freeFrSkyTelemetryPort();
 494     }
 495 }
 496
 497 void handleFrSkyTelemetry(void)
 498 {
 499     if (!frskyTelemetryEnabled) {
 500         return;
 501     }
 502
 503     uint32_t now = millis();
 504
 505     if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
 506         return;
 507     }
 508
 509     lastCycleTime = now;
 510
 511     cycleNum++;
 512
 513     // Sent every 125ms
 514     if (telemetryConfig()->frsky_pitch_roll) {
 515         sendPitch();
 516         sendRoll();
 517     } else {
 518         sendAccel();
 519     }
 520     sendVario();
 521     sendTelemetryTail();
 522
 523     if ((cycleNum % 4) == 0) {      // Sent every 500ms
 524         if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
 525             sendBaro();
 526         }
 527         sendHeading();
 528         sendTelemetryTail();
 529     }
 530
 531     if ((cycleNum % 8) == 0) {      // Sent every 1s
 532         sendFlightModeAsTemperature1();
 533         sendThrottleOrBatterySizeAsRpm();
 534
 535         if (feature(FEATURE_VBAT)) {
 536             sendVoltage();
 537             sendVoltageAmp();
 538             sendAmperage();
 539             sendFuelLevel();
 540         }
 541
 542 #ifdef USE_GPS
 543         if (sensors(SENSOR_GPS)) {
 544             sendSpeed();
 545             sendHomeDistance();
 546             sendGpsAltitude();
 547             sendSatalliteSignalQualityAsTemperature2();
 548             sendGPSLatLong();
 549         }
 550         else {
 551             sendFakeLatLongThatAllowsHeadingDisplay();
 552         }
 553 #else
 554         sendFakeLatLongThatAllowsHeadingDisplay();
 555 #endif
 556
 557         sendTelemetryTail();
 558     }
 559
 560     if (cycleNum == 40) {     //Frame 3: Sent every 5s
 561         cycleNum = 0;
 562         sendTime();
 563         sendTelemetryTail();
 564     }
 565 }
 566
 567 #endif