From 6739ae9dfc96e4e8bc3857d191722bb7b1ce7e8d Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 21 Jun 2016 14:12:05 +0200 Subject: [PATCH] ekf2: substract gyro bias from control state rates Signed-off-by: tumbili --- src/modules/ekf2/ekf2_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 8ab9faaac8..864505c7fe 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -609,10 +609,12 @@ void Ekf2::task_main() // generate control state data control_state_s ctrl_state = {}; + float gyro_bias[3] = {}; + _ekf.get_gyro_bias(gyro_bias); ctrl_state.timestamp = hrt_absolute_time(); - ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); + ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]) - gyro_bias[0]; + ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]) - gyro_bias[1]; + ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2]; // Velocity in body frame float velocity[3];