fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 04:23:34 +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/frsky_data.cpp
+47 -102
View File
@@ -51,6 +51,7 @@
#include <stdbool.h>
#include <math.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_air_data.h>
@@ -63,53 +64,36 @@
#define frac(f) (f - (int)f)
struct frsky_subscription_data_s {
struct battery_status_s battery_status;
struct vehicle_global_position_s global_pos;
struct sensor_combined_s sensor_combined;
struct vehicle_air_data_s airdata;
struct vehicle_gps_position_s vehicle_gps_position;
uint8_t current_flight_mode; // == vehicle_status.nav_state
int battery_status_sub;
int vehicle_air_data_sub;
int vehicle_global_position_sub;
int sensor_sub;
int vehicle_gps_position_sub;
int vehicle_status_sub;
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_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
};
static struct frsky_subscription_data_s *subscription_data = NULL;
static struct frsky_subscription_data_s *subscription_data = nullptr;
/**
* Initializes the uORB subscriptions.
*/
bool frsky_init()
{
subscription_data = (struct frsky_subscription_data_s *)calloc(1, sizeof(struct frsky_subscription_data_s));
subscription_data = new frsky_subscription_data_s();
if (!subscription_data) {
return false;
}
subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
subscription_data->vehicle_air_data_sub = orb_subscribe(ORB_ID(vehicle_air_data));
subscription_data->vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
subscription_data->sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
subscription_data->vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
subscription_data->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
return true;
}
void frsky_deinit()
{
if (subscription_data) {
orb_unsubscribe(subscription_data->battery_status_sub);
orb_unsubscribe(subscription_data->vehicle_global_position_sub);
orb_unsubscribe(subscription_data->sensor_sub);
orb_unsubscribe(subscription_data->vehicle_gps_position_sub);
orb_unsubscribe(subscription_data->vehicle_status_sub);
free(subscription_data);
subscription_data = NULL;
delete subscription_data;
subscription_data = nullptr;
}
}
@@ -162,51 +146,12 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)
void frsky_update_topics()
{
struct frsky_subscription_data_s *subs = 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);
}
orb_check(subs->vehicle_air_data_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_air_data), subs->vehicle_air_data_sub, &subs->airdata);
}
/* get a local copy of the vehicle status */
orb_check(subs->vehicle_status_sub, &updated);
if (updated) {
struct vehicle_status_s vehicle_status;
orb_copy(ORB_ID(vehicle_status), subs->vehicle_status_sub, &vehicle_status);
subs->current_flight_mode = vehicle_status.nav_state;
}
/* 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->vehicle_global_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position), subs->vehicle_global_position_sub, &subs->global_pos);
}
/* get a local copy of the raw GPS data */
orb_check(subs->vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), subs->vehicle_gps_position_sub, &subs->vehicle_gps_position);
}
subscription_data->battery_status_sub.update();
subscription_data->sensor_combined_sub.update();
subscription_data->vehicle_air_data_sub.update();
subscription_data->vehicle_global_position_sub.update();
subscription_data->vehicle_gps_position_sub.update();
subscription_data->vehicle_status_sub.update();
}
/**
@@ -215,26 +160,25 @@ void frsky_update_topics()
*/
void frsky_send_frame1(int uart)
{
struct frsky_subscription_data_s *subs = subscription_data;
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(subs->sensor_combined.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(subs->sensor_combined.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(subs->sensor_combined.accelerometer_m_s2[2] * 1000.0f));
const sensor_combined_s &sensor_combined = subscription_data->sensor_combined_sub.get();
frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined.accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined.accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined.accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, subs->airdata.baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, roundf(frac(subs->airdata.baro_alt_meter) * 100.0f));
const vehicle_air_data_s &air_data = subscription_data->vehicle_air_data_sub.get();
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, air_data.baro_alt_meter);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, roundf(frac(air_data.baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(subs->battery_status.voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(subs->battery_status.current_a < 0) ? 0 : roundf(subs->battery_status.current_a * 10.0f));
const battery_status_s &bat = subscription_data->battery_status_sub.get();
frsky_send_data(uart, FRSKY_ID_VFAS, roundf(bat.voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT, (bat.current_a < 0) ? 0 : roundf(bat.current_a * 10.0f));
int16_t telem_flight_mode = get_telemetry_flight_mode(subs->current_flight_mode);
int16_t telem_flight_mode = get_telemetry_flight_mode(subscription_data->vehicle_status_sub.get().nav_state);
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);
const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get();
frsky_send_data(uart, FRSKY_ID_TEMP2, gps.satellites_used * 10 + gps.fix_type);
frsky_send_startstop(uart);
}
@@ -254,32 +198,33 @@ static float frsky_format_gps(float dec)
*/
void frsky_send_frame2(int uart)
{
struct vehicle_global_position_s *global_pos = &subscription_data->global_pos;
struct battery_status_s *battery_status = &subscription_data->battery_status;
struct vehicle_gps_position_s *gps = &subscription_data->vehicle_gps_position;
const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get();
const battery_status_s &battery_status = subscription_data->battery_status_sub.get();
const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get();
/* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0;
int sec = 0;
if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) {
course = global_pos->yaw / M_PI_F * 180.0f;
if (gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000) {
course = gpos.yaw / M_PI_F * 180.0f;
if (course < 0.f) { // course is in range [0, 360], 0=north, CW
course += 360.f;
}
lat = frsky_format_gps(fabsf(global_pos->lat));
lat_ns = (global_pos->lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(global_pos->lon));
lon_ew = (global_pos->lon < 0) ? 'W' : 'E';
speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e)
lat = frsky_format_gps(fabsf(gpos.lat));
lat_ns = (gpos.lat < 0) ? 'S' : 'N';
lon = frsky_format_gps(fabsf(gpos.lon));
lon_ew = (gpos.lon < 0) ? 'W' : 'E';
speed = sqrtf(gpos.vel_n * gpos.vel_n + gpos.vel_e * gpos.vel_e)
* 25.0f / 46.0f;
alt = global_pos->alt;
alt = gpos.alt;
}
if (gps->timestamp != 0 && hrt_absolute_time() < gps->timestamp + 20000) {
time_t time_gps = gps->time_utc_usec / 1000000ULL;
if (gps.timestamp != 0 && hrt_absolute_time() < gps.timestamp + 20000) {
time_t time_gps = gps.time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
sec = tm_gps->tm_sec;
@@ -302,8 +247,7 @@ void frsky_send_frame2(int uart)
frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt);
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(battery_status->remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_FUEL, roundf(battery_status.remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
@@ -317,7 +261,8 @@ void frsky_send_frame2(int uart)
void frsky_send_frame3(int uart)
{
/* send formatted frame */
time_t time_gps = subscription_data->vehicle_gps_position.time_utc_usec / 1000000ULL;
time_t time_gps = subscription_data->vehicle_gps_position_sub.get().time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff);
frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);
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);
}