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
  83 #define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX
  84 #define LTM_CYCLETIME   100
  85
  86 static serialPort_t *ltmPort;
  87 static const serialPortConfig_t *portConfig;
  88 static bool ltmEnabled;
  89 static portSharing_e ltmPortSharing;
  90 static uint8_t ltm_crc;
  91
  92 static void ltm_initialise_packet(uint8_t ltm_id)
  93 {
  94     ltm_crc = 0;
  95     serialWrite(ltmPort, '$');
  96     serialWrite(ltmPort, 'T');
  97     serialWrite(ltmPort, ltm_id);
  98 }
  99
 100 static void ltm_serialise_8(uint8_t v)
 101 {
 102     serialWrite(ltmPort, v);
 103     ltm_crc ^= v;
 104 }
 105
 106 static void ltm_serialise_16(uint16_t v)
 107 {
 108     ltm_serialise_8((uint8_t)v);
 109     ltm_serialise_8((v >> 8));
 110 }
 111
 112 static void ltm_serialise_32(uint32_t v)
 113 {
 114     ltm_serialise_8((uint8_t)v);
 115     ltm_serialise_8((v >> 8));
 116     ltm_serialise_8((v >> 16));
 117     ltm_serialise_8((v >> 24));
 118 }
 119
 120 static void ltm_finalise(void)
 121 {
 122     serialWrite(ltmPort, ltm_crc);
 123 }
 124
 125 /*
 126  * GPS G-frame 5Hhz at > 2400 baud
 127  * LAT LON SPD ALT SAT/FIX
 128  */
 129 static void ltm_gframe(void)
 130 {
 131 #if defined(USE_GPS)
 132     uint8_t gps_fix_type = 0;
 133     int32_t ltm_alt;
 134
 135     if (!sensors(SENSOR_GPS))
 136         return;
 137
 138     if (!STATE(GPS_FIX))
 139         gps_fix_type = 1;
 140     else if (gpsSol.numSat < 5)
 141         gps_fix_type = 2;
 142     else
 143         gps_fix_type = 3;
 144
 145     ltm_initialise_packet('G');
 146     ltm_serialise_32(gpsSol.llh.lat);
 147     ltm_serialise_32(gpsSol.llh.lon);
 148     ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
 149
 150 #if defined(USE_BARO) || defined(USE_RANGEFINDER)
 151     ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitudeCm() : gpsSol.llh.altCm;
 152 #else
 153     ltm_alt = gpsSol.llh.altCm;
 154 #endif
 155     ltm_serialise_32(ltm_alt);
 156     ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
 157     ltm_finalise();
 158 #endif
 159 }
 160
 161 /*
 162  * Sensors S-frame 5Hhz at > 2400 baud
 163  * VBAT(mv)  Current(ma)   RSSI  AIRSPEED  ARM/FS/FMOD
 164  * Flight mode(0-19):
 165  *     0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon,
 166  *     4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
 167  *     8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints,
 168  *     11: Heading Hold / headFree, 12: Circle, 13: RTH, 14: FollowMe,
 169  *     15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
 170  */
 171
 172 static void ltm_sframe(void)
 173 {
 174     uint8_t lt_flightmode;
 175     uint8_t lt_statemode;
 176     if (FLIGHT_MODE(PASSTHRU_MODE))
 177         lt_flightmode = 0;
 178     else if (FLIGHT_MODE(HEADFREE_MODE))
 179         lt_flightmode = 4;
 180     else if (FLIGHT_MODE(ANGLE_MODE))
 181         lt_flightmode = 2;
 182     else if (FLIGHT_MODE(HORIZON_MODE))
 183         lt_flightmode = 3;
 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(0);             //  current, not implemented
 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[LAT]);
 222     ltm_serialise_32(GPS_home[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