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

ekf2: repeat IMU bias reset until successful

Browse Source
This commit is contained in:
Paul Riseborough
2017-10-27 15:45:53 +11:00
committed by Lorenz Meier
parent 9fde19259e
commit 0c9e07c95a
1 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+9 -2
View File
@@ -457,6 +457,8 @@ void Ekf2::run()
int sensor_selection_sub = orb_subscribe(ORB_ID(sensor_selection));
int sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
bool imu_bias_reset_request = false;
// because we can have several distance sensor instances with different orientations
int range_finder_subs[ORB_MULTI_MAX_INSTANCES];
int range_finder_sub_index = -1; // index for downward-facing range finder subscription
@@ -566,16 +568,21 @@ void Ekf2::run()
if ((sensor_selection_prev.timestamp > 0) && (sensor_selection.timestamp > sensor_selection_prev.timestamp)) {
if (sensor_selection.accel_device_id != sensor_selection_prev.accel_device_id) {
PX4_WARN("accel id changed, resetting IMU bias");
_ekf.reset_imu_bias();
imu_bias_reset_request = true;
}
if (sensor_selection.gyro_device_id != sensor_selection_prev.gyro_device_id) {
PX4_WARN("gyro id changed, resetting IMU bias");
_ekf.reset_imu_bias();
imu_bias_reset_request = true;
}
}
}
// attempt reset until successful
if (imu_bias_reset_request) {
imu_bias_reset_request = !_ekf.reset_imu_bias();
}
orb_check(optical_flow_sub, &optical_flow_updated);
if (optical_flow_updated) {