From 5b372dae061981094f9872181c5dfd35f2dcc8cf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Jul 2021 13:40:57 +1000 Subject: [PATCH] AP_AHRS: stop storing gyro drift rate as variable This is constant and is just folded into whereever it is used. --- libraries/AP_AHRS/AP_AHRS_Backend.h | 8 -------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 2 +- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index c50d2dceaa..f358c3640a 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -54,10 +54,6 @@ public: _cos_pitch(1.0f), _cos_yaw(1.0f) { - // base the ki values by the sensors maximum drift - // rate. - _gyro_drift_limit = AP::ins().get_gyro_drift_rate(); - // enable centrifugal correction by default _flags.correct_centrifugal = true; @@ -665,10 +661,6 @@ protected: Matrix3f _rotation_autopilot_body_to_vehicle_body; Matrix3f _rotation_vehicle_body_to_autopilot_body; - // the limit of the gyro drift claimed by the sensors, in - // radians/s/s - float _gyro_drift_limit; - // accelerometer values in the earth frame in m/s/s Vector3f _accel_ef[INS_MAX_INSTANCES]; Vector3f _accel_ef_blended; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index e1c1ff10e9..658a8e2899 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -930,7 +930,7 @@ AP_AHRS_DCM::drift_correction(float deltat) // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device - const float change_limit = _gyro_drift_limit * _omega_I_sum_time; + const float change_limit = AP::ins().get_gyro_drift_rate() * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);