Add LTM Telemetry // Remove MSP telemetry

This commit is contained in:
borisbstyle 2016-02-03 11:14:35 +01:00
parent 4888a3a375
commit 63dcc3dc1d
8 changed files with 328 additions and 138 deletions

View File

@ -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 \

View File

@ -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)

View File

@ -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 {

View File

@ -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);

309
src/main/telemetry/ltm.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <string.h>
#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

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* 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_ */

View File

@ -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 <http://www.gnu.org/licenses/>.
*/
/*
* telemetry_MSP.c
*
* Created on: 22 Apr 2014
* Author: trey marc
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#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

View File

@ -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