From 2f48317a9ea7b05d8c61b3d1c44f8229491350b1 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 31 Mar 2016 14:03:42 +0200 Subject: [PATCH] ekf2: publish indicated airspeed in control state topic --- src/modules/ekf2/ekf2_main.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 5c1dc10322..d704aafd4e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -486,6 +486,16 @@ void Ekf2::task_main() ctrl_state.q[2] = q(2); ctrl_state.q[3] = q(3); + // Airspeed - take airspeed measurement directly here as no wind is estimated + if (PX4_ISFINITE(airspeed.indicated_airspeed_m_s) && hrt_absolute_time() - airspeed.timestamp < 1e6 + && airspeed.timestamp > 0) { + ctrl_state.airspeed = airspeed.indicated_airspeed_m_s; + ctrl_state.airspeed_valid = true; + + } else { + ctrl_state.airspeed_valid = false; + } + // publish control state data if (_control_state_pub == nullptr) { _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);