AP_Mount: SoloGimbal: stop taking reference to AHRS

libraries/AP_Mount/SoloGimbal.cpp

@@ -27,6 +27,8 @@ bool SoloGimbal::aligned()
 
 gimbal_mode_t SoloGimbal::get_mode()
 {
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
+
     if ((_gimbalParams.initialized() && is_zero(_gimbalParams.get_K_rate())) || (_ahrs.get_rotation_body_to_ned().c.z < 0 && !(_lockedToBody || _calibrator.running()))) {
         return GIMBAL_MODE_IDLE;
     } else if (!_ekf.getStatus()) {
@@ -268,6 +270,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_yaw(const Quaternion &quatEst)
     float dt = _measurement.delta_time;
     float alpha = dt/(dt+tc);
 
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
     Matrix3f Tve = _ahrs.get_rotation_body_to_ned();
     Matrix3f Teg;
     quatEst.inverse().rotation_matrix(Teg);
@@ -358,6 +361,7 @@ Vector3f SoloGimbal::get_ang_vel_dem_body_lock()
     joint_rates_to_gimbal_ang_vel(gimbalRateDemVecBodyLock, gimbalRateDemVecBodyLock);
 
     // Add a feedforward term from vehicle gyros
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
     gimbalRateDemVecBodyLock += Tvg * _ahrs.get_gyro();
 
     return gimbalRateDemVecBodyLock;

libraries/AP_Mount/SoloGimbal.h

@@ -39,8 +39,7 @@ class SoloGimbal : AP_AccelCal_Client
 public:
     //Constructor
     SoloGimbal() :
-        _ekf(AP::ahrs_navekf()),
-        _ahrs(AP::ahrs_navekf()),
+        _ekf(),
         _state(GIMBAL_STATE_NOT_PRESENT),
         _vehicle_yaw_rate_ef_filt(0.0f),
         _vehicle_to_gimbal_quat(),
@@ -107,7 +106,6 @@ private:
 
     // private member variables
     SoloGimbalEKF            _ekf;      // state of small EKF for gimbal
-    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF
 
     gimbal_state_t _state;
 

libraries/AP_Mount/SoloGimbalEKF.cpp

@@ -28,8 +28,7 @@ const AP_Param::GroupInfo SoloGimbalEKF::var_info[] = {
 #define GYRO_BIAS_LIMIT 0.349066f // maximum allowed gyro bias (rad/sec)
 
 // constructor
-SoloGimbalEKF::SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs) :
-    _ahrs(ahrs),
+SoloGimbalEKF::SoloGimbalEKF() :
     states(),
     state(*reinterpret_cast<struct state_elements *>(&states))
 {
@@ -76,6 +75,8 @@ void SoloGimbalEKF::RunEKF(float delta_time, const Vector3f &delta_angles, const
         bool main_ekf_healthy = false;
         nav_filter_status main_ekf_status;
 
+        const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
+
         if (_ahrs.get_filter_status(main_ekf_status)) {
             if (main_ekf_status.flags.attitude) {
                 main_ekf_healthy = true;
@@ -595,6 +596,8 @@ void SoloGimbalEKF::predictCovariance()
 // Fuse the SoloGimbalEKF velocity estimates - this enables alevel reference to be maintained during constant turns
 void SoloGimbalEKF::fuseVelocity()
 {
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
+
     if (!_ahrs.have_inertial_nav()) {
         return;
     }
@@ -664,6 +667,8 @@ void SoloGimbalEKF::fuseVelocity()
 // check for new magnetometer data and update store measurements if available
 void SoloGimbalEKF::readMagData()
 {
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
+
     if (_ahrs.get_compass() &&
         _ahrs.get_compass()->use_for_yaw() &&
         _ahrs.get_compass()->last_update_usec() != lastMagUpdate) {
@@ -861,6 +866,8 @@ float SoloGimbalEKF::calcMagHeadingInnov()
     Tms[1][2] = sinPhi;
     Tms[2][2] = cosTheta*cosPhi;
 
+    const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf();
+
     // get earth magnetic field estimate from main ekf if available to take advantage of main ekf magnetic field learning
     Vector3f earth_magfield = Vector3f(0,0,0);
     _ahrs.get_mag_field_NED(earth_magfield);

libraries/AP_Mount/SoloGimbalEKF.h

@@ -75,7 +75,7 @@ public:
 #endif
 
     // Constructor
-    SoloGimbalEKF(const AP_AHRS_NavEKF &ahrs);
+    SoloGimbalEKF();
 
     // Run the EKF main loop once every time we receive sensor data
     void RunEKF(float delta_time, const Vector3f &delta_angles, const Vector3f &delta_velocity, const Vector3f &joint_angles);
@@ -100,7 +100,6 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
-    const AP_AHRS_NavEKF &_ahrs;
 
     // the states are available in two forms, either as a Vector13 or
     // broken down as individual elements. Both are equivalent (same