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