From 1dabcc473d63a3e40080de4297b70f2051232cf9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 10 Mar 2018 20:35:03 +1100 Subject: [PATCH] AP_AHRS: use ins singleton --- libraries/AP_AHRS/AP_AHRS.cpp | 2 +- libraries/AP_AHRS/AP_AHRS.h | 28 ++++++++++++---------------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 9 ++++++++- libraries/AP_AHRS/AP_AHRS_DCM.h | 4 ++-- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 16 ++++++++++------ libraries/AP_AHRS/AP_AHRS_NavEKF.h | 3 +-- 6 files changed, 34 insertions(+), 28 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 954d194535..6625873e16 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -135,7 +135,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { Vector3f AP_AHRS::get_gyro_latest(void) const { const uint8_t primary_gyro = get_primary_gyro_index(); - return get_ins().get_gyro(primary_gyro) + get_gyro_drift(); + return AP::ins().get_gyro(primary_gyro) + get_gyro_drift(); } // return airspeed estimate if available diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index e9fb153105..75d1cc167f 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -53,7 +53,7 @@ public: friend class AP_AHRS_View; // Constructor - AP_AHRS(AP_InertialSensor &ins) : + AP_AHRS() : roll(0.0f), pitch(0.0f), yaw(0.0f), @@ -66,7 +66,6 @@ public: _airspeed(nullptr), _beacon(nullptr), _compass_last_update(0), - _ins(ins), _cos_roll(1.0f), _cos_pitch(1.0f), _cos_yaw(1.0f), @@ -82,7 +81,7 @@ public: // base the ki values by the sensors maximum drift // rate. - _gyro_drift_limit = ins.get_gyro_drift_rate(); + _gyro_drift_limit = AP::ins().get_gyro_drift_rate(); // enable centrifugal correction by default _flags.correct_centrifugal = true; @@ -184,7 +183,7 @@ public: // allow for runtime change of orientation // this makes initial config easier void set_orientation() { - _ins.set_board_orientation((enum Rotation)_board_orientation.get()); + AP::ins().set_board_orientation((enum Rotation)_board_orientation.get()); if (_compass != nullptr) { _compass->set_board_orientation((enum Rotation)_board_orientation.get()); } @@ -206,18 +205,14 @@ public: return _beacon; } - const AP_InertialSensor &get_ins() const { - return _ins; - } - // get the index of the current primary accelerometer sensor virtual uint8_t get_primary_accel_index(void) const { - return _ins.get_primary_accel(); + return AP::ins().get_primary_accel(); } // get the index of the current primary gyro sensor virtual uint8_t get_primary_gyro_index(void) const { - return _ins.get_primary_gyro(); + return AP::ins().get_primary_gyro(); } // accelerometer values in the earth frame in m/s/s @@ -225,7 +220,7 @@ public: return _accel_ef[i]; } virtual const Vector3f &get_accel_ef(void) const { - return get_accel_ef(_ins.get_primary_accel()); + return get_accel_ef(AP::ins().get_primary_accel()); } // blended accelerometer values in the earth frame in m/s/s @@ -549,7 +544,12 @@ public: } // Retrieves the corrected NED delta velocity in use by the inertial navigation - virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { ret.zero(); _ins.get_delta_velocity(ret); dt = _ins.get_delta_velocity_dt(); } + virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { + ret.zero(); + const AP_InertialSensor &_ins = AP::ins(); + _ins.get_delta_velocity(ret); + dt = _ins.get_delta_velocity_dt(); + } // create a view AP_AHRS_View *create_view(enum Rotation rotation); @@ -631,10 +631,6 @@ protected: // time in microseconds of last compass update uint32_t _compass_last_update; - // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing - // IMU under us without our noticing. - AP_InertialSensor &_ins; - // a vector to capture the difference between the controller and body frames AP_Vector3f _trim; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 275dfba810..e905140dad 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -57,9 +57,11 @@ AP_AHRS_DCM::update(bool skip_ins_update) if (!skip_ins_update) { // tell the IMU to grab some data - _ins.update(); + AP::ins().update(); } + const AP_InertialSensor &_ins = AP::ins(); + // ask the IMU how much time this sensor reading represents delta_t = _ins.get_delta_time(); @@ -111,6 +113,7 @@ AP_AHRS_DCM::matrix_update(float _G_Dt) // noise uint8_t healthy_count = 0; Vector3f delta_angle; + const AP_InertialSensor &_ins = AP::ins(); for (uint8_t i=0; i<_ins.get_gyro_count(); i++) { if (_ins.get_gyro_health(i) && healthy_count < 2) { Vector3f dangle; @@ -154,6 +157,8 @@ AP_AHRS_DCM::reset(bool recover_eulers) // Use the measured accel due to gravity to calculate an initial // roll and pitch estimate + AP_InertialSensor &_ins = AP::ins(); + // Get body frame accel vector Vector3f initAccVec = _ins.get_accel(); uint8_t counter = 0; @@ -576,6 +581,8 @@ AP_AHRS_DCM::drift_correction(float deltat) // vector drift_correction_yaw(); + const AP_InertialSensor &_ins = AP::ins(); + // rotate accelerometer values into the earth frame for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (_ins.get_accel_health(i)) { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 561433a9f4..0c30b7f048 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -23,8 +23,8 @@ class AP_AHRS_DCM : public AP_AHRS { public: - AP_AHRS_DCM(AP_InertialSensor &ins) - : AP_AHRS(ins) + AP_AHRS_DCM() + : AP_AHRS() , _omega_I_sum_time(0.0f) , _renorm_val_sum(0.0f) , _renorm_val_count(0) diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 5246b3c0a0..b186fc2e59 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -30,11 +30,10 @@ extern const AP_HAL::HAL& hal; // constructor -AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, - NavEKF2 &_EKF2, +AP_AHRS_NavEKF::AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags) : - AP_AHRS_DCM(ins), + AP_AHRS_DCM(), EKF2(_EKF2), EKF3(_EKF3), _ekf2_started(false), @@ -168,6 +167,8 @@ void AP_AHRS_NavEKF::update_EKF2(void) // Use the primary EKF to select the primary gyro const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex(); + const AP_InertialSensor &_ins = AP::ins(); + // get gyro bias for primary EKF and change sign to give gyro drift // Note sign convention used by EKF is bias = measurement - truth _gyro_drift.zero(); @@ -236,6 +237,8 @@ void AP_AHRS_NavEKF::update_EKF3(void) update_cd_values(); update_trig(); + const AP_InertialSensor &_ins = AP::ins(); + // Use the primary EKF to select the primary gyro const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex(); @@ -1248,6 +1251,7 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons return; } ret.zero(); + const AP_InertialSensor &_ins = AP::ins(); _ins.get_delta_velocity((uint8_t)imu_idx, ret); dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx); ret -= accel_bias*dt; @@ -1605,7 +1609,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const break; } if (imu == -1) { - imu = _ins.get_primary_accel(); + imu = AP::ins().get_primary_accel(); } return imu; } @@ -1623,7 +1627,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const if (ekf_type() != 0) { return get_primary_IMU_index(); } - return _ins.get_primary_accel(); + return AP::ins().get_primary_accel(); } // get the index of the current primary gyro sensor @@ -1632,7 +1636,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const if (ekf_type() != 0) { return get_primary_IMU_index(); } - return _ins.get_primary_gyro(); + return AP::ins().get_primary_gyro(); } #endif // AP_AHRS_NAVEKF_AVAILABLE diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 2cb51c5071..bad9cc4691 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -44,8 +44,7 @@ public: }; // Constructor - AP_AHRS_NavEKF(AP_InertialSensor &ins, - NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE); + AP_AHRS_NavEKF(NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE); /* Do not allow copies */ AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete;