From 333d6ea12f59064d3a45dfc235d76609019a33f2 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 14:58:29 +0100 Subject: [PATCH] ekf: use airspeed directly, the other thing was just wrong --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 49f08f3cab..44c7b3e434 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -894,8 +894,10 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[3] = _ekf->states[3]; /* Airspeed (Groundspeed - Windspeed) */ - _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); - + //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL + // and in outdoor tests + _ctrl_state.airspeed = _airspeed.true_airspeed_m_s; /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;