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

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;