From 27e6f07485b5923993e7ab3a53d6fb4d1bac572e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 4 Jan 2017 14:47:45 +0100 Subject: [PATCH] ekf2: publish control state gyro bias data --- src/modules/ekf2/ekf2_main.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2cd52e4d04..febd87ed46 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -711,6 +711,9 @@ void Ekf2::task_main() ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]); ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]); + ctrl_state.roll_rate_bias = gyro_bias[0]; + ctrl_state.pitch_rate_bias = gyro_bias[1]; + ctrl_state.yaw_rate_bias = gyro_bias[2]; // Velocity in body frame Vector3f v_n(velocity);