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;
  65 #define FRSKY_BAUDRATE 9600
  66 #define FRSKY_INITIAL_PORT_MODE MODE_TX
  67
  68 static telemetryConfig_t *telemetryConfig;
  69
  70 extern batteryConfig_t *batteryConfig;
  71
  72 extern int16_t telemTemperature1; // FIXME dependency on mw.c
  73
  74 #define CYCLETIME             125
  75
  76 #define PROTOCOL_HEADER       0x5E
  77 #define PROTOCOL_TAIL         0x5E
  78
  79 // Data Ids  (bp = before decimal point; af = after decimal point)
  80 // Official data IDs
  81 #define ID_GPS_ALTIDUTE_BP    0x01
  82 #define ID_GPS_ALTIDUTE_AP    0x09
  83 #define ID_TEMPRATURE1        0x02
  84 #define ID_RPM                0x03
  85 #define ID_FUEL_LEVEL         0x04
  86 #define ID_TEMPRATURE2        0x05
  87 #define ID_VOLT               0x06
  88 #define ID_ALTITUDE_BP        0x10
  89 #define ID_ALTITUDE_AP        0x21
  90 #define ID_GPS_SPEED_BP       0x11
  91 #define ID_GPS_SPEED_AP       0x19
  92 #define ID_LONGITUDE_BP       0x12
  93 #define ID_LONGITUDE_AP       0x1A
  94 #define ID_E_W                0x22
  95 #define ID_LATITUDE_BP        0x13
  96 #define ID_LATITUDE_AP        0x1B
  97 #define ID_N_S                0x23
  98 #define ID_COURSE_BP          0x14
  99 #define ID_COURSE_AP          0x1C
 100 #define ID_DATE_MONTH         0x15
 101 #define ID_YEAR               0x16
 102 #define ID_HOUR_MINUTE        0x17
 103 #define ID_SECOND             0x18
 104 #define ID_ACC_X              0x24
 105 #define ID_ACC_Y              0x25
 106 #define ID_ACC_Z              0x26
 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
 115 #define ID_VERT_SPEED         0x30 //opentx vario
 116
 117 #define GPS_BAD_QUALITY       300
 118 #define GPS_MAX_HDOP_VAL      9999
 119 #define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s
 120 #define BLADE_NUMBER_DIVIDER  5 // should set 12 blades in Taranis
 121
 122 static uint32_t lastCycleTime = 0;
 123 static uint8_t cycleNum = 0;
 124 static void sendDataHead(uint8_t id)
 125 {
 126     serialWrite(frskyPort, PROTOCOL_HEADER);
 127     serialWrite(frskyPort, id);
 128 }
 129
 130 static void sendTelemetryTail(void)
 131 {
 132     serialWrite(frskyPort, PROTOCOL_TAIL);
 133 }
 134
 135 static void serializeFrsky(uint8_t data)
 136 {
 137     // take care of byte stuffing
 138     if (data == 0x5e) {
 139         serialWrite(frskyPort, 0x5d);
 140         serialWrite(frskyPort, 0x3e);
 141     } else if (data == 0x5d) {
 142         serialWrite(frskyPort, 0x5d);
 143         serialWrite(frskyPort, 0x3d);
 144     } else
 145         serialWrite(frskyPort, data);
 146 }
 147
 148 static void serialize16(int16_t a)
 149 {
 150     uint8_t t;
 151     t = a;
 152     serializeFrsky(t);
 153     t = a >> 8 & 0xff;
 154     serializeFrsky(t);
 155 }
 156
 157 static void sendAccel(void)
 158 {
 159     int i;
 160
 161     for (i = 0; i < 3; i++) {
 162         sendDataHead(ID_ACC_X + i);
 163         serialize16(((float)accSmooth[i] / acc_1G) * 1000);
 164     }
 165 }
 166
 167 static void sendBaro(void)
 168 {
 169     sendDataHead(ID_ALTITUDE_BP);
 170     serialize16(BaroAlt / 100);
 171     sendDataHead(ID_ALTITUDE_AP);
 172     serialize16(ABS(BaroAlt % 100));
 173 }
 174
 175 static void sendGpsAltitude(void)
 176 {
 177     uint16_t altitude = GPS_altitude;
 178     //Send real GPS altitude only if it's reliable (there's a GPS fix)
 179     if (!STATE(GPS_FIX)) {
 180         altitude = 0;
 181     }
 182     sendDataHead(ID_GPS_ALTIDUTE_BP);
 183     serialize16(altitude);
 184     sendDataHead(ID_GPS_ALTIDUTE_AP);
 185     serialize16(0);
 186 }
 187
 188
 189 static void sendThrottleOrBatterySizeAsRpm(void)
 190 {
 191     sendDataHead(ID_RPM);
 192     if (ARMING_FLAG(ARMED)) {
 193         serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);
 194     } else {
 195         serialize16((batteryConfig->batteryCapacity / BLADE_NUMBER_DIVIDER));
 196     }
 197
 198 }
 199
 200 static void sendTemperature1(void)
 201 {
 202     sendDataHead(ID_TEMPRATURE1);
 203 #ifdef BARO
 204     serialize16((baroTemperature + 50)/ 100); //Airmamaf
 205 #else
 206     serialize16(telemTemperature1 / 10);
 207 #endif
 208 }
 209
 210 static void sendSatalliteSignalQualityAsTemperature2(void)
 211 {
 212     uint16_t satellite = GPS_numSat;
 213     if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
 214         satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
 215     }
 216     sendDataHead(ID_TEMPRATURE2);
 217
 218     if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
 219         serialize16(satellite);
 220     } else {
 221         float tmp = (satellite - 32) / 1.8;
 222         //Round the value
 223         tmp += (tmp < 0) ? -0.5f : 0.5f;
 224         serialize16(tmp);
 225     }
 226 }
 227
 228 static void sendSpeed(void)
 229 {
 230     if (!STATE(GPS_FIX)) {
 231         return;
 232     }
 233     //Speed should be sent in m/s (GPS speed is in cm/s)
 234     sendDataHead(ID_GPS_SPEED_BP);
 235     serialize16((GPS_speed * 0.01 + 0.5));
 236     sendDataHead(ID_GPS_SPEED_AP);
 237     serialize16(0); //Not dipslayed
 238 }
 239
 240 static void sendTime(void)
 241 {
 242     uint32_t seconds = millis() / 1000;
 243     uint8_t minutes = (seconds / 60) % 60;
 244
 245     // if we fly for more than an hour, something's wrong anyway
 246     sendDataHead(ID_HOUR_MINUTE);
 247     serialize16(minutes << 8);
 248     sendDataHead(ID_SECOND);
 249     serialize16(seconds % 60);
 250 }
 251
 252 #ifdef GPS
 253 // Frsky pdf: dddmm.mmmm
 254 // .mmmm is returned in decimal fraction of minutes.
 255 static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
 256 {
 257     int32_t absgps, deg, min;
 258     absgps = ABS(mwiigps);
 259     deg    = absgps / GPS_DEGREES_DIVIDER;
 260     absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60;        // absgps = Minutes left * 10^7
 261     min    = absgps / GPS_DEGREES_DIVIDER;                     // minutes left
 262
 263     if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
 264         result->dddmm = deg * 100 + min;
 265     } else {
 266         result->dddmm = deg * 60 + min;
 267     }
 268
 269     result->mmmm  = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
 270 }
 271
 272 static void sendGPS(void)
 273 {
 274     int32_t localGPS_coord[2] = {0,0};
 275     // Don't set dummy GPS data, if we already had a GPS fix
 276     // it can be usefull to keep last valid coordinates
 277     static uint8_t gpsFixOccured = 0;
 278
 279     //Dummy data if no 3D fix, this way we can display heading in Taranis
 280     if (STATE(GPS_FIX) || gpsFixOccured == 1) {
 281         localGPS_coord[LAT] = GPS_coord[LAT];
 282         localGPS_coord[LON] = GPS_coord[LON];
 283         gpsFixOccured = 1;
 284     } else {
 285         // Send dummy GPS Data in order to display compass value
 286         localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
 287         localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
 288     }
 289
 290     gpsCoordinateDDDMMmmmm_t coordinate;
 291     GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
 292     sendDataHead(ID_LATITUDE_BP);
 293     serialize16(coordinate.dddmm);
 294     sendDataHead(ID_LATITUDE_AP);
 295     serialize16(coordinate.mmmm);
 296     sendDataHead(ID_N_S);
 297     serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
 298
 299     GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
 300     sendDataHead(ID_LONGITUDE_BP);
 301     serialize16(coordinate.dddmm);
 302     sendDataHead(ID_LONGITUDE_AP);
 303     serialize16(coordinate.mmmm);
 304     sendDataHead(ID_E_W);
 305     serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
 306 }
 307 #endif
 308
 309
 310 /*
 311  * Send vertical speed for opentx. ID_VERT_SPEED
 312  * Unit is cm/s
 313  */
 314 static void sendVario(void)
 315 {
 316     sendDataHead(ID_VERT_SPEED);
 317     serialize16(vario);
 318 }
 319
 320 /*
 321  * Send voltage via ID_VOLT
 322  *
 323  * NOTE: This sends voltage divided by batteryCellCount. To get the real
 324  * battery voltage, you need to multiply the value by batteryCellCount.
 325  */
 326 static void sendVoltage(void)
 327 {
 328     static uint16_t currentCell = 0;
 329     uint16_t cellNumber;
 330     uint32_t cellVoltage;
 331     uint16_t payload;
 332
 333     /*
 334      * Format for Voltage Data for single cells is like this:
 335      *
 336      *  llll llll cccc hhhh
 337      *  l: Low voltage bits
 338      *  h: High voltage bits
 339      *  c: Cell number (starting at 0)
 340      */
 341     cellVoltage = vbat / batteryCellCount;
 342
 343     // Map to 12 bit range
 344     cellVoltage = (cellVoltage * 2100) / 42;
 345
 346     cellNumber = currentCell % batteryCellCount;
 347
 348     // Cell number is at bit 9-12
 349     payload = (cellNumber << 4);
 350
 351     // Lower voltage bits are at bit 0-8
 352     payload |= ((cellVoltage & 0x0ff) << 8);
 353
 354     // Higher voltage bits are at bits 13-15
 355     payload |= ((cellVoltage & 0xf00) >> 8);
 356
 357     sendDataHead(ID_VOLT);
 358     serialize16(payload);
 359
 360     currentCell++;
 361     currentCell %= batteryCellCount;
 362 }
 363
 364 /*
 365  * Send voltage with ID_VOLTAGE_AMP
 366  */
 367 static void sendVoltageAmp(void)
 368 {
 369     uint16_t voltage = (vbat * 110) / 21;
 370
 371     sendDataHead(ID_VOLTAGE_AMP_BP);
 372     serialize16(voltage / 100);
 373     sendDataHead(ID_VOLTAGE_AMP_AP);
 374     serialize16(((voltage % 100) + 5) / 10);
 375 }
 376
 377 static void sendAmperage(void)
 378 {
 379     sendDataHead(ID_CURRENT);
 380     serialize16((uint16_t)(amperage / 10));
 381 }
 382
 383 static void sendFuelLevel(void)
 384 {
 385     sendDataHead(ID_FUEL_LEVEL);
 386
 387     if (batteryConfig->batteryCapacity > 0) {
 388         serialize16((uint16_t)calculateBatteryCapacityRemainingPercentage());
 389     } else {
 390         serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
 391     }
 392 }
 393
 394 static void sendHeading(void)
 395 {
 396     sendDataHead(ID_COURSE_BP);
 397     serialize16(heading);
 398     sendDataHead(ID_COURSE_AP);
 399     serialize16(0);
 400 }
 401
 402 void initFrSkyTelemetry(telemetryConfig_t *initialTelemetryConfig)
 403 {
 404     telemetryConfig = initialTelemetryConfig;
 405 }
 406
 407 static portMode_t previousPortMode;
 408 static uint32_t previousBaudRate;
 409
 410 void freeFrSkyTelemetryPort(void)
 411 {
 412     // FIXME only need to reset the port if the port is shared
 413     serialSetMode(frskyPort, previousPortMode);
 414     serialSetBaudRate(frskyPort, previousBaudRate);
 415
 416     endSerialPortFunction(frskyPort, FUNCTION_TELEMETRY);
 417 }
 418
 419 void configureFrSkyTelemetryPort(void)
 420 {
 421     frskyPort = findOpenSerialPort(FUNCTION_TELEMETRY);
 422     if (frskyPort) {
 423         previousPortMode = frskyPort->mode;
 424         previousBaudRate = frskyPort->baudRate;
 425
 426         //waitForSerialPortToFinishTransmitting(frskyPort); // FIXME locks up the system
 427
 428         serialSetBaudRate(frskyPort, FRSKY_BAUDRATE);
 429         serialSetMode(frskyPort, FRSKY_INITIAL_PORT_MODE);
 430         beginSerialPortFunction(frskyPort, FUNCTION_TELEMETRY);
 431     } else {
 432         frskyPort = openSerialPort(FUNCTION_TELEMETRY, NULL, FRSKY_BAUDRATE, FRSKY_INITIAL_PORT_MODE, telemetryConfig->telemetry_inversion);
 433
 434         // FIXME only need these values to reset the port if the port is shared
 435         previousPortMode = frskyPort->mode;
 436         previousBaudRate = frskyPort->baudRate;
 437     }
 438 }
 439
 440
 441 bool canSendFrSkyTelemetry(void)
 442 {
 443     return serialTotalBytesWaiting(frskyPort) == 0;
 444 }
 445
 446 bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
 447 {
 448     return currentMillis - lastCycleTime >= CYCLETIME;
 449 }
 450
 451 void handleFrSkyTelemetry(void)
 452 {
 453     if (!canSendFrSkyTelemetry()) {
 454         return;
 455     }
 456
 457     uint32_t now = millis();
 458
 459     if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
 460         return;
 461     }
 462
 463     lastCycleTime = now;
 464
 465     cycleNum++;
 466
 467     // Sent every 125ms
 468     sendAccel();
 469     sendVario();
 470     sendTelemetryTail();
 471
 472     if ((cycleNum % 4) == 0) {      // Sent every 500ms
 473         if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
 474             sendBaro();
 475         }
 476         sendHeading();
 477         sendTelemetryTail();
 478     }
 479
 480     if ((cycleNum % 8) == 0) {      // Sent every 1s
 481         sendTemperature1();
 482         sendThrottleOrBatterySizeAsRpm();
 483
 484         if (feature(FEATURE_VBAT)) {
 485             sendVoltage();
 486             sendVoltageAmp();
 487             sendAmperage();
 488             sendFuelLevel();
 489         }
 490
 491 #ifdef GPS
 492         if (sensors(SENSOR_GPS)) {
 493             sendSpeed();
 494             sendGpsAltitude();
 495             sendSatalliteSignalQualityAsTemperature2();
 496         }
 497 #endif
 498
 499         //  Send GPS information to display compass information
 500         if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
 501             sendGPS();
 502         }
 503         sendTelemetryTail();
 504     }
 505
 506     if (cycleNum == 40) {     //Frame 3: Sent every 5s
 507         cycleNum = 0;
 508         sendTime();
 509         sendTelemetryTail();
 510     }
 511 }
 512
 513 uint32_t getFrSkyTelemetryProviderBaudRate(void) {
 514     return FRSKY_BAUDRATE;
 515 }
 516 #endif