fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 19:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

frsky_telemetry: send flight mode & gps info

Browse Source 
This uses the TEMP1 & TEMP2 fields, which probably were used for something
else initially. However this implementation matches with OpenTX and APM.
This commit is contained in:
Beat Küng
2017-08-08 14:47:01 +02:00
parent a2bfcb94ef
commit cb23817317
5 changed files with 164 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/common.h
+81
View File
@@ -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 <stdint.h>
/* 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);
src/drivers/frsky_telemetry/frsky_data.c
+7 -33
View File
@@ -41,6 +41,7 @@
*/
#include "frsky_data.h"
#include "common.h"
#include <stdlib.h>
#include <stdio.h>
@@ -58,39 +59,6 @@
#include <drivers/drv_hrt.h>
/* 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);
}
src/drivers/frsky_telemetry/frsky_telemetry.c
+55 -1
View File
@@ -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;
}
}
src/drivers/frsky_telemetry/sPort_data.c
+17
View File
@@ -42,6 +42,7 @@
*/
#include "sPort_data.h"
#include "common.h"
#include <stdlib.h>
#include <stdio.h>
@@ -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);
}
src/drivers/frsky_telemetry/sPort_data.h
+4 -1
View File
@@ -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);