From 8186139dad11611a4c5443cc00424013903babb6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 27 Feb 2016 09:12:05 -0700 Subject: [PATCH] reduce D-type stack usage and static allocations --- src/drivers/frsky_telemetry/frsky_data.c | 87 +++++++++++-------- src/drivers/frsky_telemetry/frsky_data.h | 3 +- src/drivers/frsky_telemetry/frsky_telemetry.c | 21 +++-- src/drivers/frsky_telemetry/sPort_data.c | 2 +- 4 files changed, 66 insertions(+), 47 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index ef5beeeb44..70b2c2ab43 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -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; diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index 96948e0cff..b3c41d0aa4 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -45,7 +45,8 @@ #include // 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); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 606431a5a8..c95a9c839d 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -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"); diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index 6ae0c0a105..0017c7052b 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -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));