diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-18 20:49:18 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-18 20:49:18 +0200 |
commit | 897827832029c3d2c9775ec0e7de919a8c1b62c2 (patch) | |
tree | 7bd33228a5166db1562baca0c0f4a8f57c939728 /src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | |
parent | 3658bf83ed92522f26bce5a4496ca3ac68c577fc (diff) | |
download | px4-firmware-mag_robustness.tar.gz px4-firmware-mag_robustness.tar.bz2 px4-firmware-mag_robustness.zip |
EKF combined att + pos estimator: Robustify against mag 0 vectors and timeoutsmag_robustness
Diffstat (limited to 'src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp')
-rw-r--r-- | src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 29 |
1 files changed, 20 insertions, 9 deletions
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 4e1573d99..e0b4cb2a0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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) { |