From a11dca52296d305a0e9d586f0b071215cac52711 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Apr 2017 11:03:45 +1000 Subject: [PATCH] AP_AHRS: added optional skip_ins_update to AHRS::update() avoid the ins update if already done in copter fast_loop() --- libraries/AP_AHRS/AP_AHRS.h | 2 +- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 8 +++++--- libraries/AP_AHRS/AP_AHRS_DCM.h | 2 +- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 10 +++++----- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 4 ++-- libraries/AP_AHRS/AP_AHRS_View.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_View.h | 2 +- 7 files changed, 16 insertions(+), 14 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e4fc4df531..09b732f2ed 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -210,7 +210,7 @@ public: } // Methods - virtual void update(void) = 0; + virtual void update(bool skip_ins_update=false) = 0; // report any reason for why the backend is refusing to initialise virtual const char *prearm_failure_reason(void) const { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index cb357a4f28..f5db679c87 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -47,7 +47,7 @@ AP_AHRS_DCM::reset_gyro_drift(void) // run a full DCM update round void -AP_AHRS_DCM::update(void) +AP_AHRS_DCM::update(bool skip_ins_update) { float delta_t; @@ -55,8 +55,10 @@ AP_AHRS_DCM::update(void) _last_startup_ms = AP_HAL::millis(); } - // tell the IMU to grab some data - _ins.update(); + if (!skip_ins_update) { + // tell the IMU to grab some data + _ins.update(); + } // ask the IMU how much time this sensor reading represents 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 7c26951026..09bc7b09cf 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -78,7 +78,7 @@ public: void reset_gyro_drift() override; // Methods - void update() override; + void update(bool skip_ins_update=false) override; void reset(bool recover_eulers = false) override; // reset the current attitude, used on new IMU calibration diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 8f0e589106..342ca04d22 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -79,13 +79,13 @@ void AP_AHRS_NavEKF::reset_gyro_drift(void) EKF3.resetGyroBias(); } -void AP_AHRS_NavEKF::update(void) +void AP_AHRS_NavEKF::update(bool skip_ins_update) { // EKF1 is no longer supported - handle case where it is selected if (_ekf_type == 1) { _ekf_type.set(2); } - update_DCM(); + update_DCM(skip_ins_update); update_EKF2(); update_EKF3(); #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -103,11 +103,11 @@ void AP_AHRS_NavEKF::update(void) if (_view != nullptr) { // update optional alternative attitude view - _view->update(); + _view->update(skip_ins_update); } } -void AP_AHRS_NavEKF::update_DCM(void) +void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update) { // we need to restore the old DCM attitude values as these are // used internally in DCM to calculate error values for gyro drift @@ -117,7 +117,7 @@ void AP_AHRS_NavEKF::update_DCM(void) yaw = _dcm_attitude.z; update_cd_values(); - AP_AHRS_DCM::update(); + AP_AHRS_DCM::update(skip_ins_update); // keep DCM attitude available for get_secondary_attitude() _dcm_attitude(roll, pitch, yaw); diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 5591dcc780..0d1298b485 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -59,7 +59,7 @@ public: // should be called if gyro offsets are recalculated void reset_gyro_drift() override; - void update() override; + void update(bool skip_ins_update=false) override; void reset(bool recover_eulers = false) override; // reset the current attitude, used on new IMU calibration @@ -268,7 +268,7 @@ private: Flags _ekf_flags; uint8_t ekf_type(void) const; - void update_DCM(void); + void update_DCM(bool skip_ins_update); void update_EKF2(void); void update_EKF3(void); diff --git a/libraries/AP_AHRS/AP_AHRS_View.cpp b/libraries/AP_AHRS/AP_AHRS_View.cpp index 163d4fb7b7..13d328a7cb 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.cpp +++ b/libraries/AP_AHRS/AP_AHRS_View.cpp @@ -44,7 +44,7 @@ AP_AHRS_View::AP_AHRS_View(AP_AHRS &_ahrs, enum Rotation _rotation) : } // update state -void AP_AHRS_View::update(void) +void AP_AHRS_View::update(bool skip_ins_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 33a93adbdb..fb16d17c7f 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -29,7 +29,7 @@ public: AP_AHRS_View(AP_AHRS &ahrs, enum Rotation rotation); // update state - void update(void); + void update(bool skip_ins_update=false); // empty virtual destructor virtual ~AP_AHRS_View() {}