src/main/telemetry/ltm.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 /*
  22  * LightTelemetry from KipK
  23  *
  24  * Minimal one way telemetry protocol for really bitrates (1200/2400 bauds).
  25  * Effective for ground OSD, groundstation HUD and Antenna tracker
  26  * http://www.wedontneednasa.com/2014/02/lighttelemetry-v2-en-route-to-ground-osd.html
  27  *
  28  * This implementation is for LTM v2 > 2400 baud rates
  29  *
  30  * Cleanflight implementation by Jonathan Hudson
  31  */
  32
  33 #include <stdbool.h>
  34 #include <stdint.h>
  35 #include <string.h>
  36
  37 #include "platform.h"
  38
  39 #ifdef USE_TELEMETRY_LTM
  40
  41 #include "build/build_config.h"
  42
  43 #include "common/maths.h"
  44 #include "common/axis.h"
  45 #include "common/color.h"
  46 #include "common/utils.h"
  47
  48 #include "drivers/time.h"
  49 #include "drivers/sensor.h"
  50 #include "drivers/accgyro/accgyro.h"
  51
  52 #include "config/config.h"
  53 #include "fc/rc_controls.h"
  54 #include "fc/runtime_config.h"
  55
  56 #include "sensors/sensors.h"
  57 #include "sensors/acceleration.h"
  58 #include "sensors/gyro.h"
  59 #include "sensors/barometer.h"
  60 #include "sensors/boardalignment.h"
  61 #include "sensors/battery.h"
  62
  63 #include "io/serial.h"
  64 #include "io/gimbal.h"
  65 #include "io/gps.h"
  66 #include "io/ledstrip.h"
  67 #include "io/beeper.h"
  68
  69 #include "pg/rx.h"
  70
  71 #include "rx/rx.h"
  72
  73 #include "flight/mixer.h"
  74 #include "flight/pid.h"
  75 #include "flight/imu.h"
  76 #include "flight/failsafe.h"
  77 #include "flight/position.h"
  78
  79 #include "telemetry/telemetry.h"
  80 #include "telemetry/ltm.h"
  81
  82 #define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX
  83 #define LTM_CYCLETIME   100
  84
  85 static serialPort_t *ltmPort;
  86 static const serialPortConfig_t *portConfig;
  87 static bool ltmEnabled;
  88 static portSharing_e ltmPortSharing;
  89 static uint8_t ltm_crc;
  90
  91 static void ltm_initialise_packet(uint8_t ltm_id)
  92 {
  93     ltm_crc = 0;
  94     serialWrite(ltmPort, '$');
  95     serialWrite(ltmPort, 'T');
  96     serialWrite(ltmPort, ltm_id);
  97 }
  98
  99 static void ltm_serialise_8(uint8_t v)
 100 {
 101     serialWrite(ltmPort, v);
 102     ltm_crc ^= v;
 103 }
 104
 105 static void ltm_serialise_16(uint16_t v)
 106 {
 107     ltm_serialise_8((uint8_t)v);
 108     ltm_serialise_8((v >> 8));
 109 }
 110
 111 static void ltm_serialise_32(uint32_t v)
 112 {
 113     ltm_serialise_8((uint8_t)v);
 114     ltm_serialise_8((v >> 8));
 115     ltm_serialise_8((v >> 16));
 116     ltm_serialise_8((v >> 24));
 117 }
 118
 119 static void ltm_finalise(void)
 120 {
 121     serialWrite(ltmPort, ltm_crc);
 122 }
 123
 124 /*
 125  * GPS G-frame 5Hhz at > 2400 baud
 126  * LAT LON SPD ALT SAT/FIX
 127  */
 128 static void ltm_gframe(void)
 129 {
 130 #if defined(USE_GPS)
 131     uint8_t gps_fix_type = 0;
 132     int32_t ltm_alt;
 133
 134     if (!sensors(SENSOR_GPS))
 135         return;
 136
 137     if (!STATE(GPS_FIX))
 138         gps_fix_type = 1;
 139     else if (gpsSol.numSat < GPS_MIN_SAT_COUNT)
 140         gps_fix_type = 2;
 141     else
 142         gps_fix_type = 3;
 143
 144     ltm_initialise_packet('G');
 145     ltm_serialise_32(gpsSol.llh.lat);
 146     ltm_serialise_32(gpsSol.llh.lon);
 147     ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
 148     ltm_alt = getEstimatedAltitudeCm();
 149     ltm_serialise_32(ltm_alt);
 150     ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
 151     ltm_finalise();
 152 #endif
 153 }
 154
 155 /*
 156  * Sensors S-frame 5Hhz at > 2400 baud
 157  * VBAT(mv)  Current(ma)   RSSI  AIRSPEED  ARM/FS/FMOD
 158  * Flight mode(0-19):
 159  *     0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon,
 160  *     4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
 161  *     8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints,
 162  *     11: Heading Hold / headFree, 12: Circle, 13: RTH, 14: FollowMe,
 163  *     15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
 164  */
 165
 166 static void ltm_sframe(void)
 167 {
 168     uint8_t lt_flightmode;
 169     uint8_t lt_statemode;
 170     if (FLIGHT_MODE(PASSTHRU_MODE))
 171         lt_flightmode = 0;
 172     else if (FLIGHT_MODE(HEADFREE_MODE))
 173         lt_flightmode = 4;
 174     else if (FLIGHT_MODE(ANGLE_MODE))
 175         lt_flightmode = 2;
 176     else if (FLIGHT_MODE(HORIZON_MODE))
 177         lt_flightmode = 3;
 178     else if (FLIGHT_MODE(ALT_HOLD_MODE))
 179         lt_flightmode = 8;
 180     else if (FLIGHT_MODE(POS_HOLD_MODE))
 181         lt_flightmode = 9;
 182     else if (FLIGHT_MODE(GPS_RESCUE_MODE))
 183         lt_flightmode = 13;
 184     else
 185         lt_flightmode = 1;      // Rate mode
 186
 187     lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
 188     if (failsafeIsActive())
 189         lt_statemode |= 2;
 190     ltm_initialise_packet('S');
 191     ltm_serialise_16(getBatteryVoltage() * 10); // vbat converted to mV
 192     ltm_serialise_16((uint16_t)constrain(getMAhDrawn(), 0, UINT16_MAX)); // consumption in mAh (65535 mAh max)
 193     ltm_serialise_8(constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255));        // scaled RSSI (uchar)
 194     ltm_serialise_8(0);              // no airspeed
 195     ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
 196     ltm_finalise();
 197 }
 198
 199 /*
 200  * Attitude A-frame - 10 Hz at > 2400 baud
 201  *  PITCH ROLL HEADING
 202  */
 203 static void ltm_aframe(void)
 204 {
 205     ltm_initialise_packet('A');
 206     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch));
 207     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.roll));
 208     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 209     ltm_finalise();
 210 }
 211
 212 /*
 213  * OSD additional data frame, 1 Hz rate
 214  *  This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
 215  *  home pos, home alt, direction to home
 216  */
 217 static void ltm_oframe(void)
 218 {
 219     ltm_initialise_packet('O');
 220 #if defined(USE_GPS)
 221     ltm_serialise_32(GPS_home_llh.lat);
 222     ltm_serialise_32(GPS_home_llh.lon);
 223 #else
 224     ltm_serialise_32(0);
 225     ltm_serialise_32(0);
 226 #endif
 227     ltm_serialise_32(0);                // Don't have GPS home altitude
 228     ltm_serialise_8(1);                 // OSD always ON
 229     ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
 230     ltm_finalise();
 231 }
 232
 233 static void process_ltm(void)
 234 {
 235     static uint8_t ltm_scheduler;
 236     ltm_aframe();
 237     if (ltm_scheduler & 1)
 238         ltm_gframe();
 239     else
 240         ltm_sframe();
 241     if (ltm_scheduler == 0)
 242         ltm_oframe();
 243     ltm_scheduler++;
 244     ltm_scheduler %= 10;
 245 }
 246
 247 void handleLtmTelemetry(void)
 248 {
 249     static uint32_t ltm_lastCycleTime;
 250     uint32_t now;
 251     if (!ltmEnabled)
 252         return;
 253     if (!ltmPort)
 254         return;
 255     now = millis();
 256     if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
 257         process_ltm();
 258         ltm_lastCycleTime = now;
 259     }
 260 }
 261
 262 void freeLtmTelemetryPort(void)
 263 {
 264     closeSerialPort(ltmPort);
 265     ltmPort = NULL;
 266     ltmEnabled = false;
 267 }
 268
 269 void initLtmTelemetry(void)
 270 {
 271     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
 272     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 273 }
 274
 275 void configureLtmTelemetryPort(void)
 276 {
 277     if (!portConfig) {
 278         return;
 279     }
 280     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 281     if (baudRateIndex == BAUD_AUTO) {
 282         baudRateIndex = BAUD_19200;
 283     }
 284     ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
 285     if (!ltmPort)
 286         return;
 287     ltmEnabled = true;
 288 }
 289
 290 void checkLtmTelemetryState(void)
 291 {
 292     if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 293         if (!ltmEnabled && telemetrySharedPort != NULL) {
 294             ltmPort = telemetrySharedPort;
 295             ltmEnabled = true;
 296         }
 297     } else {
 298         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
 299         if (newTelemetryEnabledValue == ltmEnabled)
 300             return;
 301         if (newTelemetryEnabledValue)
 302             configureLtmTelemetryPort();
 303         else
 304             freeLtmTelemetryPort();
 305     }
 306 }
 307 #endif