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

frsky_telemetry D protocol: refactor to use less memory & allocations

Browse Source 
Also add the vehicle_gps_position & flight mode information
This commit is contained in:
Beat Küng
2017-08-08 14:47:01 +02:00
parent 9dea515eaa
commit a2bfcb94ef
1 changed files with 65 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/frsky_data.c
+65 -33
View File
@@ -53,6 +53,8 @@
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@@ -91,38 +93,52 @@
#define frac(f) (f - (int)f) #define frac(f) (f - (int)f)
static struct battery_status_s *battery_status; struct frsky_subscription_data_s {
static struct vehicle_global_position_s *global_pos; struct battery_status_s battery_status;
static struct sensor_combined_s *sensor_combined; 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; int battery_status_sub;
static int vehicle_global_position_sub = -1; int vehicle_global_position_sub;
static int sensor_sub = -1; 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. * Initializes the uORB subscriptions.
*/ */
bool frsky_init() bool frsky_init()
{ {
battery_status = malloc(sizeof(struct battery_status_s)); subscription_data = (struct frsky_subscription_data_s *)calloc(1, sizeof(struct frsky_subscription_data_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
sensor_combined = malloc(sizeof(struct sensor_combined_s));
if (battery_status == NULL || global_pos == NULL || sensor_combined == NULL) { if (!subscription_data) {
return false; return false;
} }
battery_status_sub = orb_subscribe(ORB_ID(battery_status)); subscription_data->battery_status_sub = orb_subscribe(ORB_ID(battery_status));
vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subscription_data->vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); 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; return true;
} }
void frsky_deinit() void frsky_deinit()
{ {
free(battery_status); if (subscription_data) {
free(global_pos); orb_unsubscribe(subscription_data->battery_status_sub);
free(sensor_combined); 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() void frsky_update_topics()
{ {
struct frsky_subscription_data_s *subs = subscription_data;
bool updated; bool updated;
/* get a local copy of the current sensor values */ /* get a local copy of the current sensor values */
orb_check(sensor_sub, &updated); orb_check(subs->sensor_sub, &updated);
if (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 */ /* get a local copy of the battery data */
orb_check(battery_status_sub, &updated); orb_check(subs->battery_status_sub, &updated);
if (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 */ /* 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) { 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) void frsky_send_frame1(int uart)
{ {
struct frsky_subscription_data_s *subs = subscription_data;
/* send formatted frame */ /* 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_X, roundf(subs->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_Y, roundf(subs->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_Z, roundf(subs->sensor_combined.accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, 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, frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(sensor_combined->baro_alt_meter) * 100.0f)); roundf(frac(subs->sensor_combined.baro_alt_meter) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(sensor_combined->baro_temp_celcius));
frsky_send_data(uart, FRSKY_ID_VFAS, 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, 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); frsky_send_startstop(uart);
} }
@@ -239,6 +269,8 @@ static float frsky_format_gps(float dec)
*/ */
void frsky_send_frame2(int uart) 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 */ /* send formatted frame */
float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
char lat_ns = 0, lon_ew = 0; char lat_ns = 0, lon_ew = 0;
@@ -296,7 +328,7 @@ void frsky_send_frame2(int uart)
void frsky_send_frame3(int uart) void frsky_send_frame3(int uart)
{ {
/* send formatted frame */ /* 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); struct tm *tm_gps = gmtime(&time_gps);
uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); 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); frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday);