src/main/telemetry/ltm.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  * LightTelemetry from KipK
  20  *
  21  * Minimal one way telemetry protocol for really low bitrates (1200/2400 bauds).
  22  * Effective for ground OSD, groundstation HUD and Antenna tracker
  23  * http://www.wedontneednasa.com/2014/02/lighttelemetry-v2-en-route-to-ground-osd.html
  24  *
  25  * This implementation is for LTM v2 > 2400 baud rates
  26  *
  27  * Cleanflight implementation by Jonathan Hudson
  28  */
  29
  30 #include <stdbool.h>
  31 #include <stdint.h>
  32
  33 #include "platform.h"
  34
  35
  36 #if defined(USE_TELEMETRY_LTM)
  37
  38 #include "build/build_config.h"
  39
  40 #include "common/axis.h"
  41 #include "common/color.h"
  42 #include "common/streambuf.h"
  43 #include "common/utils.h"
  44
  45 #include "drivers/serial.h"
  46 #include "drivers/time.h"
  47
  48 #include "fc/config.h"
  49 #include "fc/fc_core.h"
  50 #include "fc/rc_controls.h"
  51 #include "fc/runtime_config.h"
  52
  53 #include "flight/imu.h"
  54 #include "flight/failsafe.h"
  55 #include "flight/mixer.h"
  56 #include "flight/pid.h"
  57
  58 #include "io/gps.h"
  59 #include "io/ledstrip.h"
  60 #include "io/serial.h"
  61
  62 #include "navigation/navigation.h"
  63
  64 #include "rx/rx.h"
  65
  66 #include "sensors/acceleration.h"
  67 #include "sensors/barometer.h"
  68 #include "sensors/battery.h"
  69 #include "sensors/boardalignment.h"
  70 #include "sensors/diagnostics.h"
  71 #include "sensors/gyro.h"
  72 #include "sensors/sensors.h"
  73 #include "sensors/pitotmeter.h"
  74 #include "sensors/diagnostics.h"
  75
  76 #include "telemetry/ltm.h"
  77 #include "telemetry/telemetry.h"
  78
  79
  80 #define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX
  81 #define LTM_CYCLETIME   100
  82 #define LTM_SCHEDULE_SIZE (1000/LTM_CYCLETIME)
  83
  84 static serialPort_t *ltmPort;
  85 static serialPortConfig_t *portConfig;
  86 static bool ltmEnabled;
  87 static portSharing_e ltmPortSharing;
  88 static uint8_t ltmFrame[LTM_MAX_MESSAGE_SIZE];
  89 static uint8_t ltm_x_counter;
  90
  91 static void ltm_initialise_packet(sbuf_t *dst)
  92 {
  93     dst->ptr = ltmFrame;
  94     dst->end = ARRAYEND(ltmFrame);
  95
  96     sbufWriteU8(dst, '$');
  97     sbufWriteU8(dst, 'T');
  98 }
  99
 100 static void ltm_finalise(sbuf_t *dst)
 101 {
 102     uint8_t crc = 0;
 103     for (const uint8_t *ptr = &ltmFrame[3]; ptr < dst->ptr; ++ptr) {
 104         crc ^= *ptr;
 105     }
 106     sbufWriteU8(dst, crc);
 107     sbufSwitchToReader(dst, ltmFrame);
 108     serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst));
 109 }
 110
 111 #if defined(USE_GPS)
 112 /*
 113  * GPS G-frame 5Hhz at > 2400 baud
 114  * LAT LON SPD ALT SAT/FIX
 115  */
 116 void ltm_gframe(sbuf_t *dst)
 117 {
 118     uint8_t gps_fix_type = 0;
 119     int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
 120
 121     if (sensors(SENSOR_GPS)
 122 #ifdef USE_GPS_FIX_ESTIMATION
 123             || STATE(GPS_ESTIMATED_FIX)
 124 #endif
 125         ) {
 126         if (gpsSol.fixType == GPS_NO_FIX)
 127             gps_fix_type = 1;
 128         else if (gpsSol.fixType == GPS_FIX_2D)
 129             gps_fix_type = 2;
 130         else if (gpsSol.fixType == GPS_FIX_3D)
 131             gps_fix_type = 3;
 132
 133         ltm_lat = gpsSol.llh.lat;
 134         ltm_lon = gpsSol.llh.lon;
 135         ltm_gs = gpsSol.groundSpeed / 100;
 136     }
 137
 138     ltm_alt = getEstimatedActualPosition(Z); // cm
 139
 140     sbufWriteU8(dst, 'G');
 141     sbufWriteU32(dst, ltm_lat);
 142     sbufWriteU32(dst, ltm_lon);
 143     sbufWriteU8(dst, (uint8_t)ltm_gs);
 144     sbufWriteU32(dst, ltm_alt);
 145     sbufWriteU8(dst, (gpsSol.numSat << 2) | gps_fix_type);
 146 }
 147 #endif
 148
 149 /*
 150  * Sensors S-frame 5Hhz at > 2400 baud
 151  * VBAT(mv)  Current(ma)   RSSI  AIRSPEED  ARM/FS/FMOD
 152  * Flight mode(0-19):
 153  *     0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon,
 154  *     4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
 155  *     8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints,
 156  *     11: Heading Hold / headFree, 12: Circle, 13: RTH, 14: FollowMe,
 157  *     15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
 158  */
 159
 160 void ltm_sframe(sbuf_t *dst)
 161 {
 162     ltm_modes_e lt_flightmode;
 163
 164     if (FLIGHT_MODE(MANUAL_MODE))
 165         lt_flightmode = LTM_MODE_MANUAL;
 166     else if (FLIGHT_MODE(NAV_WP_MODE))
 167         lt_flightmode = LTM_MODE_WAYPOINTS;
 168     else if (FLIGHT_MODE(NAV_RTH_MODE))
 169         lt_flightmode = LTM_MODE_RTH;
 170     else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
 171         lt_flightmode = LTM_MODE_GPSHOLD;
 172     else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
 173         lt_flightmode = LTM_MODE_CRUISE;
 174     else if (FLIGHT_MODE(NAV_LAUNCH_MODE))
 175         lt_flightmode = LTM_MODE_LAUNCH;
 176     else if (FLIGHT_MODE(AUTO_TUNE))
 177         lt_flightmode = LTM_MODE_AUTOTUNE;
 178     else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
 179         lt_flightmode = LTM_MODE_ALTHOLD;
 180     else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE))
 181         lt_flightmode = LTM_MODE_HEADHOLD;
 182     else if (FLIGHT_MODE(ANGLE_MODE))
 183         lt_flightmode = LTM_MODE_ANGLE;
 184     else if (FLIGHT_MODE(HORIZON_MODE))
 185         lt_flightmode = LTM_MODE_HORIZON;
 186 #ifdef USE_FW_AUTOLAND
 187     else if (FLIGHT_MODE(NAV_FW_AUTOLAND))
 188         lt_flightmode = LTM_MODE_LAND;
 189 #endif
 190     else
 191         lt_flightmode = LTM_MODE_RATE;      // Rate mode
 192
 193     uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
 194     if (failsafeIsActive())
 195         lt_statemode |= 2;
 196     sbufWriteU8(dst, 'S');
 197     sbufWriteU16(dst, getBatteryVoltage() * 10);    //vbat converted to mv
 198     sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF));    // current mAh (65535 mAh max)
 199     sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023));        // scaled RSSI (uchar)
 200 #if defined(USE_PITOT)
 201     sbufWriteU8(dst, (sensors(SENSOR_PITOT) && pitotIsHealthy())? getAirspeedEstimate() / 100.0f : 0);  // in m/s
 202 #else
 203     sbufWriteU8(dst, 0);
 204 #endif
 205     sbufWriteU8(dst, (lt_flightmode << 2) | lt_statemode);
 206 }
 207
 208 /*
 209  * Attitude A-frame - 10 Hz at > 2400 baud
 210  *  PITCH ROLL HEADING
 211  */
 212 void ltm_aframe(sbuf_t *dst)
 213 {
 214     sbufWriteU8(dst, 'A');
 215     sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.pitch));
 216     sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.roll));
 217     sbufWriteU16(dst, DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 218 }
 219
 220 #if defined(USE_GPS)
 221 /*
 222  * OSD additional data frame, 1 Hz rate
 223  *  This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
 224  *  home pos, home alt, direction to home
 225  */
 226 void ltm_oframe(sbuf_t *dst)
 227 {
 228     sbufWriteU8(dst, 'O');
 229     sbufWriteU32(dst, GPS_home.lat);
 230     sbufWriteU32(dst, GPS_home.lon);
 231     sbufWriteU32(dst, GPS_home.alt);
 232     sbufWriteU8(dst, 1);                 // OSD always ON
 233     sbufWriteU8(dst, STATE(GPS_FIX_HOME) ? 1 : 0);
 234 }
 235 #endif
 236
 237 /*
 238  * Extended information data frame, 1 Hz rate
 239  *  This frame is intended to report extended GPS and NAV data, however at the moment it contains only HDOP value
 240  */
 241 void ltm_xframe(sbuf_t *dst)
 242 {
 243     uint8_t sensorStatus =
 244         (isHardwareHealthy() ? 0 : 1) << 0;     // bit 0 - hardware failure indication (1 - something is wrong with the hardware sensors)
 245
 246     sbufWriteU8(dst, 'X');
 247 #if defined(USE_GPS)
 248     sbufWriteU16(dst, gpsSol.hdop);
 249 #else
 250     sbufWriteU16(dst, 9999);
 251 #endif
 252     sbufWriteU8(dst, sensorStatus);
 253     sbufWriteU8(dst, ltm_x_counter);
 254     sbufWriteU8(dst, getDisarmReason());
 255     sbufWriteU8(dst, 0);
 256     ltm_x_counter++; // overflow is OK
 257 }
 258
 259 /** OSD additional data frame, ~4 Hz rate, navigation system status
 260  */
 261 void ltm_nframe(sbuf_t *dst)
 262 {
 263     sbufWriteU8(dst, 'N');
 264     sbufWriteU8(dst, NAV_Status.mode);
 265     sbufWriteU8(dst, NAV_Status.state);
 266     sbufWriteU8(dst, NAV_Status.activeWpAction);
 267     sbufWriteU8(dst, NAV_Status.activeWpNumber);
 268     sbufWriteU8(dst, NAV_Status.error);
 269     sbufWriteU8(dst, NAV_Status.flags);
 270 }
 271
 272 #define LTM_BIT_AFRAME  (1 << 0)
 273 #define LTM_BIT_GFRAME  (1 << 1)
 274 #define LTM_BIT_SFRAME  (1 << 2)
 275 #define LTM_BIT_OFRAME  (1 << 3)
 276 #define LTM_BIT_NFRAME  (1 << 4)
 277 #define LTM_BIT_XFRAME  (1 << 5)
 278
 279 /*
 280  * This is the normal (default) scheduler, needs c. 4800 baud or faster
 281  * Equates to c. 303 bytes / second
 282  */
 283 static uint8_t ltm_normal_schedule[LTM_SCHEDULE_SIZE] = {
 284     LTM_BIT_AFRAME | LTM_BIT_GFRAME,
 285     LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_OFRAME,
 286     LTM_BIT_AFRAME | LTM_BIT_GFRAME,
 287     LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME,
 288     LTM_BIT_AFRAME | LTM_BIT_GFRAME,
 289     LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_XFRAME,
 290     LTM_BIT_AFRAME | LTM_BIT_GFRAME,
 291     LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME,
 292     LTM_BIT_AFRAME | LTM_BIT_GFRAME,
 293     LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME
 294 };
 295
 296 /*
 297  * This is the medium scheduler, needs c. 2400 baud or faster
 298  * Equates to c. 164 bytes / second
 299  */
 300 static uint8_t ltm_medium_schedule[LTM_SCHEDULE_SIZE] = {
 301     LTM_BIT_AFRAME,
 302     LTM_BIT_GFRAME,
 303     LTM_BIT_AFRAME | LTM_BIT_SFRAME,
 304     LTM_BIT_OFRAME,
 305     LTM_BIT_AFRAME | LTM_BIT_XFRAME,
 306     LTM_BIT_OFRAME,
 307     LTM_BIT_AFRAME | LTM_BIT_SFRAME,
 308     LTM_BIT_GFRAME,
 309     LTM_BIT_AFRAME,
 310     LTM_BIT_NFRAME
 311 };
 312
 313 /*
 314  * This is the slow scheduler, needs c. 1200 baud or faster
 315  * Equates to c. 105 bytes / second (91 b/s if the second GFRAME is zeroed)
 316  */
 317 static uint8_t ltm_slow_schedule[LTM_SCHEDULE_SIZE] = {
 318     LTM_BIT_GFRAME,
 319     LTM_BIT_SFRAME,
 320     LTM_BIT_AFRAME,
 321     0,
 322     LTM_BIT_OFRAME,
 323     LTM_BIT_XFRAME,
 324     LTM_BIT_GFRAME, // consider zeroing this for even lower bytes/sec
 325     0,
 326     LTM_BIT_AFRAME,
 327     LTM_BIT_NFRAME,
 328 };
 329
 330 /* Set by initialisation */
 331 static uint8_t *ltm_schedule;
 332
 333 static void process_ltm(void)
 334 {
 335     static uint8_t ltm_scheduler = 0;
 336     uint8_t current_schedule = ltm_schedule[ltm_scheduler];
 337
 338     sbuf_t ltmFrameBuf;
 339     sbuf_t *dst = &ltmFrameBuf;
 340
 341     if (current_schedule & LTM_BIT_AFRAME) {
 342         ltm_initialise_packet(dst);
 343         ltm_aframe(dst);
 344         ltm_finalise(dst);
 345     }
 346
 347 #if defined(USE_GPS)
 348     if (current_schedule & LTM_BIT_GFRAME) {
 349         ltm_initialise_packet(dst);
 350         ltm_gframe(dst);
 351         ltm_finalise(dst);
 352     }
 353
 354     if (current_schedule & LTM_BIT_OFRAME) {
 355         ltm_initialise_packet(dst);
 356         ltm_oframe(dst);
 357         ltm_finalise(dst);
 358     }
 359
 360     if (current_schedule & LTM_BIT_XFRAME) {
 361         ltm_initialise_packet(dst);
 362         ltm_xframe(dst);
 363         ltm_finalise(dst);
 364     }
 365 #endif
 366
 367     if (current_schedule & LTM_BIT_SFRAME) {
 368         ltm_initialise_packet(dst);
 369         ltm_sframe(dst);
 370         ltm_finalise(dst);
 371     }
 372
 373     if (current_schedule & LTM_BIT_NFRAME) {
 374         ltm_initialise_packet(dst);
 375         ltm_nframe(dst);
 376         ltm_finalise(dst);
 377     }
 378
 379     ltm_scheduler = (ltm_scheduler + 1) % 10;
 380 }
 381
 382 void handleLtmTelemetry(void)
 383 {
 384     static uint32_t ltm_lastCycleTime;
 385     if (!ltmEnabled)
 386         return;
 387     if (!ltmPort)
 388         return;
 389     const uint32_t now = millis();
 390     if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
 391         process_ltm();
 392         ltm_lastCycleTime = now;
 393     }
 394 }
 395
 396 void freeLtmTelemetryPort(void)
 397 {
 398     closeSerialPort(ltmPort);
 399     ltmPort = NULL;
 400     ltmEnabled = false;
 401 }
 402
 403 void initLtmTelemetry(void)
 404 {
 405     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
 406     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 407 }
 408
 409
 410
 411 static void configureLtmScheduler(void)
 412 {
 413
 414     /* setup scheduler, default to 'normal' */
 415     if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
 416         ltm_schedule = ltm_medium_schedule;
 417     else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW)
 418         ltm_schedule = ltm_slow_schedule;
 419     else
 420         ltm_schedule = ltm_normal_schedule;
 421
 422 }
 423
 424 void configureLtmTelemetryPort(void)
 425 {
 426     if (!portConfig) {
 427         return;
 428     }
 429     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 430     if (baudRateIndex == BAUD_AUTO) {
 431         baudRateIndex = BAUD_19200;
 432     }
 433
 434     /* Sanity check that we can support the scheduler */
 435     if (baudRateIndex == BAUD_2400 && telemetryConfig()->ltmUpdateRate == LTM_RATE_NORMAL)
 436          ltm_schedule = ltm_medium_schedule;
 437     if (baudRateIndex == BAUD_1200)
 438          ltm_schedule = ltm_slow_schedule;
 439
 440     ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
 441     if (!ltmPort)
 442         return;
 443     ltm_x_counter = 0;
 444     ltmEnabled = true;
 445 }
 446
 447 void checkLtmTelemetryState(void)
 448 {
 449     if (portConfig && telemetryCheckRxPortShared(portConfig)) {
 450         if (!ltmEnabled && telemetrySharedPort != NULL) {
 451             ltmPort = telemetrySharedPort;
 452             configureLtmScheduler();
 453             ltmEnabled = true;
 454         }
 455     } else {
 456         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
 457         if (newTelemetryEnabledValue == ltmEnabled)
 458             return;
 459         if (newTelemetryEnabledValue){
 460             configureLtmScheduler();
 461             configureLtmTelemetryPort();
 462
 463     }
 464         else
 465             freeLtmTelemetryPort();
 466     }
 467 }
 468
 469 int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType)
 470 {
 471     static uint8_t ltmFrame[LTM_MAX_MESSAGE_SIZE];
 472
 473     sbuf_t ltmFrameBuf = { .ptr = ltmFrame, .end =ARRAYEND(ltmFrame) };
 474     sbuf_t * const sbuf = &ltmFrameBuf;
 475
 476     switch (ltmFrameType) {
 477     default:
 478     case LTM_AFRAME:
 479         ltm_aframe(sbuf);
 480         break;
 481     case LTM_SFRAME:
 482         ltm_sframe(sbuf);
 483         break;
 484 #if defined(USE_GPS)
 485     case LTM_GFRAME:
 486         ltm_gframe(sbuf);
 487         break;
 488     case LTM_OFRAME:
 489         ltm_oframe(sbuf);
 490         break;
 491 #endif
 492     case LTM_XFRAME:
 493         ltm_xframe(sbuf);
 494         break;
 495     case LTM_NFRAME:
 496         ltm_nframe(sbuf);
 497         break;
 498     }
 499     sbufSwitchToReader(sbuf, ltmFrame);
 500     const int frameSize = sbufBytesRemaining(sbuf);
 501     for (int ii = 0; sbufBytesRemaining(sbuf); ++ii) {
 502         frame[ii] = sbufReadU8(sbuf);
 503     }
 504     return frameSize;
 505 }
 506 #endif