diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index a05bb9aabd..10239a50ae 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -30,6 +30,7 @@ #include #include #include +#include #define ATTITUDE_CHECK_THRESH_ROLL_PITCH_RAD radians(10) #define ATTITUDE_CHECK_THRESH_YAW_RAD radians(20) @@ -287,6 +288,11 @@ void AP_AHRS::reset_gyro_drift(void) void AP_AHRS::update(bool skip_ins_update) { + if (!skip_ins_update) { + // tell the IMU to grab some data + AP::ins().update(); + } + // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); @@ -303,7 +309,7 @@ void AP_AHRS::update(bool skip_ins_update) // update autopilot-body-to-vehicle-body from _trim parameters: update_trim_rotation_matrices(); - update_DCM(skip_ins_update); + update_DCM(); // update takeoff/touchdown flags update_flags(); @@ -348,7 +354,7 @@ void AP_AHRS::update(bool skip_ins_update) if (_view != nullptr) { // update optional alternative attitude view - _view->update(skip_ins_update); + _view->update(); } // update AOA and SSA @@ -394,7 +400,7 @@ void AP_AHRS::update(bool skip_ins_update) } } -void AP_AHRS::update_DCM(bool skip_ins_update) +void AP_AHRS::update_DCM() { // we need to restore the old DCM attitude values as these are // used internally in DCM to calculate error values for gyro drift @@ -404,7 +410,7 @@ void AP_AHRS::update_DCM(bool skip_ins_update) yaw = _dcm_attitude.z; update_cd_values(); - AP_AHRS_DCM::update(skip_ins_update); + AP_AHRS_DCM::update(); // keep DCM attitude available for get_secondary_attitude() _dcm_attitude = {roll, pitch, yaw}; diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9944c10ac8..b1bcb3a094 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -90,7 +90,7 @@ public: // should be called if gyro offsets are recalculated void reset_gyro_drift() override; - void update(bool skip_ins_update=false) override; + void update(bool skip_ins_update=false); void reset(bool recover_eulers = false) override; // dead-reckoning support @@ -532,7 +532,7 @@ private: uint8_t _ekf_flags; // bitmask from Flags enumeration EKFType ekf_type(void) const; - void update_DCM(bool skip_ins_update); + void update_DCM(); // get the index of the current primary IMU uint8_t get_primary_IMU_index(void) const; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 8ea582b0fd..a5732b4a17 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -78,7 +78,7 @@ public: } // Methods - virtual void update(bool skip_ins_update=false) = 0; + virtual void update() = 0; // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 846d9fc941..851e39262d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -65,18 +65,10 @@ void AP_AHRS::load_watchdog_home() // run a full DCM update round void -AP_AHRS_DCM::update(bool skip_ins_update) +AP_AHRS_DCM::update() { - // support locked access functions to AHRS data - WITH_SEMAPHORE(_rsem); - AP_InertialSensor &_ins = AP::ins(); - if (!skip_ins_update) { - // tell the IMU to grab some data - _ins.update(); - } - // ask the IMU how much time this sensor reading represents const float delta_t = _ins.get_delta_time(); diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 6505d5bf20..029bde0df2 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -58,7 +58,7 @@ public: void reset_gyro_drift() override; // Methods - void update(bool skip_ins_update=false) override; + void update() override; void reset(bool recover_eulers = false) override; // return true if yaw has been initialised diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index c401cb474f..a8517077a8 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -58,7 +58,7 @@ void AP_AHRS_View::set_pitch_trim(float trim_deg) { }; // update state -void AP_AHRS_View::update(bool skip_ins_update) +void AP_AHRS_View::update() { rot_body_to_ned = ahrs.get_rotation_body_to_ned(); gyro = ahrs.get_gyro(); diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index e0a6d75250..10df0a911d 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -34,7 +34,7 @@ public: AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation, float pitch_trim_deg=0); // update state - void update(bool skip_ins_update=false); + void update(); // empty virtual destructor virtual ~AP_AHRS_View() {}