diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index c8232eee43..3492eaef0c 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -634,12 +634,14 @@ void AttitudeEstimatorQ::task_main() // use estimated velocity for airspeed estimate if (_airspeed_mode == 1) { if (hrt_absolute_time() - _gpos.timestamp < 1e6) { - ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e +_gpos.vel_d * _gpos.vel_d); + ctrl_state.airspeed = sqrtf(_gpos.vel_n * _gpos.vel_n + _gpos.vel_e * _gpos.vel_e + _gpos.vel_d * _gpos.vel_d); } - // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed + + // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed + } else if (_airspeed_mode == 2) { - // use the measured airspeed + // use the measured airspeed } else { /* 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 diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ca94eaa7ce..51f15cfc46 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -667,12 +667,14 @@ void Ekf2::task_main() // use estimated velocity for airspeed estimate if (_airspeed_mode.get() == 1) { if (_ekf.local_position_is_valid()) { - ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] +vel[2] * vel[2]); + ctrl_state.airspeed = sqrtf(vel[0] * vel[0] + vel[1] * vel[1] + vel[2] * vel[2]); } - // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed + + // do nothing, airspeed has been declared as non-valid above, controllers will handle this assuming always trim airspeed + } else if (_airspeed_mode.get() == 2) { - // use the measured airspeed + // use the measured airspeed } else { /* 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