fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf_att_pos_estimator: Publish control state gyro bias

Browse Source
This commit is contained in:
Paul Riseborough 2017-01-14 05:08:48 +01:00 committed by Lorenz Meier
parent f8cef1e9ab
commit 6a78df7fa0
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -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) ||