fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 07:33:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

reduce D-type stack usage and static allocations

Browse Source
This commit is contained in:
Mark Whitehorn
2016-03-01 09:02:14 +01:00
committed by Lorenz Meier
parent 6d2927cd7a
commit 8186139dad
4 changed files with 66 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/frsky_data.c
+50 -37
View File
@@ -92,20 +92,43 @@
#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_data;
static struct vehicle_status_s *vehicle_status;
static int battery_sub = -1;
static int sensor_sub = -1;
static int global_position_sub = -1;
static int sensor_sub = -1;
static int vehicle_status_sub = -1;
/**
* Initializes the uORB subscriptions.
*/
void frsky_init()
bool frsky_init()
{
battery_status = malloc(sizeof(struct battery_status_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
sensor_data = malloc(sizeof(struct sensor_combined_s));
vehicle_status = malloc(sizeof(struct vehicle_status_s));
if (battery_status == NULL || global_pos == NULL || sensor_data == NULL || vehicle_status == NULL) {
return false;
}
battery_sub = orb_subscribe(ORB_ID(battery_status));
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
return true;
}
void frsky_deinit()
{
free(battery_status);
free(global_pos);
free(sensor_data);
free(vehicle_status);
}
/**
@@ -162,35 +185,31 @@ static void frsky_send_data(int uart, uint8_t id, int16_t data)
void frsky_send_frame1(int uart)
{
/* get a local copy of the current sensor values */
struct sensor_combined_s raw;
memset(&raw, 0, sizeof(raw));
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID(battery_status), battery_sub, battery_status);
/* send formatted frame */
frsky_send_data(uart, FRSKY_ID_ACCEL_X,
roundf(raw.accelerometer_m_s2[0] * 1000.0f));
roundf(sensor_data->accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y,
roundf(raw.accelerometer_m_s2[1] * 1000.0f));
roundf(sensor_data->accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z,
roundf(raw.accelerometer_m_s2[2] * 1000.0f));
roundf(sensor_data->accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
raw.baro_alt_meter[0]);
sensor_data->baro_alt_meter[0]);
frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP,
roundf(frac(raw.baro_alt_meter[0]) * 100.0f));
roundf(frac(sensor_data->baro_alt_meter[0]) * 100.0f));
frsky_send_data(uart, FRSKY_ID_TEMP1,
roundf(raw.baro_temp_celcius[0]));
roundf(sensor_data->baro_temp_celcius[0]));
frsky_send_data(uart, FRSKY_ID_VFAS,
roundf(battery.voltage_v * 10.0f));
roundf(battery_status->voltage_v * 10.0f));
frsky_send_data(uart, FRSKY_ID_CURRENT,
(battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f));
(battery_status->current_a < 0) ? 0 : roundf(battery_status->current_a * 10.0f));
frsky_send_startstop(uart);
}
@@ -211,32 +230,28 @@ static float frsky_format_gps(float dec)
void frsky_send_frame2(int uart)
{
/* get a local copy of the global position data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
/* get a local copy of the vehicle status data */
struct vehicle_status_s vehicle_status;
memset(&vehicle_status, 0, sizeof(vehicle_status));
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
/* 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) {
time_t time_gps = global_pos.time_utc_usec / 1000000ULL;
if (global_pos->timestamp != 0 && hrt_absolute_time() < global_pos->timestamp + 20000) {
time_t time_gps = global_pos->time_utc_usec / 1000000ULL;
struct tm *tm_gps = gmtime(&time_gps);
course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
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)
course = (global_pos->yaw + M_PI_F) / M_PI_F * 180.0f;
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)
* 25.0f / 46.0f;
alt = global_pos.alt;
alt = global_pos->alt;
sec = tm_gps->tm_sec;
}
@@ -258,7 +273,7 @@ void frsky_send_frame2(int uart)
frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f);
frsky_send_data(uart, FRSKY_ID_FUEL,
roundf(vehicle_status.battery_remaining * 100.0f));
roundf(vehicle_status->battery_remaining * 100.0f));
frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec);
@@ -272,12 +287,10 @@ void frsky_send_frame2(int uart)
void frsky_send_frame3(int uart)
{
/* get a local copy of the battery data */
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
/* send formatted frame */
time_t time_gps = global_pos.time_utc_usec / 1000000ULL;
time_t time_gps = 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);
@@ -337,7 +350,7 @@ bool frsky_parse_host(uint8_t *sbuf, int nbytes, struct adc_linkquality *v)
state = HEADER;
if (sbuf[i] != 0x7E) {
warnx("host packet error: %x", sbuf[i]);
// warnx("host packet error: %x", sbuf[i]);
} else {
data_ready = true;
src/drivers/frsky_telemetry/frsky_data.h
+2 -1
View File
@@ -45,7 +45,8 @@
#include <stdbool.h>
// Public functions
void frsky_init(void);
bool frsky_init(void);
void frsky_deinit(void);
void frsky_send_frame1(int uart);
void frsky_send_frame2(int uart);
void frsky_send_frame3(int uart);
src/drivers/frsky_telemetry/frsky_telemetry.c
+13 -8
View File
@@ -401,8 +401,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* either no traffic on the port (0=>timeout), or D type packet */
} else if (status > 0) {
warnx("sending FrSky D-type telemetry");
/* detected D type telemetry: reconfigure UART */
warnx("sending FrSky D type telemetry");
status = set_uart_speed(uart, &uart_config, B9600);
if (status < 0) {
@@ -418,11 +418,13 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
int iteration = 0;
/* Subscribe to topics */
frsky_init();
if (!frsky_init()) {
err(1, "could not allocate memory");
}
warnx("sending FrSky D type telemetry");
struct adc_linkquality host_frame;
uint8_t dbuf[45];
// uint8_t dbuf[45];
/* send D8 mode telemetry */
while (!thread_should_exit) {
@@ -431,8 +433,8 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
usleep(100000);
/* parse incoming data */
int nbytes = read(uart, &dbuf[0], sizeof(dbuf));
bool new_input = frsky_parse_host(&dbuf[0], nbytes, &host_frame);
int nbytes = read(uart, &sbuf[0], sizeof(sbuf));
bool new_input = frsky_parse_host((uint8_t *)&sbuf[0], nbytes, &host_frame);
/* the RSSI value could be useful */
if (false && new_input) {
@@ -460,8 +462,11 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
iteration++;
}
/* TODO: flush the input buffer if in full duplex mode */
read(uart, &sbuf[0], sizeof(sbuf));
// /* TODO: flush the input buffer if in full duplex mode */
// read(uart, &sbuf[0], sizeof(sbuf));
warnx("freeing frsky memory");
frsky_deinit();
} else {
warnx("FrSky receiver not detected, exiting");
src/drivers/frsky_telemetry/sPort_data.c
+1 -1
View File
@@ -72,7 +72,7 @@ bool sPort_init()
vehicle_status = malloc(sizeof(struct vehicle_status_s));
sensor_data = malloc(sizeof(struct sensor_combined_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
if (vehicle_status == NULL || vehicle_status == NULL || vehicle_status == NULL) {
if (vehicle_status == NULL || sensor_data == NULL || global_pos == NULL) {
return false;
}
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));