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 = 5;
 180     else
 181         lt_flightmode = 1;      // Rate mode
 182
 183     lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
 184     if (failsafeIsActive())
 185         lt_statemode |= 2;
 186     ltm_initialise_packet('S');
 187     ltm_serialise_16(getBatteryVoltage() * 10); // vbat converted to mV
 188     ltm_serialise_16((uint16_t)constrain(getMAhDrawn(), 0, UINT16_MAX)); // consumption in mAh (65535 mAh max)
 189     ltm_serialise_8(constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255));        // scaled RSSI (uchar)
 190     ltm_serialise_8(0);              // no airspeed
 191     ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
 192     ltm_finalise();
 193 }
 194
 195 /*
 196  * Attitude A-frame - 10 Hz at > 2400 baud
 197  *  PITCH ROLL HEADING
 198  */
 199 static void ltm_aframe(void)
 200 {
 201     ltm_initialise_packet('A');
 202     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch));
 203     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.roll));
 204     ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
 205     ltm_finalise();
 206 }
 207
 208 /*
 209  * OSD additional data frame, 1 Hz rate
 210  *  This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
 211  *  home pos, home alt, direction to home
 212  */
 213 static void ltm_oframe(void)
 214 {
 215     ltm_initialise_packet('O');
 216 #if defined(USE_GPS)
 217     ltm_serialise_32(GPS_home_llh.lat);
 218     ltm_serialise_32(GPS_home_llh.lon);
 219 #else
 220     ltm_serialise_32(0);
 221     ltm_serialise_32(0);
 222 #endif
 223     ltm_serialise_32(0);                // Don't have GPS home altitude
 224     ltm_serialise_8(1);                 // OSD always ON
 225     ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
 226     ltm_finalise();
 227 }
 228
 229 static void process_ltm(void)
 230 {
 231     static uint8_t ltm_scheduler;
 232     ltm_aframe();
 233     if (ltm_scheduler & 1)
 234         ltm_gframe();
 235     else
 236         ltm_sframe();
 237     if (ltm_scheduler == 0)
 238         ltm_oframe();
 239     ltm_scheduler++;
 240     ltm_scheduler %= 10;
 241 }
 242
 243 void handleLtmTelemetry(void)
 244 {
 245     static uint32_t ltm_lastCycleTime;
 246     uint32_t now;
 247     if (!ltmEnabled)
 248         return;
 249     if (!ltmPort)
 250         return;
 251     now = millis();
 252     if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
 253         process_ltm();
 254         ltm_lastCycleTime = now;
 255     }
 256 }
 257
 258 void freeLtmTelemetryPort(void)
 259 {
 260     closeSerialPort(ltmPort);
 261     ltmPort = NULL;
 262     ltmEnabled = false;
 263 }
 264
 265 void initLtmTelemetry(void)
 266 {
 267     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
 268     ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
 269 }
 270
 271 void configureLtmTelemetryPort(void)
 272 {
 273     if (!portConfig) {
 274         return;
 275     }
 276     baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
 277     if (baudRateIndex == BAUD_AUTO) {
 278         baudRateIndex = BAUD_19200;
 279     }
 280     ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, telemetryConfig()->telemetry_inverted ? SERIAL_INVERTED : SERIAL_NOT_INVERTED);
 281     if (!ltmPort)
 282         return;
 283     ltmEnabled = true;
 284 }
 285
 286 void checkLtmTelemetryState(void)
 287 {
 288     if (portConfig && telemetryCheckRxPortShared(portConfig, rxRuntimeState.serialrxProvider)) {
 289         if (!ltmEnabled && telemetrySharedPort != NULL) {
 290             ltmPort = telemetrySharedPort;
 291             ltmEnabled = true;
 292         }
 293     } else {
 294         bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
 295         if (newTelemetryEnabledValue == ltmEnabled)
 296             return;
 297         if (newTelemetryEnabledValue)
 298             configureLtmTelemetryPort();
 299         else
 300             freeLtmTelemetryPort();
 301     }
 302 }
 303 #endif