diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-18 20:34:44 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-04-18 20:34:44 +0200 |
| commit | 3658bf83ed92522f26bce5a4496ca3ac68c577fc (patch) | |
| tree | 383601564cc7f3517564ab124bde5f20842c50e3 | |
| parent | 1c8e79cbf18cd2c41024ec1d91fb8a16b1900c9f (diff) | |
| download | px4-firmware-3658bf83ed92522f26bce5a4496ca3ac68c577fc.tar.gz px4-firmware-3658bf83ed92522f26bce5a4496ca3ac68c577fc.tar.bz2 px4-firmware-3658bf83ed92522f26bce5a4496ca3ac68c577fc.zip | |
EKF att-only estimator: Do not fuse zero-length mag vector.
| -rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 6 |
1 files changed, 5 insertions, 1 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 9bb9393c5..b94a7a079 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -447,7 +447,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds z_k[5] = raw.accelerometer_m_s2[2] - acc(2); /* update magnetometer measurements */ - if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp && + /* check that the mag vector is > 0 */ + fabsf(sqrtf(raw.magnetometer_ga[0] * raw.magnetometer_ga[0] + + raw.magnetometer_ga[1] * raw.magnetometer_ga[1] + + raw.magnetometer_ga[2] * raw.magnetometer_ga[2])) > 0.1f) { update_vect[2] = 1; // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; |