From 54fac26327a63e5d0721250bd503cf3e98beed2e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Mon, 23 Apr 2018 16:20:18 +0200
Subject: [PATCH] attitude_estimator_q: allow it to be used on boards w/o a mag

By setting ATT_W_MAG to 0
---
 .../attitude_estimator_q/attitude_estimator_q_main.cpp    | 8 ++++++++
 .../attitude_estimator_q/attitude_estimator_q_params.c    | 2 ++
 2 files changed, 10 insertions(+)

diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
index ce2fdb1de3..17443f94d1 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
@@ -39,6 +39,7 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
+#include <cfloat>
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
 #include <lib/ecl/geo_lookup/geo_mag_declination.h>
@@ -473,6 +474,13 @@ void AttitudeEstimatorQ::update_parameters(bool force)
 
 		param_get(_params_handles.w_acc, &_w_accel);
 		param_get(_params_handles.w_mag, &_w_mag);
+
+		if (_w_mag < FLT_EPSILON) { // if the weight is zero (=mag disabled), make sure the estimator initializes
+			_mag(0) = 1.f;
+			_mag(1) = 0.f;
+			_mag(2) = 0.f;
+		}
+
 		param_get(_params_handles.w_ext_hdg, &_w_ext_hdg);
 		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
 
diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
index ef1e9e7d99..bb2ab35561 100644
--- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c
@@ -54,6 +54,8 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
 /**
  * Complimentary filter magnetometer weight
  *
+ * Set to 0 to avoid using the magnetometer.
+ *
  * @group Attitude Q estimator
  * @min 0
  * @max 1