mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
506 lines
14 KiB
C
506 lines
14 KiB
C
/*
|
|
* 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 low 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 "platform.h"
|
|
|
|
|
|
#if defined(USE_TELEMETRY_LTM)
|
|
|
|
#include "build/build_config.h"
|
|
|
|
#include "common/axis.h"
|
|
#include "common/color.h"
|
|
#include "common/streambuf.h"
|
|
#include "common/utils.h"
|
|
|
|
#include "drivers/serial.h"
|
|
#include "drivers/time.h"
|
|
|
|
#include "fc/config.h"
|
|
#include "fc/fc_core.h"
|
|
#include "fc/rc_controls.h"
|
|
#include "fc/runtime_config.h"
|
|
|
|
#include "flight/imu.h"
|
|
#include "flight/failsafe.h"
|
|
#include "flight/mixer.h"
|
|
#include "flight/pid.h"
|
|
|
|
#include "io/gps.h"
|
|
#include "io/ledstrip.h"
|
|
#include "io/serial.h"
|
|
|
|
#include "navigation/navigation.h"
|
|
|
|
#include "rx/rx.h"
|
|
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/barometer.h"
|
|
#include "sensors/battery.h"
|
|
#include "sensors/boardalignment.h"
|
|
#include "sensors/diagnostics.h"
|
|
#include "sensors/gyro.h"
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/pitotmeter.h"
|
|
#include "sensors/diagnostics.h"
|
|
|
|
#include "telemetry/ltm.h"
|
|
#include "telemetry/telemetry.h"
|
|
|
|
|
|
#define TELEMETRY_LTM_INITIAL_PORT_MODE MODE_TX
|
|
#define LTM_CYCLETIME 100
|
|
#define LTM_SCHEDULE_SIZE (1000/LTM_CYCLETIME)
|
|
|
|
static serialPort_t *ltmPort;
|
|
static serialPortConfig_t *portConfig;
|
|
static bool ltmEnabled;
|
|
static portSharing_e ltmPortSharing;
|
|
static uint8_t ltmFrame[LTM_MAX_MESSAGE_SIZE];
|
|
static uint8_t ltm_x_counter;
|
|
|
|
static void ltm_initialise_packet(sbuf_t *dst)
|
|
{
|
|
dst->ptr = ltmFrame;
|
|
dst->end = ARRAYEND(ltmFrame);
|
|
|
|
sbufWriteU8(dst, '$');
|
|
sbufWriteU8(dst, 'T');
|
|
}
|
|
|
|
static void ltm_finalise(sbuf_t *dst)
|
|
{
|
|
uint8_t crc = 0;
|
|
for (const uint8_t *ptr = <mFrame[3]; ptr < dst->ptr; ++ptr) {
|
|
crc ^= *ptr;
|
|
}
|
|
sbufWriteU8(dst, crc);
|
|
sbufSwitchToReader(dst, ltmFrame);
|
|
serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst));
|
|
}
|
|
|
|
#if defined(USE_GPS)
|
|
/*
|
|
* GPS G-frame 5Hhz at > 2400 baud
|
|
* LAT LON SPD ALT SAT/FIX
|
|
*/
|
|
void ltm_gframe(sbuf_t *dst)
|
|
{
|
|
uint8_t gps_fix_type = 0;
|
|
int32_t ltm_lat = 0, ltm_lon = 0, ltm_alt = 0, ltm_gs = 0;
|
|
|
|
if (sensors(SENSOR_GPS)
|
|
#ifdef USE_GPS_FIX_ESTIMATION
|
|
|| STATE(GPS_ESTIMATED_FIX)
|
|
#endif
|
|
) {
|
|
if (gpsSol.fixType == GPS_NO_FIX)
|
|
gps_fix_type = 1;
|
|
else if (gpsSol.fixType == GPS_FIX_2D)
|
|
gps_fix_type = 2;
|
|
else if (gpsSol.fixType == GPS_FIX_3D)
|
|
gps_fix_type = 3;
|
|
|
|
ltm_lat = gpsSol.llh.lat;
|
|
ltm_lon = gpsSol.llh.lon;
|
|
ltm_gs = gpsSol.groundSpeed / 100;
|
|
}
|
|
|
|
ltm_alt = getEstimatedActualPosition(Z); // cm
|
|
|
|
sbufWriteU8(dst, 'G');
|
|
sbufWriteU32(dst, ltm_lat);
|
|
sbufWriteU32(dst, ltm_lon);
|
|
sbufWriteU8(dst, (uint8_t)ltm_gs);
|
|
sbufWriteU32(dst, ltm_alt);
|
|
sbufWriteU8(dst, (gpsSol.numSat << 2) | gps_fix_type);
|
|
}
|
|
#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
|
|
*/
|
|
|
|
void ltm_sframe(sbuf_t *dst)
|
|
{
|
|
ltm_modes_e lt_flightmode;
|
|
|
|
if (FLIGHT_MODE(MANUAL_MODE))
|
|
lt_flightmode = LTM_MODE_MANUAL;
|
|
else if (FLIGHT_MODE(NAV_WP_MODE))
|
|
lt_flightmode = LTM_MODE_WAYPOINTS;
|
|
else if (FLIGHT_MODE(NAV_RTH_MODE))
|
|
lt_flightmode = LTM_MODE_RTH;
|
|
else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
|
|
lt_flightmode = LTM_MODE_GPSHOLD;
|
|
else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
|
|
lt_flightmode = LTM_MODE_CRUISE;
|
|
else if (FLIGHT_MODE(NAV_LAUNCH_MODE))
|
|
lt_flightmode = LTM_MODE_LAUNCH;
|
|
else if (FLIGHT_MODE(AUTO_TUNE))
|
|
lt_flightmode = LTM_MODE_AUTOTUNE;
|
|
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
|
|
lt_flightmode = LTM_MODE_ALTHOLD;
|
|
else if (FLIGHT_MODE(HEADFREE_MODE) || FLIGHT_MODE(HEADING_MODE))
|
|
lt_flightmode = LTM_MODE_HEADHOLD;
|
|
else if (FLIGHT_MODE(ANGLE_MODE))
|
|
lt_flightmode = LTM_MODE_ANGLE;
|
|
else if (FLIGHT_MODE(HORIZON_MODE))
|
|
lt_flightmode = LTM_MODE_HORIZON;
|
|
#ifdef USE_FW_AUTOLAND
|
|
else if (FLIGHT_MODE(NAV_FW_AUTOLAND))
|
|
lt_flightmode = LTM_MODE_LAND;
|
|
#endif
|
|
else
|
|
lt_flightmode = LTM_MODE_RATE; // Rate mode
|
|
|
|
uint8_t lt_statemode = (ARMING_FLAG(ARMED)) ? 1 : 0;
|
|
if (failsafeIsActive())
|
|
lt_statemode |= 2;
|
|
sbufWriteU8(dst, 'S');
|
|
sbufWriteU16(dst, getBatteryVoltage() * 10); //vbat converted to mv
|
|
sbufWriteU16(dst, (uint16_t)constrain(getMAhDrawn(), 0, 0xFFFF)); // current mAh (65535 mAh max)
|
|
sbufWriteU8(dst, (uint8_t)((getRSSI() * 254) / 1023)); // scaled RSSI (uchar)
|
|
#if defined(USE_PITOT)
|
|
sbufWriteU8(dst, (sensors(SENSOR_PITOT) && pitotIsHealthy())? getAirspeedEstimate() / 100.0f : 0); // in m/s
|
|
#else
|
|
sbufWriteU8(dst, 0);
|
|
#endif
|
|
sbufWriteU8(dst, (lt_flightmode << 2) | lt_statemode);
|
|
}
|
|
|
|
/*
|
|
* Attitude A-frame - 10 Hz at > 2400 baud
|
|
* PITCH ROLL HEADING
|
|
*/
|
|
void ltm_aframe(sbuf_t *dst)
|
|
{
|
|
sbufWriteU8(dst, 'A');
|
|
sbufWriteU16(dst, (int16_t)DECIDEGREES_TO_DEGREES(attitude.values.pitch));
|
|
sbufWriteU16(dst, (int16_t)DECIDEGREES_TO_DEGREES(attitude.values.roll));
|
|
sbufWriteU16(dst, (int16_t)DECIDEGREES_TO_DEGREES(attitude.values.yaw));
|
|
}
|
|
|
|
#if defined(USE_GPS)
|
|
/*
|
|
* 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
|
|
*/
|
|
void ltm_oframe(sbuf_t *dst)
|
|
{
|
|
sbufWriteU8(dst, 'O');
|
|
sbufWriteU32(dst, GPS_home.lat);
|
|
sbufWriteU32(dst, GPS_home.lon);
|
|
sbufWriteU32(dst, GPS_home.alt);
|
|
sbufWriteU8(dst, 1); // OSD always ON
|
|
sbufWriteU8(dst, STATE(GPS_FIX_HOME) ? 1 : 0);
|
|
}
|
|
#endif
|
|
|
|
/*
|
|
* Extended information data frame, 1 Hz rate
|
|
* This frame is intended to report extended GPS and NAV data, however at the moment it contains only HDOP value
|
|
*/
|
|
void ltm_xframe(sbuf_t *dst)
|
|
{
|
|
uint8_t sensorStatus =
|
|
(isHardwareHealthy() ? 0 : 1) << 0; // bit 0 - hardware failure indication (1 - something is wrong with the hardware sensors)
|
|
|
|
sbufWriteU8(dst, 'X');
|
|
#if defined(USE_GPS)
|
|
sbufWriteU16(dst, gpsSol.hdop);
|
|
#else
|
|
sbufWriteU16(dst, 9999);
|
|
#endif
|
|
sbufWriteU8(dst, sensorStatus);
|
|
sbufWriteU8(dst, ltm_x_counter);
|
|
sbufWriteU8(dst, getDisarmReason());
|
|
sbufWriteU8(dst, 0);
|
|
ltm_x_counter++; // overflow is OK
|
|
}
|
|
|
|
/** OSD additional data frame, ~4 Hz rate, navigation system status
|
|
*/
|
|
void ltm_nframe(sbuf_t *dst)
|
|
{
|
|
sbufWriteU8(dst, 'N');
|
|
sbufWriteU8(dst, NAV_Status.mode);
|
|
sbufWriteU8(dst, NAV_Status.state);
|
|
sbufWriteU8(dst, NAV_Status.activeWpAction);
|
|
sbufWriteU8(dst, NAV_Status.activeWpNumber);
|
|
sbufWriteU8(dst, NAV_Status.error);
|
|
sbufWriteU8(dst, NAV_Status.flags);
|
|
}
|
|
|
|
#define LTM_BIT_AFRAME (1 << 0)
|
|
#define LTM_BIT_GFRAME (1 << 1)
|
|
#define LTM_BIT_SFRAME (1 << 2)
|
|
#define LTM_BIT_OFRAME (1 << 3)
|
|
#define LTM_BIT_NFRAME (1 << 4)
|
|
#define LTM_BIT_XFRAME (1 << 5)
|
|
|
|
/*
|
|
* This is the normal (default) scheduler, needs c. 4800 baud or faster
|
|
* Equates to c. 303 bytes / second
|
|
*/
|
|
static uint8_t ltm_normal_schedule[LTM_SCHEDULE_SIZE] = {
|
|
LTM_BIT_AFRAME | LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_OFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_XFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME | LTM_BIT_NFRAME
|
|
};
|
|
|
|
/*
|
|
* This is the medium scheduler, needs c. 2400 baud or faster
|
|
* Equates to c. 164 bytes / second
|
|
*/
|
|
static uint8_t ltm_medium_schedule[LTM_SCHEDULE_SIZE] = {
|
|
LTM_BIT_AFRAME,
|
|
LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME,
|
|
LTM_BIT_OFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_XFRAME,
|
|
LTM_BIT_OFRAME,
|
|
LTM_BIT_AFRAME | LTM_BIT_SFRAME,
|
|
LTM_BIT_GFRAME,
|
|
LTM_BIT_AFRAME,
|
|
LTM_BIT_NFRAME
|
|
};
|
|
|
|
/*
|
|
* This is the slow scheduler, needs c. 1200 baud or faster
|
|
* Equates to c. 105 bytes / second (91 b/s if the second GFRAME is zeroed)
|
|
*/
|
|
static uint8_t ltm_slow_schedule[LTM_SCHEDULE_SIZE] = {
|
|
LTM_BIT_GFRAME,
|
|
LTM_BIT_SFRAME,
|
|
LTM_BIT_AFRAME,
|
|
0,
|
|
LTM_BIT_OFRAME,
|
|
LTM_BIT_XFRAME,
|
|
LTM_BIT_GFRAME, // consider zeroing this for even lower bytes/sec
|
|
0,
|
|
LTM_BIT_AFRAME,
|
|
LTM_BIT_NFRAME,
|
|
};
|
|
|
|
/* Set by initialisation */
|
|
static uint8_t *ltm_schedule;
|
|
|
|
static void process_ltm(void)
|
|
{
|
|
static uint8_t ltm_scheduler = 0;
|
|
uint8_t current_schedule = ltm_schedule[ltm_scheduler];
|
|
|
|
sbuf_t ltmFrameBuf;
|
|
sbuf_t *dst = <mFrameBuf;
|
|
|
|
if (current_schedule & LTM_BIT_AFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_aframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
|
|
#if defined(USE_GPS)
|
|
if (current_schedule & LTM_BIT_GFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_gframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
|
|
if (current_schedule & LTM_BIT_OFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_oframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
|
|
if (current_schedule & LTM_BIT_XFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_xframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
#endif
|
|
|
|
if (current_schedule & LTM_BIT_SFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_sframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
|
|
if (current_schedule & LTM_BIT_NFRAME) {
|
|
ltm_initialise_packet(dst);
|
|
ltm_nframe(dst);
|
|
ltm_finalise(dst);
|
|
}
|
|
|
|
ltm_scheduler = (ltm_scheduler + 1) % 10;
|
|
}
|
|
|
|
void handleLtmTelemetry(void)
|
|
{
|
|
static uint32_t ltm_lastCycleTime;
|
|
if (!ltmEnabled)
|
|
return;
|
|
if (!ltmPort)
|
|
return;
|
|
const uint32_t now = millis();
|
|
if ((now - ltm_lastCycleTime) >= LTM_CYCLETIME) {
|
|
process_ltm();
|
|
ltm_lastCycleTime = now;
|
|
}
|
|
}
|
|
|
|
void freeLtmTelemetryPort(void)
|
|
{
|
|
closeSerialPort(ltmPort);
|
|
ltmPort = NULL;
|
|
ltmEnabled = false;
|
|
}
|
|
|
|
void initLtmTelemetry(void)
|
|
{
|
|
portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_LTM);
|
|
ltmPortSharing = determinePortSharing(portConfig, FUNCTION_TELEMETRY_LTM);
|
|
}
|
|
|
|
|
|
|
|
static void configureLtmScheduler(void)
|
|
{
|
|
|
|
/* setup scheduler, default to 'normal' */
|
|
if (telemetryConfig()->ltmUpdateRate == LTM_RATE_MEDIUM)
|
|
ltm_schedule = ltm_medium_schedule;
|
|
else if (telemetryConfig()->ltmUpdateRate == LTM_RATE_SLOW)
|
|
ltm_schedule = ltm_slow_schedule;
|
|
else
|
|
ltm_schedule = ltm_normal_schedule;
|
|
|
|
}
|
|
|
|
void configureLtmTelemetryPort(void)
|
|
{
|
|
if (!portConfig) {
|
|
return;
|
|
}
|
|
baudRate_e baudRateIndex = portConfig->telemetry_baudrateIndex;
|
|
if (baudRateIndex == BAUD_AUTO) {
|
|
baudRateIndex = BAUD_19200;
|
|
}
|
|
|
|
/* Sanity check that we can support the scheduler */
|
|
if (baudRateIndex == BAUD_2400 && telemetryConfig()->ltmUpdateRate == LTM_RATE_NORMAL)
|
|
ltm_schedule = ltm_medium_schedule;
|
|
if (baudRateIndex == BAUD_1200)
|
|
ltm_schedule = ltm_slow_schedule;
|
|
|
|
ltmPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_LTM, NULL, NULL, baudRates[baudRateIndex], TELEMETRY_LTM_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED);
|
|
if (!ltmPort)
|
|
return;
|
|
ltm_x_counter = 0;
|
|
ltmEnabled = true;
|
|
}
|
|
|
|
void checkLtmTelemetryState(void)
|
|
{
|
|
if (portConfig && telemetryCheckRxPortShared(portConfig)) {
|
|
if (!ltmEnabled && telemetrySharedPort != NULL) {
|
|
ltmPort = telemetrySharedPort;
|
|
configureLtmScheduler();
|
|
ltmEnabled = true;
|
|
}
|
|
} else {
|
|
bool newTelemetryEnabledValue = telemetryDetermineEnabledState(ltmPortSharing);
|
|
if (newTelemetryEnabledValue == ltmEnabled)
|
|
return;
|
|
if (newTelemetryEnabledValue){
|
|
configureLtmScheduler();
|
|
configureLtmTelemetryPort();
|
|
|
|
}
|
|
else
|
|
freeLtmTelemetryPort();
|
|
}
|
|
}
|
|
|
|
int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType)
|
|
{
|
|
static uint8_t ltmFrame[LTM_MAX_MESSAGE_SIZE];
|
|
|
|
sbuf_t ltmFrameBuf = { .ptr = ltmFrame, .end =ARRAYEND(ltmFrame) };
|
|
sbuf_t * const sbuf = <mFrameBuf;
|
|
|
|
switch (ltmFrameType) {
|
|
default:
|
|
case LTM_AFRAME:
|
|
ltm_aframe(sbuf);
|
|
break;
|
|
case LTM_SFRAME:
|
|
ltm_sframe(sbuf);
|
|
break;
|
|
#if defined(USE_GPS)
|
|
case LTM_GFRAME:
|
|
ltm_gframe(sbuf);
|
|
break;
|
|
case LTM_OFRAME:
|
|
ltm_oframe(sbuf);
|
|
break;
|
|
#endif
|
|
case LTM_XFRAME:
|
|
ltm_xframe(sbuf);
|
|
break;
|
|
case LTM_NFRAME:
|
|
ltm_nframe(sbuf);
|
|
break;
|
|
}
|
|
sbufSwitchToReader(sbuf, ltmFrame);
|
|
const int frameSize = sbufBytesRemaining(sbuf);
|
|
for (int ii = 0; sbufBytesRemaining(sbuf); ++ii) {
|
|
frame[ii] = sbufReadU8(sbuf);
|
|
}
|
|
return frameSize;
|
|
}
|
|
#endif
|