From 4ce7e2acc149dd5d64a3fb0a37a940eaa61cdae1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 13 Apr 2015 19:32:23 +0200 Subject: attitude_estimator_q: Max gyro bias limiting, auto declination --- .../attitude_estimator_q_main.cpp | 20 ++++++++++++++++++-- .../attitude_estimator_q_params.c | 1 + 2 files changed, 19 insertions(+), 2 deletions(-) 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 7b5c8b7dc..a1c56aa1f 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -122,18 +121,22 @@ private: param_t w_mag; param_t w_gyro_bias; param_t mag_decl; + param_t mag_decl_auto; param_t acc_comp; + param_t bias_max; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; float _w_mag = 0.0f; float _w_gyro_bias = 0.0f; float _mag_decl = 0.0f; + bool _mag_decl_auto = false; + bool _acc_comp = false; + float _bias_max = 0.0f; Vector<3> _gyro; Vector<3> _accel; Vector<3> _mag; - bool _acc_comp = false; Quaternion _q; Vector<3> _rates; @@ -164,7 +167,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() { _params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.mag_decl = param_find("ATT_MAG_DECL"); + _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); + _params_handles.bias_max = param_find("ATT_BIAS_MAX"); } /** @@ -257,6 +262,10 @@ void AttitudeEstimatorQ::task_main() { orb_check(_global_pos_sub, &gpos_updated); if (gpos_updated) { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); + if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) { + /* set magnetic declination automatically */ + _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon)); + } } if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) { @@ -341,9 +350,13 @@ void AttitudeEstimatorQ::update_parameters(bool force) { float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); _mag_decl = math::radians(mag_decl_deg); + int32_t mag_decl_auto_int; + param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int); + _mag_decl_auto = mag_decl_auto_int != 0; int32_t acc_comp_int; param_get(_params_handles.acc_comp, &acc_comp_int); _acc_comp = acc_comp_int != 0; + param_get(_params_handles.bias_max, &_bias_max); } } @@ -400,6 +413,9 @@ void AttitudeEstimatorQ::update(float dt) { // Gyro bias estimation _gyro_bias += corr * (_w_gyro_bias * dt); + for (int i = 0; i < 3; i++) { + _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max); + } _rates = _gyro + _gyro_bias; // Feed forward gyro 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 35ffe538a..fa1592307 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -45,5 +45,6 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); ///< magnetic declination, in degrees +PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); ///< automatic GPS based magnetic declination PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); ///< acceleration compensation PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s -- cgit v1.2.3