diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 5e67d41969..28e04032ec 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,6 +53,8 @@ #include #include #include +#include +#include #include @@ -91,38 +93,52 @@ #define frac(f) (f - (int)f) -static struct battery_status_s *battery_status; -static struct vehicle_global_position_s *global_pos; -static struct sensor_combined_s *sensor_combined; +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_gps_position_s vehicle_gps_position; + uint8_t current_flight_mode; // == vehicle_status.nav_state -static int battery_status_sub = -1; -static int vehicle_global_position_sub = -1; -static int sensor_sub = -1; + int battery_status_sub; + int vehicle_global_position_sub; + int sensor_sub; + int vehicle_gps_position_sub; + int vehicle_status_sub; +}; + +static struct frsky_subscription_data_s *subscription_data = NULL; /** * Initializes the uORB subscriptions. */ bool frsky_init() { - battery_status = malloc(sizeof(struct battery_status_s)); - global_pos = malloc(sizeof(struct vehicle_global_position_s)); - sensor_combined = malloc(sizeof(struct sensor_combined_s)); + subscription_data = (struct frsky_subscription_data_s *)calloc(1, sizeof(struct frsky_subscription_data_s)); - if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) { + if (!subscription_data) { return false; } - battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + 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() { - free(battery_status); - free(global_pos); - free(sensor_combined); + 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; + } } /** @@ -174,26 +190,41 @@ 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(sensor_sub, &updated); + orb_check(subs->sensor_sub, &updated); if (updated) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_combined); + orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &subs->sensor_combined); + } + + 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); + } + + 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(battery_status_sub, &updated); + orb_check(subs->battery_status_sub, &updated); if (updated) { - orb_copy(ORB_ID(battery_status), battery_status_sub, battery_status); + orb_copy(ORB_ID(battery_status), subs->battery_status_sub, &subs->battery_status); } /* get a local copy of the global position data */ - orb_check(vehicle_global_position_sub, &updated); + orb_check(subs->vehicle_global_position_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, global_pos); + orb_copy(ORB_ID(vehicle_global_position), subs->vehicle_global_position_sub, &subs->global_pos); } } @@ -203,23 +234,22 @@ 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(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_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)); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, - sensor_combined->baro_alt_meter); + subs->sensor_combined.baro_alt_meter); frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(sensor_combined->baro_alt_meter) * 100.0f)); - - frsky_send_data(uart, FRSKY_ID_TEMP1, - roundf(sensor_combined->baro_temp_celcius)); + roundf(frac(subs->sensor_combined.baro_alt_meter) * 100.0f)); frsky_send_data(uart, FRSKY_ID_VFAS, - roundf(battery_status->voltage_v * 10.0f)); + roundf(subs->battery_status.voltage_v * 10.0f)); frsky_send_data(uart, FRSKY_ID_CURRENT, - (battery_status->current_a < 0) ? 0 : roundf(battery_status->current_a * 10.0f)); + (subs->battery_status.current_a < 0) ? 0 : roundf(subs->battery_status.current_a * 10.0f)); frsky_send_startstop(uart); } @@ -239,6 +269,8 @@ 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; /* send formatted frame */ float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; @@ -296,7 +328,7 @@ void frsky_send_frame2(int uart) void frsky_send_frame3(int uart) { /* send formatted frame */ - time_t time_gps = global_pos->time_utc_usec / 1000000ULL; + time_t time_gps = subscription_data->global_pos.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);