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

EKF att-only estimator: Do not fuse zero-length mag vector.

Browse Source
This commit is contained in:
Lorenz Meier 2015-04-18 20:34:44 +02:00
parent 1c8e79cbf1
commit 3658bf83ed
1 changed files with 5 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -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;