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

frsky_telemetry: move to uORB::Subscription

Browse Source
This commit is contained in:
Daniel Agar
2019-09-30 10:28:03 +02:00
committed by Beat Küng
parent 0af942ce12
commit 34b03d5659
2 changed files with 90 additions and 215 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
+43 -113
View File
@@ -51,6 +51,7 @@
#include <lib/ecl/geo/geo.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_air_data.h>
@@ -59,30 +60,21 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <drivers/drv_hrt.h>
#define frac(f) (f - (int)f)
struct s_port_subscription_data_s {
int sensor_sub;
int global_position_sub;
int local_position_sub;
int battery_status_sub;
int vehicle_status_sub;
int gps_position_sub;
int vehicle_air_data_sub;
struct sensor_combined_s sensor_combined;
struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct battery_status_s battery_status;
struct vehicle_status_s vehicle_status;
struct vehicle_air_data_s vehicle_air_data;
struct vehicle_gps_position_s gps_position;
uORB::SubscriptionData<battery_status_s> battery_status_sub{ORB_ID(battery_status)};
uORB::SubscriptionData<sensor_combined_s> sensor_combined_sub{ORB_ID(sensor_combined)};
uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
};
static struct s_port_subscription_data_s *s_port_subscription_data = NULL;
static struct s_port_subscription_data_s *s_port_subscription_data = nullptr;
/**
@@ -90,95 +82,34 @@ static struct s_port_subscription_data_s *s_port_subscription_data = NULL;
*/
bool sPort_init()
{
s_port_subscription_data = (struct s_port_subscription_data_s *)calloc(1, sizeof(struct s_port_subscription_data_s));
s_port_subscription_data = new s_port_subscription_data_s();
if (s_port_subscription_data == NULL) {
if (s_port_subscription_data == nullptr) {
return false;
}
s_port_subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
s_port_subscription_data->global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
s_port_subscription_data->local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
s_port_subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
s_port_subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
s_port_subscription_data->gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
s_port_subscription_data->vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data));
return true;
}
void sPort_deinit()
{
if (s_port_subscription_data) {
orb_unsubscribe(s_port_subscription_data->sensor_sub);
orb_unsubscribe(s_port_subscription_data->global_position_sub);
orb_unsubscribe(s_port_subscription_data->local_position_sub);
orb_unsubscribe(s_port_subscription_data->battery_status_sub);
orb_unsubscribe(s_port_subscription_data->vehicle_status_sub);
orb_unsubscribe(s_port_subscription_data->gps_position_sub);
orb_unsubscribe(s_port_subscription_data->vehicle_air_data_sub);
free(s_port_subscription_data);
s_port_subscription_data = NULL;
delete s_port_subscription_data;
s_port_subscription_data = nullptr;
}
}
void sPort_update_topics()
{
struct s_port_subscription_data_s *subs = s_port_subscription_data;
bool updated;
/* get a local copy of the current sensor values */
orb_check(subs->sensor_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined);
}
/* get a local copy of the battery data */
orb_check(subs->battery_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(battery_status), subs->battery_status_sub, &subs->battery_status);
}
/* get a local copy of the global position data */
orb_check(subs->global_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), subs->global_position_sub, &subs->global_pos);
}
/* get a local copy of the local position data */
orb_check(subs->local_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_local_position), subs->local_position_sub, &subs->local_pos);
}
/* get a local copy of the vehicle status data */
orb_check(subs->vehicle_status_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_status), subs->vehicle_status_sub, &subs->vehicle_status);
}
/* get a local copy of the gps position data */
orb_check(subs->gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), subs->gps_position_sub, &subs->gps_position);
}
/* get a local copy of the gps position data */
orb_check(subs->vehicle_air_data_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_air_data), subs->vehicle_air_data_sub, &subs->vehicle_air_data);
}
s_port_subscription_data->battery_status_sub.update();
s_port_subscription_data->sensor_combined_sub.update();
s_port_subscription_data->vehicle_air_data_sub.update();
s_port_subscription_data->vehicle_global_position_sub.update();
s_port_subscription_data->vehicle_gps_position_sub.update();
s_port_subscription_data->vehicle_local_position_sub.update();
s_port_subscription_data->vehicle_status_sub.update();
}
static void update_crc(uint16_t *crc, unsigned char b)
{
*crc += b;
@@ -248,7 +179,7 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
void sPort_send_BATV(int uart)
{
/* send battery voltage as VFAS */
uint32_t voltage = (int)(100 * s_port_subscription_data->battery_status.voltage_v);
uint32_t voltage = (int)(100 * s_port_subscription_data->battery_status_sub.get().voltage_v);
sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
}
@@ -256,7 +187,7 @@ void sPort_send_BATV(int uart)
void sPort_send_CUR(int uart)
{
/* send data */
uint32_t current = (int)(10 * s_port_subscription_data->battery_status.current_a);
uint32_t current = (int)(10 * s_port_subscription_data->battery_status_sub.get().current_a);
sPort_send_data(uart, SMARTPORT_ID_CURR, current);
}
@@ -266,16 +197,17 @@ void sPort_send_CUR(int uart)
void sPort_send_ALT(int uart)
{
/* send data */
uint32_t alt = (int)(100 * s_port_subscription_data->vehicle_air_data.baro_alt_meter);
uint32_t alt = (int)(100 * s_port_subscription_data->vehicle_air_data_sub.get().baro_alt_meter);
sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
}
// verified scaling for "calculated" option
void sPort_send_SPD(int uart)
{
struct vehicle_global_position_s *global_pos = &s_port_subscription_data->global_pos;
const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
/* send data for A2 */
float speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint32_t ispeed = (int)(10 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
@@ -292,7 +224,7 @@ void sPort_send_VSPD(int uart, float speed)
void sPort_send_FUEL(int uart)
{
/* send data */
uint32_t fuel = (int)(100 * s_port_subscription_data->battery_status.remaining);
uint32_t fuel = (int)(100 * s_port_subscription_data->battery_status_sub.get().remaining);
sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
}
@@ -301,11 +233,11 @@ void sPort_send_GPS_LON(int uart)
/* send longitude */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
/* precision is approximately 0.1m */
uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->gps_position.lon);
uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon);
iLon |= (1 << 31);
if (s_port_subscription_data->gps_position.lon < 0) { iLon |= (1 << 30); }
if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 0) { iLon |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
}
@@ -314,9 +246,9 @@ void sPort_send_GPS_LAT(int uart)
{
/* send latitude */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->gps_position.lat);
uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat);
if (s_port_subscription_data->gps_position.lat < 0) { iLat |= (1 << 30); }
if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 0) { iLat |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
}
@@ -324,7 +256,7 @@ void sPort_send_GPS_LAT(int uart)
void sPort_send_GPS_ALT(int uart)
{
/* send altitude */
uint32_t iAlt = s_port_subscription_data->gps_position.alt / 10;
uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10;
sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
}
@@ -333,7 +265,7 @@ void sPort_send_GPS_CRS(int uart)
/* send course */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
int32_t iYaw = s_port_subscription_data->local_pos.yaw * 18000.0f / M_PI_F;
int32_t iYaw = s_port_subscription_data->vehicle_local_position_sub.get().yaw * 18000.0f / M_PI_F;
if (iYaw < 0) { iYaw += 36000; }
@@ -345,7 +277,7 @@ void sPort_send_GPS_TIME(int uart)
static int date = 0;
/* send formatted frame */
time_t time_gps = s_port_subscription_data->gps_position.time_utc_usec / 1000000ULL;
time_t time_gps = s_port_subscription_data->vehicle_gps_position_sub.get().time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
if (date) {
@@ -365,9 +297,10 @@ void sPort_send_GPS_TIME(int uart)
void sPort_send_GPS_SPD(int uart)
{
struct vehicle_global_position_s *global_pos = &s_port_subscription_data->global_pos;
const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get();
/* send 100 * knots */
float speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
uint32_t ispeed = (int)(1944 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
@@ -377,7 +310,7 @@ void sPort_send_GPS_SPD(int uart)
*/
void sPort_send_NAV_STATE(int uart)
{
uint32_t navstate = (int)(128 + s_port_subscription_data->vehicle_status.nav_state);
uint32_t navstate = (int)(128 + s_port_subscription_data->vehicle_status_sub.get().nav_state);
/* send data */
sPort_send_data(uart, SMARTPORT_ID_DIY_NAVSTATE, navstate);
@@ -388,24 +321,21 @@ void sPort_send_NAV_STATE(int uart)
void sPort_send_GPS_FIX(int uart)
{
/* send data */
uint32_t satcount = (int)(s_port_subscription_data->gps_position.satellites_used);
uint32_t fixtype = (int)(s_port_subscription_data->gps_position.fix_type);
uint32_t satcount = (int)(s_port_subscription_data->vehicle_gps_position_sub.get().satellites_used);
uint32_t fixtype = (int)(s_port_subscription_data->vehicle_gps_position_sub.get().fix_type);
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);
int16_t telem_flight_mode = get_telemetry_flight_mode(s_port_subscription_data->vehicle_status_sub.get().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);
const vehicle_gps_position_s &gps = s_port_subscription_data->vehicle_gps_position_sub.get();
sPort_send_data(uart, FRSKY_ID_TEMP2, gps.satellites_used * 10 + gps.fix_type);
}