diff --git a/Makefile b/Makefile
index df040d46e9..47472614c8 100644
--- a/Makefile
+++ b/Makefile
@@ -335,8 +335,8 @@ HIGHEND_SRC = \
telemetry/telemetry.c \
telemetry/frsky.c \
telemetry/hott.c \
- telemetry/msp.c \
telemetry/smartport.c \
+ telemetry/ltm.c \
sensors/sonar.c \
sensors/barometer.c \
blackbox/blackbox.c \
diff --git a/src/main/io/serial.c b/src/main/io/serial.c
index d6cb9711fd..901f14d2d2 100644
--- a/src/main/io/serial.c
+++ b/src/main/io/serial.c
@@ -182,7 +182,7 @@ serialPort_t *findNextSharedSerialPort(uint16_t functionMask, serialPortFunction
return NULL;
}
-#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_MSP | FUNCTION_TELEMETRY_SMARTPORT)
+#define ALL_TELEMETRY_FUNCTIONS_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM)
#define ALL_FUNCTIONS_SHARABLE_WITH_MSP (FUNCTION_BLACKBOX | ALL_TELEMETRY_FUNCTIONS_MASK)
bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 2af50b7c89..b5dd13f010 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -29,10 +29,10 @@ typedef enum {
FUNCTION_GPS = (1 << 1), // 2
FUNCTION_TELEMETRY_FRSKY = (1 << 2), // 4
FUNCTION_TELEMETRY_HOTT = (1 << 3), // 8
- FUNCTION_TELEMETRY_MSP = (1 << 4), // 16
+ FUNCTION_TELEMETRY_LTM = (1 << 4), // 16
FUNCTION_TELEMETRY_SMARTPORT = (1 << 5), // 32
FUNCTION_RX_SERIAL = (1 << 6), // 64
- FUNCTION_BLACKBOX = (1 << 7) // 128
+ FUNCTION_BLACKBOX = (1 << 7), // 128
} serialPortFunction_e;
typedef enum {
diff --git a/src/main/mw.c b/src/main/mw.c
index 1778eb8d53..f3a30c6571 100644
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -345,7 +345,7 @@ void mwDisarm(void)
}
}
-#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_MSP | FUNCTION_TELEMETRY_SMARTPORT)
+#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)
void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
new file mode 100644
index 0000000000..0f2f1d12f8
--- /dev/null
+++ b/src/main/telemetry/ltm.c
@@ -0,0 +1,309 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+/*
+ * LightTelemetry from KipK
+ *
+ * Minimal one way telemetry protocol for really bitrates (1200/2400 bauds).
+ * Effective for ground OSD, groundstation HUD and Antenna tracker
+ * http://www.wedontneednasa.com/2014/02/lighttelemetry-v2-en-route-to-ground-osd.html
+ *
+ * This implementation is for LTM v2 > 2400 baud rates
+ *
+ * Cleanflight implementation by Jonathan Hudson
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#include "build_config.h"
+
+#ifdef TELEMETRY
+
+#include "common/maths.h"
+#include "common/axis.h"
+#include "common/color.h"
+
+#include "drivers/system.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro.h"
+#include "drivers/gpio.h"
+#include "drivers/timer.h"
+#include "drivers/serial.h"
+#include "drivers/pwm_rx.h"
+
+#include "sensors/sensors.h"
+#include "sensors/acceleration.h"
+#include "sensors/gyro.h"
+#include "sensors/barometer.h"
+#include "sensors/boardalignment.h"
+#include "sensors/battery.h"
+
+#include "io/serial.h"
+#include "io/rc_controls.h"
+#include "io/gimbal.h"
+#include "io/gps.h"
+#include "io/ledstrip.h"
+#include "io/beeper.h"
+
+#include "rx/rx.h"
+
+#include "flight/mixer.h"
+#include "flight/pid.h"
+#include "flight/imu.h"
+#include "flight/failsafe.h"
+#include "flight/altitudehold.h"
+#include "flight/navigation.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/ltm.h"
+
+#include "config/config.h"
+#include "config/runtime_config.h"
+#include "config/config_profile.h"
+#include "config/config_master.h"
+
+#define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX
+#define LTM_CYCLETIME 100
+
+extern uint16_t rssi; // FIXME dependency on mw.c
+static serialPort_t *ltmPort;
+static serialPortConfig_t *portConfig;
+static telemetryConfig_t *telemetryConfig;
+static bool ltmEnabled;
+static portSharing_e ltmPortSharing;
+static uint8_t ltm_crc;
+
+static void ltm_initialise_packet(uint8_t ltm_id)
+{
+ ltm_crc = 0;
+ serialWrite(ltmPort, '$');
+ serialWrite(ltmPort, 'T');
+ serialWrite(ltmPort, ltm_id);
+}
+
+static void ltm_serialise_8(uint8_t v)
+{
+ serialWrite(ltmPort, v);
+ ltm_crc ^= v;
+}
+
+static void ltm_serialise_16(uint16_t v)
+{
+ ltm_serialise_8((uint8_t)v);
+ ltm_serialise_8((v >> 8));
+}
+
+static void ltm_serialise_32(uint32_t v)
+{
+ ltm_serialise_8((uint8_t)v);
+ ltm_serialise_8((v >> 8));
+ ltm_serialise_8((v >> 16));
+ ltm_serialise_8((v >> 24));
+}
+
+static void ltm_finalise(void)
+{
+ serialWrite(ltmPort, ltm_crc);
+}
+
+/*
+ * GPS G-frame 5Hhz at > 2400 baud
+ * LAT LON SPD ALT SAT/FIX
+ */
+static void ltm_gframe(void)
+{
+#if defined(GPS)
+ uint8_t gps_fix_type = 0;
+ int32_t ltm_alt;
+
+ if (!sensors(SENSOR_GPS))
+ return;
+
+ if (!STATE(GPS_FIX))
+ gps_fix_type = 1;
+ else if (GPS_numSat < 5)
+ gps_fix_type = 2;
+ else
+ gps_fix_type = 3;
+
+ ltm_initialise_packet('G');
+ ltm_serialise_32(GPS_coord[LAT]);
+ ltm_serialise_32(GPS_coord[LON]);
+ ltm_serialise_8((uint8_t)(GPS_speed / 100));
+
+#if defined(BARO) || defined(SONAR)
+ ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() : GPS_altitude * 100;
+#else
+ ltm_alt = GPS_altitude * 100;
+#endif
+ ltm_serialise_32(ltm_alt);
+ ltm_serialise_8((GPS_numSat << 2) | gps_fix_type);
+ ltm_finalise();
+#endif
+}
+
+/*
+ * Sensors S-frame 5Hhz at > 2400 baud
+ * VBAT(mv) Current(ma) RSSI AIRSPEED ARM/FS/FMOD
+ * Flight mode(0-19):
+ * 0: Manual, 1: Rate, 2: Attitude/Angle, 3: Horizon,
+ * 4: Acro, 5: Stabilized1, 6: Stabilized2, 7: Stabilized3,
+ * 8: Altitude Hold, 9: Loiter/GPS Hold, 10: Auto/Waypoints,
+ * 11: Heading Hold / headFree, 12: Circle, 13: RTH, 14: FollowMe,
+ * 15: LAND, 16:FlybyWireA, 17: FlybywireB, 18: Cruise, 19: Unknown
+ */
+
+static void ltm_sframe(void)
+{
+ uint8_t lt_flightmode;
+ uint8_t lt_statemode;
+ if (FLIGHT_MODE(PASSTHRU_MODE))
+ lt_flightmode = 0;
+ else if (FLIGHT_MODE(GPS_HOME_MODE))
+ lt_flightmode = 13;
+ else if (FLIGHT_MODE(GPS_HOLD_MODE))
+ lt_flightmode = 9;
+ else if (FLIGHT_MODE(HEADFREE_MODE))
+ lt_flightmode = 4;
+ else if (FLIGHT_MODE(BARO_MODE))
+ lt_flightmode = 8;
+ else if (FLIGHT_MODE(ANGLE_MODE))
+ lt_flightmode = 2;
+ else if (FLIGHT_MODE(HORIZON_MODE))
+ lt_flightmode = 3;
+ else
+ lt_flightmode = 1; // Rate mode
+
+ lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
+ if (failsafeIsActive())
+ lt_statemode |= 2;
+ ltm_initialise_packet('S');
+ ltm_serialise_16(vbat * 100); //vbat converted to mv
+ ltm_serialise_16(0); // current, not implemented
+ ltm_serialise_8((uint8_t)((rssi * 254) / 1023)); // scaled RSSI (uchar)
+ ltm_serialise_8(0); // no airspeed
+ ltm_serialise_8((lt_flightmode << 2) | lt_statemode);
+ ltm_finalise();
+}
+
+/*
+ * Attitude A-frame - 10 Hz at > 2400 baud
+ * PITCH ROLL HEADING
+ */
+static void ltm_aframe()
+{
+ ltm_initialise_packet('A');
+ ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.pitch));
+ ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.roll));
+ ltm_serialise_16(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
+ ltm_finalise();
+}
+
+/*
+ * OSD additional data frame, 1 Hz rate
+ * This frame will be ignored by Ghettostation, but processed by GhettOSD if it is used as standalone onboard OSD
+ * home pos, home alt, direction to home
+ */
+static void ltm_oframe()
+{
+ ltm_initialise_packet('O');
+#if defined(GPS)
+ ltm_serialise_32(GPS_home[LAT]);
+ ltm_serialise_32(GPS_home[LON]);
+#else
+ ltm_serialise_32(0);
+ ltm_serialise_32(0);
+#endif
+ ltm_serialise_32(0); // Don't have GPS home altitude
+ ltm_serialise_8(1); // OSD always ON
+ ltm_serialise_8(STATE(GPS_FIX_HOME) ? 1 : 0);
+ ltm_finalise();
+}
+
+static void process_ltm(void)
+{
+ static uint8_t ltm_scheduler;
+ ltm_aframe();
+ if (ltm_scheduler & 1)
+ ltm_gframe();
+ else
+ ltm_sframe();
+ if (ltm_scheduler == 0)
+ ltm_oframe();
+ ltm_scheduler++;
+ ltm_scheduler %= 10;
+}
+
+void handleLtmTelemetry(void)
+{
+ static uint32_t ltm_lastCycleTime;
+ uint32_t now;
+ if (!ltmEnabled)
+ return;
+ if (!ltmPort)
+ return;
+ now = millis();
+ if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
+ process_ltm();
+ ltm_lastCycleTime = now;
+ }
+}
+
+void freeLtmTelemetryPort(void)
+{
+ closeSerialPort(ltmPort);
+ ltmPort = NULL;
+ ltmEnabled = false;
+}
+
+void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig)
+{
+ telemetryConfig = initialTelemetryConfig;
+ portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
+ ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
+}
+
+void configureLtmTelemetryPort(void)
+{
+ if (!portConfig) {
+ return;
+ }
+ baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
+ if (baudRateIndex == BAUD_AUTO) {
+ baudRateIndex = BAUD_19200;
+ }
+ ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
+ if (!ltmPort)
+ return;
+ ltmEnabled = true;
+}
+
+void checkLtmTelemetryState(void)
+{
+ bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
+ if (newTelemetryEnabledValue == ltmEnabled)
+ return;
+ if (newTelemetryEnabledValue)
+ configureLtmTelemetryPort();
+ else
+ freeLtmTelemetryPort();
+}
+#endif
diff --git a/src/main/telemetry/msp.h b/src/main/telemetry/ltm.h
similarity index 65%
rename from src/main/telemetry/msp.h
rename to src/main/telemetry/ltm.h
index eb06ae54e5..c78dfc22e4 100644
--- a/src/main/telemetry/msp.h
+++ b/src/main/telemetry/ltm.h
@@ -1,4 +1,6 @@
/*
+ * ltm.h
+ *
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
@@ -15,21 +17,14 @@
* along with Cleanflight. If not, see .
*/
-/*
- * telemetry_MSP.h
- *
- * Created on: 22 Apr 2014
- * Author: trey marc
- */
+#ifndef TELEMETRY_LTM_H_
+#define TELEMETRY_LTM_H_
-#ifndef TELEMETRY_MSP_H_
-#define TELEMETRY_MSP_H_
+void initLtmTelemetry(telemetryConfig_t *initialTelemetryConfig);
+void handleLtmTelemetry(void);
+void checkLtmTelemetryState(void);
-void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig);
-void handleMSPTelemetry(void);
-void checkMSPTelemetryState(void);
+void freeLtmTelemetryPort(void);
+void configureLtmTelemetryPort(void);
-void freeMSPTelemetryPort(void);
-void configureMSPTelemetryPort(void);
-
-#endif /* TELEMETRY_MSP_H_ */
+#endif /* TELEMETRY_LTM_H_ */
diff --git a/src/main/telemetry/msp.c b/src/main/telemetry/msp.c
deleted file mode 100644
index 7ba96884de..0000000000
--- a/src/main/telemetry/msp.c
+++ /dev/null
@@ -1,114 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight. If not, see .
- */
-
-/*
- * telemetry_MSP.c
- *
- * Created on: 22 Apr 2014
- * Author: trey marc
- */
-#include
-#include
-#include
-
-#include "platform.h"
-
-#include "build_config.h"
-
-#ifdef TELEMETRY
-
-#include "drivers/serial.h"
-#include "io/serial.h"
-#include "io/serial_msp.h"
-
-#include "telemetry/telemetry.h"
-#include "telemetry/msp.h"
-
-static telemetryConfig_t *telemetryConfig;
-static serialPortConfig_t *portConfig;
-
-static bool mspTelemetryEnabled = false;
-static portSharing_e mspPortSharing;
-
-#define TELEMETRY_MSP_INITIAL_PORT_MODE MODE_TX
-
-static serialPort_t *mspTelemetryPort = NULL;
-
-void initMSPTelemetry(telemetryConfig_t *initialTelemetryConfig)
-{
- telemetryConfig = initialTelemetryConfig;
- portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_MSP);
- mspPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_MSP);
-}
-
-void checkMSPTelemetryState(void)
-{
- bool newTelemetryEnabledValue = telemetryDetermineEnabledState(mspPortSharing);
-
- if (newTelemetryEnabledValue == mspTelemetryEnabled) {
- return;
- }
-
- if (newTelemetryEnabledValue)
- configureMSPTelemetryPort();
- else
- freeMSPTelemetryPort();
-}
-
-void handleMSPTelemetry(void)
-{
- if (!mspTelemetryEnabled) {
- return;
- }
-
- if (!mspTelemetryPort) {
- return;
- }
-
- //sendMspTelemetry(); TODO - Cleanup / fix
-}
-
-void freeMSPTelemetryPort(void)
-{
- mspReleasePortIfAllocated(mspTelemetryPort);
- closeSerialPort(mspTelemetryPort);
- mspTelemetryPort = NULL;
- mspTelemetryEnabled = false;
-}
-
-void configureMSPTelemetryPort(void)
-{
- if (!portConfig) {
- return;
- }
-
- baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
- if (baudRateIndex == BAUD_AUTO) {
- baudRateIndex = BAUD_19200;
- }
-
- mspTelemetryPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_MSP, NULL, baudRates[baudRateIndex], TELEMETRY_MSP_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
-
- if (!mspTelemetryPort) {
- return;
- }
- //mspSetTelemetryPort(mspTelemetryPort); TODO - Cleanup / Fix
-
- mspTelemetryEnabled = true;
-}
-
-#endif
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index 4edc4b6e47..4ac2713900 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -38,8 +38,8 @@
#include "telemetry/telemetry.h"
#include "telemetry/frsky.h"
#include "telemetry/hott.h"
-#include "telemetry/msp.h"
#include "telemetry/smartport.h"
+#include "telemetry/ltm.h"
static telemetryConfig_t *telemetryConfig;
@@ -52,8 +52,8 @@ void telemetryInit(void)
{
initFrSkyTelemetry(telemetryConfig);
initHoTTTelemetry(telemetryConfig);
- initMSPTelemetry(telemetryConfig);
initSmartPortTelemetry(telemetryConfig);
+ initLtmTelemetry(telemetryConfig);
telemetryCheckState();
}
@@ -76,16 +76,16 @@ void telemetryCheckState(void)
{
checkFrSkyTelemetryState();
checkHoTTTelemetryState();
- checkMSPTelemetryState();
checkSmartPortTelemetryState();
+ checkLtmTelemetryState();
}
void telemetryProcess(rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
{
handleFrSkyTelemetry(rxConfig, deadband3d_throttle);
handleHoTTTelemetry();
- handleMSPTelemetry();
handleSmartPortTelemetry();
+ handleLtmTelemetry();
}
#endif