AP_AHRS: use ins singleton

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

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;
 

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)) {

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)

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

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;