From 6a78df7fa059c1630104a475b2bdca6eb1401ea4 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 14 Jan 2017 05:08:48 +0100 Subject: [PATCH] ekf_att_pos_estimator: Publish control state gyro bias --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7cff56de42..115db064e6 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -909,6 +909,11 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; _ctrl_state.yaw_rate = _LP_att_R.apply(_ekf->dAngIMU.z / _ekf->dtIMU) - _ekf->states[12] / _ekf->dtIMUfilt; + /* Gyro bias estimates */ + _ctrl_state.roll_rate_bias = _ekf->states[10] / _ekf->dtIMUfilt; + _ctrl_state.pitch_rate_bias = _ekf->states[11] / _ekf->dtIMUfilt; + _ctrl_state.yaw_rate_bias = _ekf->states[12] / _ekf->dtIMUfilt; + /* Guard from bad data */ if (!PX4_ISFINITE(_ctrl_state.x_vel) || !PX4_ISFINITE(_ctrl_state.y_vel) ||