From 63dcc3dc1deb20ac9980119265dc3c99a80df351 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 3 Feb 2016 11:14:35 +0100 Subject: [PATCH] Add LTM Telemetry // Remove MSP telemetry --- Makefile | 2 +- src/main/io/serial.c | 2 +- src/main/io/serial.h | 4 +- src/main/mw.c | 2 +- src/main/telemetry/ltm.c | 309 ++++++++++++++++++++++++++++ src/main/telemetry/{msp.h => ltm.h} | 25 +-- src/main/telemetry/msp.c | 114 ---------- src/main/telemetry/telemetry.c | 8 +- 8 files changed, 328 insertions(+), 138 deletions(-) create mode 100644 src/main/telemetry/ltm.c rename src/main/telemetry/{msp.h => ltm.h} (65%) delete mode 100644 src/main/telemetry/msp.c 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