fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 11:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: substract gyro bias from control state rates

Browse Source 
Signed-off-by: tumbili <roman@px4.io>
This commit is contained in:
tumbili
2016-06-22 07:10:20 +02:00
committed by Roman
parent c84870046e
commit 6739ae9dfc
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+5 -3
View File
@@ -609,10 +609,12 @@ void Ekf2::task_main()
// generate control state data // generate control state data
control_state_s ctrl_state = {}; control_state_s ctrl_state = {};
float gyro_bias[3] = {};
_ekf.get_gyro_bias(gyro_bias);
ctrl_state.timestamp = hrt_absolute_time(); ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); 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]); 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]); ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]) - gyro_bias[2];
// Velocity in body frame // Velocity in body frame
float velocity[3]; float velocity[3];