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 */ +#include #include #include #include @@ -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