diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index c6de84084e..ffd0e32205 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -67,6 +67,8 @@ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; static int frsky_task; +static struct sensor_combined_s sensor_raw; +static float filtered_alt = NAN; /* functions */ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); @@ -246,17 +248,18 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) * in order to apply a lowpass filter to baro pressure. */ static float last_baro_alt = 0; - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + bool sensor_updated; + orb_check(sensor_sub, &sensor_updated); - static float filtered_alt = NAN; + if (sensor_updated) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_raw); - if (isnan(filtered_alt)) { - filtered_alt = raw.baro_alt_meter[0]; + if (isnan(filtered_alt)) { + filtered_alt = sensor_raw.baro_alt_meter[0]; - } else { - filtered_alt = .05f * raw.baro_alt_meter[0] + (1.0f - .05f) * filtered_alt; + } else { + filtered_alt = .05f * sensor_raw.baro_alt_meter[0] + .95f * filtered_alt; + } } // allow a minimum of 500usec before reply diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c index a4f2a615c6..465b05477b 100644 --- a/src/drivers/frsky_telemetry/sPort_data.c +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -147,7 +147,6 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) void sPort_send_BATV(int uart) { /* get a local copy of the vehicle status data */ - memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send battery voltage as VFAS */ @@ -158,10 +157,6 @@ void sPort_send_BATV(int uart) // verified scaling void sPort_send_CUR(int uart) { - /* get a local copy of the vehicle status data */ - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - /* send data */ uint32_t current = (int)(10 * vehicle_status.battery_current); sPort_send_data(uart, SMARTPORT_ID_CURR, current); @@ -173,7 +168,6 @@ void sPort_send_CUR(int uart) void sPort_send_ALT(int uart) { /* get a local copy of the current sensor values */ - memset(&sensor_data, 0, sizeof(sensor_data)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_data); /* send data */ @@ -184,10 +178,6 @@ void sPort_send_ALT(int uart) // verified scaling for "calculated" option void sPort_send_SPD(int uart) { - /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* send data */ float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint32_t ispeed = (int)(10 * speed); @@ -205,10 +195,6 @@ void sPort_send_VSPD(int uart, float speed) // verified scaling void sPort_send_FUEL(int uart) { - /* get a local copy of the vehicle status data */ - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - /* send data */ uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); @@ -218,7 +204,6 @@ void sPort_send_GPS_LON(int uart) { /* send longitude */ /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ @@ -234,9 +219,6 @@ void sPort_send_GPS_LON(int uart) void sPort_send_GPS_LAT(int uart) { /* send latitude */ - /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */ uint32_t iLat = 6E5 * fabs(global_pos.lat); @@ -249,9 +231,6 @@ void sPort_send_GPS_LAT(int uart) void sPort_send_GPS_ALT(int uart) { /* send altitude */ - /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* convert to 100 * m/sec */ uint32_t iAlt = 100 * global_pos.alt; @@ -261,9 +240,6 @@ void sPort_send_GPS_ALT(int uart) void sPort_send_GPS_COG(int uart) { /* send course */ - /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ uint32_t iYaw = 100 * global_pos.yaw; @@ -272,10 +248,6 @@ void sPort_send_GPS_COG(int uart) void sPort_send_GPS_SPD(int uart) { - /* get a local copy of the global position data */ - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* send 100 * knots */ float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint32_t ispeed = (int)(1944 * speed);