From 34b03d56593815e16a29ba5dcef9b66208285c98 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 28 Sep 2019 15:46:11 -0400 Subject: [PATCH] frsky_telemetry: move to uORB::Subscription --- .../telemetry/frsky_telemetry/frsky_data.cpp | 149 ++++++----------- .../telemetry/frsky_telemetry/sPort_data.cpp | 156 +++++------------- 2 files changed, 90 insertions(+), 215 deletions(-) diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp index 85ab413b41..6659b8b2f3 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp @@ -51,6 +51,7 @@ #include #include +#include #include #include #include @@ -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_sub{ORB_ID(battery_status)}; + uORB::SubscriptionData sensor_combined_sub{ORB_ID(sensor_combined)}; + uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData 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); diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp index ae5e5f0c40..a0247991b8 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp @@ -51,6 +51,7 @@ #include +#include #include #include #include @@ -59,30 +60,21 @@ #include #include - #include #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_sub{ORB_ID(battery_status)}; + uORB::SubscriptionData sensor_combined_sub{ORB_ID(sensor_combined)}; + uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::SubscriptionData 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); }