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 < gpsConfig()->gpsMinimumSats)
 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     ltm_alt = getEstimatedAltitudeCm();
 150     ltm_serialise_32(ltm_alt);
 151     ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
 152     ltm_finalise();
 153 #endif
 154 }
 155
 156 /*
 157  * Sensors S-frame 5Hhz at > 2400 baud
 158  * VBAT(mv)  Current(ma)   RSSI  AIRSPEED  ARM/FS/FMOD
 159  * Flight mode(0-19):
 160  *     0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon,
 161  *     4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
 162  *     8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints,
 163  *     11: Heading Hold / headFree, 12: Circle, 13: RTH, 14: FollowMe,
 164  *     15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
 165  */
 166
 167 static void ltm_sframe(void)
 168 {
 169     uint8_t lt_flightmode;
 170     uint8_t lt_statemode;
 171     if (FLIGHT_MODE(PASSTHRU_MODE))
 172         lt_flightmode = 0;
 173     else if (FLIGHT_MODE(HEADFREE_MODE))
 174         lt_flightmode = 4;
 175     else if (FLIGHT_MODE(ANGLE_MODE))
 176         lt_flightmode = 2;
 177     else if (FLIGHT_MODE(HORIZON_MODE))
 178         lt_flightmode = 3;
 179     else
 180         lt_flightmode = 1;      // Rate mode
 181
 182     lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
 183     if (failsafeIsActive())
 184         lt_statemode |= 2;
 185     ltm_initialise_packet('S');
 186     ltm_serialise_16(getBatteryVoltage() * 10); // vbat converted to mV
 187     ltm_serialise_16((uint16_t)constrain(getMAhDrawn(), 0, UINT16_MAX)); // consumption in mAh (65535 mAh max)
 188     ltm_serialise_8(constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255));        // scaled RSSI (uchar)
 189     ltm_serialise_8(0);              // no airspeed
 190     ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
 191     ltm_finalise();
 192 }
 193
 194 /*
 195  * Attitude A-frame - 10 Hz at > 2400 baud
 196  *  PITCH ROLL HEADING
 197  */
 198 static void ltm_aframe(void)
 199 {
 200     ltm_initialise_packet('A');
 201     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch));
 202     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.roll));
 203     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 204     ltm_finalise();
 205 }
 206
 207 /*
 208  * OSD additional data frame, 1 Hz rate
 209  *  This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
 210  *  home pos, home alt, direction to home
 211  */
 212 static void ltm_oframe(void)
 213 {
 214     ltm_initialise_packet('O');
 215 #if defined(USE_GPS)
 216     ltm_serialise_32(GPS_home[GPS_LATITUDE]);
 217     ltm_serialise_32(GPS_home[GPS_LONGITUDE]);
 218 #else
 219     ltm_serialise_32(0);
 220     ltm_serialise_32(0);
 221 #endif
 222     ltm_serialise_32(0);                // Don't have GPS home altitude
 223     ltm_serialise_8(1);                 // OSD always ON
 224     ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
 225     ltm_finalise();
 226 }
 227
 228 static void process_ltm(void)
 229 {
 230     static uint8_t ltm_scheduler;
 231     ltm_aframe();
 232     if (ltm_scheduler & 1)
 233         ltm_gframe();
 234     else
 235         ltm_sframe();
 236     if (ltm_scheduler == 0)
 237         ltm_oframe();
 238     ltm_scheduler++;
 239     ltm_scheduler %= 10;
 240 }
 241
 242 void handleLtmTelemetry(void)
 243 {
 244     static uint32_t ltm_lastCycleTime;
 245     uint32_t now;
 246     if (!ltmEnabled)
 247         return;
 248     if (!ltmPort)
 249         return;
 250     now = millis();
 251     if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
 252         process_ltm();
 253         ltm_lastCycleTime = now;
 254     }
 255 }
 256
 257 void freeLtmTelemetryPort(void)
 258 {
 259     closeSerialPort(ltmPort);
 260     ltmPort = NULL;
 261     ltmEnabled = false;
 262 }
 263
 264 void initLtmTelemetry(void)
 265 {
 266     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
 267     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 268 }
 269
 270 void configureLtmTelemetryPort(void)
 271 {
 272     if (!portConfig) {
 273         return;
 274     }
 275     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 276     if (baudRateIndex == BAUD_AUTO) {
 277         baudRateIndex = BAUD_19200;
 278     }
 279     ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
 280     if (!ltmPort)
 281         return;
 282     ltmEnabled = true;
 283 }
 284
 285 void checkLtmTelemetryState(void)
 286 {
 287     if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 288         if (!ltmEnabled && telemetrySharedPort != NULL) {
 289             ltmPort = telemetrySharedPort;
 290             ltmEnabled = true;
 291         }
 292     } else {
 293         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
 294         if (newTelemetryEnabledValue == ltmEnabled)
 295             return;
 296         if (newTelemetryEnabledValue)
 297             configureLtmTelemetryPort();
 298         else
 299             freeLtmTelemetryPort();
 300     }
 301 }
 302 #endif