Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 10:26:52 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

attitude_estimator_q: Max gyro bias limiting, auto declination

Browse Source
This commit is contained in:
Anton Babushkin
2015-04-13 19:32:23 +02:00
parent cc99e6dc1f
commit 4ce7e2acc1
2 changed files with 19 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+18 -2
View File
@@ -58,7 +58,6 @@
#include <uORB/topics/sensor_combined.h> #include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
@@ -122,18 +121,22 @@ private:
param_t w_mag; param_t w_mag;
param_t w_gyro_bias; param_t w_gyro_bias;
param_t mag_decl; param_t mag_decl;
param_t mag_decl_auto;
param_t acc_comp; param_t acc_comp;
param_t bias_max;
} _params_handles; /**< handles for interesting parameters */ } _params_handles; /**< handles for interesting parameters */
float _w_accel = 0.0f; float _w_accel = 0.0f;
float _w_mag = 0.0f; float _w_mag = 0.0f;
float _w_gyro_bias = 0.0f; float _w_gyro_bias = 0.0f;
float _mag_decl = 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> _gyro;
Vector<3> _accel; Vector<3> _accel;
Vector<3> _mag; Vector<3> _mag;
bool _acc_comp = false;
Quaternion _q; Quaternion _q;
Vector<3> _rates; Vector<3> _rates;
@@ -164,7 +167,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() {
_params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_mag = param_find("ATT_W_MAG");
_params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS");
_params_handles.mag_decl = param_find("ATT_MAG_DECL"); _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.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); orb_check(_global_pos_sub, &gpos_updated);
if (gpos_updated) { if (gpos_updated) {
orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); 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) { 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; float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg); param_get(_params_handles.mag_decl, &mag_decl_deg);
_mag_decl = math::radians(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; int32_t acc_comp_int;
param_get(_params_handles.acc_comp, &acc_comp_int); param_get(_params_handles.acc_comp, &acc_comp_int);
_acc_comp = acc_comp_int != 0; _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 estimation
_gyro_bias += corr * (_w_gyro_bias * dt); _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; _rates = _gyro + _gyro_bias;
// Feed forward gyro // Feed forward gyro
src/modules/attitude_estimator_q/attitude_estimator_q_params.c
+1
View File
@@ -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_MAG, 0.1f);
PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 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_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_INT32(ATT_ACC_COMP, 2); ///< acceleration compensation
PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); ///< gyro bias limit, rad/s