fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-18 12:13:35 +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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+5 -1
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); z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */ /* 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; update_vect[2] = 1;
// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); // sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
sensor_last_timestamp[2] = raw.magnetometer_timestamp; sensor_last_timestamp[2] = raw.magnetometer_timestamp;