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

EKF combined att + pos estimator: Robustify against mag 0 vectors and timeouts

Browse Source
This commit is contained in:
Lorenz Meier
2015-04-18 20:49:18 +02:00
parent 3658bf83ed
commit 8978278320
1 changed files with 20 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+20 -9
View File
@@ -1422,30 +1422,41 @@ void AttitudePositionEstimatorEKF::pollData()
int last_mag_main = _mag_main;
// XXX we compensate the offsets upfront - should be close to zero.
Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
_sensor_combined.magnetometer1_ga[2]);
const unsigned mag_timeout_us = 200000;
/* fail over to the 2nd mag if we know the first is down */
if (_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount) {
_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
_ekf->magData.y = mag0.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
_ekf->magData.z = mag0.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 0;
} else {
_ekf->magData.x = _sensor_combined.magnetometer1_ga[0];
} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
mag1.length() > 0.1f) {
_ekf->magData.x = mag1.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = _sensor_combined.magnetometer1_ga[1];
_ekf->magData.y = mag1.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = _sensor_combined.magnetometer1_ga[2];
_ekf->magData.z = mag1.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 1;
} else {
_mag_valid = false;
}
if (last_mag_main != _mag_main) {