From cb238173178008eac71aa6b2ccdcacb030094496 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 31 Jul 2017 15:39:14 +0200 Subject: [PATCH] frsky_telemetry: send flight mode & gps info This uses the TEMP1 & TEMP2 fields, which probably were used for something else initially. However this implementation matches with OpenTX and APM. --- src/drivers/frsky_telemetry/common.h | 81 +++++++++++++++++++ src/drivers/frsky_telemetry/frsky_data.c | 40 ++------- src/drivers/frsky_telemetry/frsky_telemetry.c | 56 ++++++++++++- src/drivers/frsky_telemetry/sPort_data.c | 17 ++++ src/drivers/frsky_telemetry/sPort_data.h | 5 +- 5 files changed, 164 insertions(+), 35 deletions(-) create mode 100644 src/drivers/frsky_telemetry/common.h diff --git a/src/drivers/frsky_telemetry/common.h b/src/drivers/frsky_telemetry/common.h new file mode 100644 index 0000000000..3ee37f0d95 --- /dev/null +++ b/src/drivers/frsky_telemetry/common.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file common.h + * + * common declarations + * + */ +#pragma once + +#include + +/* FrSky sensor hub data IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + + +/** + * Map the PX4 flight mode (vehicle_status_s::nav_state) to the telemetry flight mode + */ +uint16_t get_telemetry_flight_mode(int px4_flight_mode); diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 28e04032ec..3fb3c169b4 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -41,6 +41,7 @@ */ #include "frsky_data.h" +#include "common.h" #include #include @@ -58,39 +59,6 @@ #include -/* FrSky sensor hub data IDs */ -#define FRSKY_ID_GPS_ALT_BP 0x01 -#define FRSKY_ID_TEMP1 0x02 -#define FRSKY_ID_RPM 0x03 -#define FRSKY_ID_FUEL 0x04 -#define FRSKY_ID_TEMP2 0x05 -#define FRSKY_ID_VOLTS 0x06 -#define FRSKY_ID_GPS_ALT_AP 0x09 -#define FRSKY_ID_BARO_ALT_BP 0x10 -#define FRSKY_ID_GPS_SPEED_BP 0x11 -#define FRSKY_ID_GPS_LONG_BP 0x12 -#define FRSKY_ID_GPS_LAT_BP 0x13 -#define FRSKY_ID_GPS_COURS_BP 0x14 -#define FRSKY_ID_GPS_DAY_MONTH 0x15 -#define FRSKY_ID_GPS_YEAR 0x16 -#define FRSKY_ID_GPS_HOUR_MIN 0x17 -#define FRSKY_ID_GPS_SEC 0x18 -#define FRSKY_ID_GPS_SPEED_AP 0x19 -#define FRSKY_ID_GPS_LONG_AP 0x1A -#define FRSKY_ID_GPS_LAT_AP 0x1B -#define FRSKY_ID_GPS_COURS_AP 0x1C -#define FRSKY_ID_BARO_ALT_AP 0x21 -#define FRSKY_ID_GPS_LONG_EW 0x22 -#define FRSKY_ID_GPS_LAT_NS 0x23 -#define FRSKY_ID_ACCEL_X 0x24 -#define FRSKY_ID_ACCEL_Y 0x25 -#define FRSKY_ID_ACCEL_Z 0x26 -#define FRSKY_ID_CURRENT 0x28 -#define FRSKY_ID_VARIO 0x30 -#define FRSKY_ID_VFAS 0x39 -#define FRSKY_ID_VOLTS_BP 0x3A -#define FRSKY_ID_VOLTS_AP 0x3B - #define frac(f) (f - (int)f) struct frsky_subscription_data_s { @@ -251,6 +219,12 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_CURRENT, (subs->battery_status.current_a < 0) ? 0 : roundf(subs->battery_status.current_a * 10.0f)); + int16_t telem_flight_mode = get_telemetry_flight_mode(subs->current_flight_mode); + frsky_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM + + frsky_send_data(uart, FRSKY_ID_TEMP2, subs->vehicle_gps_position.satellites_used * 10 + + subs->vehicle_gps_position.fix_type); + frsky_send_startstop(uart); } diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index fd47fad761..ffb80d83a9 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -64,6 +64,7 @@ #include "sPort_data.h" #include "frsky_data.h" +#include "common.h" /* thread state */ @@ -80,7 +81,44 @@ static void usage(void); static int frsky_telemetry_thread_main(int argc, char *argv[]); __EXPORT int frsky_telemetry_main(int argc, char *argv[]); -#define DEBUG + +uint16_t get_telemetry_flight_mode(int px4_flight_mode) +{ + // map the flight modes (see https://github.com/ilihack/LuaPilot_Taranis_Telemetry/blob/master/SCRIPTS/TELEMETRY/LuaPil.lua#L790) + switch (px4_flight_mode) { + case 0: return 18; // manual + + case 1: return 23; // alt control + + case 2: return 22; // pos control + + case 3: return 27; // mission + + case 4: return 26; // loiter + + case 5: + case 6: + case 7: return 28; // rtl + + case 10: return 19; // acro + + case 14: return 24; // offboard + + case 15: return 20; // stabilized + + case 16: return 21; // rattitude + + case 17: return 25; // takeoff + + case 8: + case 9: + case 18: return 29; // land + + case 19: return 30; // follow target + } + + return -1; +} /** * Opens the UART device and sets all required serial parameters. @@ -486,6 +524,22 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) sPort_send_GPS_FIX(uart); } + break; + + case SMARTPORT_SENSOR_ID_SP2UR: { + static int elementCount = 0; + + switch (elementCount++ % 2) { + case 0: + sPort_send_flight_mode(uart); + break; + + default: + sPort_send_GPS_info(uart); + break; + } + } + break; } } diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index 0fce287c57..5c737be194 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -42,6 +42,7 @@ */ #include "sPort_data.h" +#include "common.h" #include #include @@ -380,3 +381,19 @@ void sPort_send_GPS_FIX(int uart) uint32_t t2 = satcount * 10 + fixtype; sPort_send_data(uart, SMARTPORT_ID_DIY_GPSFIX, t2); } + + +void sPort_send_flight_mode(int uart) +{ + struct s_port_subscription_data_s *subs = s_port_subscription_data; + int16_t telem_flight_mode = get_telemetry_flight_mode(subs->vehicle_status.nav_state); + + sPort_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM + +} + +void sPort_send_GPS_info(int uart) +{ + struct s_port_subscription_data_s *subs = s_port_subscription_data; + sPort_send_data(uart, FRSKY_ID_TEMP2, subs->gps_position.satellites_used * 10 + subs->gps_position.fix_type); +} diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h index eb6b009969..e020f74e2b 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -54,8 +54,9 @@ #define SMARTPORT_POLL_6 0x00 #define SMARTPORT_POLL_7 0x83 #define SMARTPORT_POLL_8 0xBA +#define SMARTPORT_SENSOR_ID_SP2UR 0xC6 // Sensor ID 6 -/* FrSky SmartPort sensor IDs. See more here: https://github.com/opentx/opentx/blob/master/radio/src/telemetry/frsky.h#L109 */ +/* FrSky SmartPort sensor IDs. See more here: https://github.com/opentx/opentx/blob/2.2/radio/src/telemetry/frsky.h */ #define SMARTPORT_ID_RSSI 0xf101 #define SMARTPORT_ID_RXA1 0xf102 // supplied by RX #define SMARTPORT_ID_RXA2 0xf103 // supplied by RX @@ -100,6 +101,8 @@ void sPort_send_GPS_ALT(int uart); void sPort_send_GPS_SPD(int uart); void sPort_send_GPS_CRS(int uart); void sPort_send_GPS_TIME(int uart); +void sPort_send_flight_mode(int uart); +void sPort_send_GPS_info(int uart); void sPort_send_NAV_STATE(int uart); void sPort_send_GPS_FIX(int uart);