diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 45803fc1f0..f1cfb3d571 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -38,10 +38,7 @@ class AP_AHRS_Backend public: // Constructor - AP_AHRS_Backend() { - // enable centrifugal correction by default - _flags.correct_centrifugal = true; - } + AP_AHRS_Backend() {} // empty virtual destructor virtual ~AP_AHRS_Backend() {} @@ -281,17 +278,6 @@ public: return _flags.have_initial_yaw; } - // set the correct centrifugal flag - // allows arducopter to disable corrections when disarmed - void set_correct_centrifugal(bool setting) { - _flags.correct_centrifugal = setting; - } - - // get the correct centrifugal flag - bool get_correct_centrifugal(void) const { - return _flags.correct_centrifugal; - } - // helper trig value accessors float cos_roll() const { return _cos_roll; @@ -494,7 +480,6 @@ protected: // flags structure struct ahrs_flags { uint8_t have_initial_yaw : 1; // whether the yaw value has been intialised with a reference - uint8_t correct_centrifugal : 1; // 1 if we should correct for centrifugal forces (allows arducopter to turn this off when motors are disarmed) uint8_t wind_estimation : 1; // 1 if we should do wind estimation } _flags; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5625eba632..246bf9713b 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -609,6 +609,20 @@ Vector3f AP_AHRS_DCM::ra_delayed(uint8_t instance, const Vector3f &ra) return ret; } +/* returns true if attitude should be corrected from GPS-derived + * velocity-deltas. We turn this off for Copter and other similar + * vehicles while the vehicle is disarmed to avoid the HUD bobbing + * around while the vehicle is disarmed. + */ +bool AP_AHRS_DCM::should_correct_centrifugal() const +{ +#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub) || APM_BUILD_TYPE(APM_BUILD_Blimp) + return hal.util->get_soft_armed(); +#endif + + return true; +} + // perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution @@ -762,7 +776,7 @@ AP_AHRS_DCM::drift_correction(float deltat) bool using_gps_corrections = false; float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS); - if (_flags.correct_centrifugal && (_have_gps_lock || fly_forward)) { + if (should_correct_centrifugal() && (_have_gps_lock || fly_forward)) { const float v_scale = gps_gain.get() * ra_scale; const Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 05b68ad228..550ebd7a65 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -156,6 +156,13 @@ private: // P term yaw gain based on rate of change of horiz velocity float _yaw_gain(void) const; + /* returns true if attitude should be corrected from GPS-derived + * velocity-deltas. We turn this off for Copter and other similar + * vehicles while the vehicle is disarmed to avoid the HUD bobbing + * around while the vehicle is disarmed. + */ + bool should_correct_centrifugal() const; + // state to support status reporting float _renorm_val_sum; uint16_t _renorm_val_count;