fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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-07-31 15:35:22 +02:00
parent 9dea515eaa
commit a2bfcb94ef
1 changed files with 65 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -53,6 +53,8 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/sensor_combined.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>
@ -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);