AP_AHRS: stop using AHRS as conduit for Compass pointer

libraries/AP_AHRS/AP_AHRS.cpp

@@ -1762,7 +1762,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
         }
         if (!filt_state.flags.horiz_vel ||
             (!filt_state.flags.horiz_pos_abs && !filt_state.flags.horiz_pos_rel)) {
-            if ((!_compass || !_compass->use_for_yaw()) &&
+            if ((!AP::compass().use_for_yaw()) &&
                 AP::gps().status() >= AP_GPS::GPS_OK_FIX_3D &&
                 AP::gps().ground_speed() < 2) {
                 /*
@@ -2175,7 +2175,7 @@ bool AP_AHRS::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_
     Quaternion primary_quat;
     get_quat_body_to_ned(primary_quat);
     // only check yaw if compasses are being used
-    bool check_yaw = _compass && _compass->use_for_yaw();
+    const bool check_yaw = AP::compass().use_for_yaw();
     uint8_t total_ekf_cores = 0;
 
 #if HAL_NAVEKF2_AVAILABLE

libraries/AP_AHRS/AP_AHRS.h

@@ -75,6 +75,10 @@ public:
         return _singleton;
     }
 
+    // allow for runtime change of orientation
+    // this makes initial config easier
+    void update_orientation();
+
     // return the smoothed gyro vector corrected for drift
     const Vector3f &get_gyro(void) const override;
     const Matrix3f &get_rotation_body_to_ned(void) const override;

libraries/AP_AHRS/AP_AHRS_Backend.cpp

@@ -26,7 +26,6 @@ extern const AP_HAL::HAL& hal;
 // init sets up INS board orientation
 void AP_AHRS_Backend::init()
 {
-    update_orientation();
 }
 
 // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet)
@@ -64,20 +63,16 @@ void AP_AHRS_Backend::add_trim(float roll_in_radians, float pitch_in_radians, bo
 }
 
 // Set the board mounting orientation, may be called while disarmed
-void AP_AHRS_Backend::update_orientation()
+void AP_AHRS::update_orientation()
 {
     const enum Rotation orientation = (enum Rotation)_board_orientation.get();
     if (orientation != ROTATION_CUSTOM) {
         AP::ins().set_board_orientation(orientation);
-        if (_compass != nullptr) {
+        AP::compass().set_board_orientation(orientation);
-            _compass->set_board_orientation(orientation);
-        }
     } else {
         _custom_rotation.from_euler(radians(_custom_roll), radians(_custom_pitch), radians(_custom_yaw));
         AP::ins().set_board_orientation(orientation, &_custom_rotation);
-        if (_compass != nullptr) {
+        AP::compass().set_board_orientation(orientation, &_custom_rotation);
-            _compass->set_board_orientation(orientation, &_custom_rotation);
-        }
     }
 }
 

libraries/AP_AHRS/AP_AHRS_Backend.h

@@ -95,19 +95,6 @@ public:
         _flags.wind_estimation = b;
     }
 
-    void set_compass(Compass *compass) {
-        _compass = compass;
-        update_orientation();
-    }
-
-    const Compass* get_compass() const {
-        return _compass;
-    }
-
-    // allow for runtime change of orientation
-    // this makes initial config easier
-    void update_orientation();
-
     // return the index of the primary core or -1 if no primary core selected
     virtual int8_t get_primary_core_index() const { return -1; }
 
@@ -332,9 +319,7 @@ public:
     }
 
     // return true if we will use compass for yaw
-    virtual bool use_compass(void) {
+    virtual bool use_compass(void) = 0;
-        return _compass && _compass->use_for_yaw();
-    }
 
     // return true if yaw has been initialised
     bool yaw_initialised(void) const {
@@ -624,14 +609,8 @@ protected:
     // update takeoff/touchdown flags
     void update_flags();
 
-    // pointer to compass object, if available
-    Compass         * _compass;
-
     // pointer to airspeed object, if available
 
-    // time in microseconds of last compass update
-    uint32_t _compass_last_update;
-
     // a vector to capture the difference between the controller and body frames
     AP_Vector3f         _trim;
 

libraries/AP_AHRS/AP_AHRS_DCM.cpp

@@ -346,7 +346,7 @@ AP_AHRS_DCM::normalize(void)
 // produce a yaw error value. The returned value is proportional
 // to sin() of the current heading error in earth frame
 float
-AP_AHRS_DCM::yaw_error_compass(void)
+AP_AHRS_DCM::yaw_error_compass(Compass *_compass)
 {
     const Vector3f &mag = _compass->get_field();
     // get the mag vector in the earth frame
@@ -434,6 +434,9 @@ bool AP_AHRS_DCM::use_fast_gains(void) const
 // return true if we should use the compass for yaw correction
 bool AP_AHRS_DCM::use_compass(void)
 {
+    Compass &compass = AP::compass();
+    Compass *_compass = &compass;
+
     if (!_compass || !_compass->use_for_yaw()) {
         // no compass available
         return false;
@@ -485,6 +488,9 @@ AP_AHRS_DCM::drift_correction_yaw(void)
 
     const AP_GPS &_gps = AP::gps();
 
+    Compass &compass = AP::compass();
+    Compass *_compass = &compass;
+
     if (_compass && _compass->is_calibrating()) {
         // don't do any yaw correction while calibrating
         return;
@@ -507,7 +513,7 @@ AP_AHRS_DCM::drift_correction_yaw(void)
                 _flags.have_initial_yaw = true;
             }
             new_value = true;
-            yaw_error = yaw_error_compass();
+            yaw_error = yaw_error_compass(_compass);
 
             // also update the _gps_last_update, so if we later
             // disable the compass due to significant yaw error we

libraries/AP_AHRS/AP_AHRS_DCM.h

@@ -128,7 +128,7 @@ private:
     bool            renorm(Vector3f const &a, Vector3f &result);
     void            drift_correction(float deltat);
     void            drift_correction_yaw(void);
-    float           yaw_error_compass();
+    float           yaw_error_compass(Compass *_compass);
     void            euler_angles(void);
     bool            have_gps(void) const;
     bool            use_fast_gains(void) const;
@@ -164,6 +164,9 @@ private:
     float _error_rp{1.0f};
     float _error_yaw{1.0f};
 
+    // time in microseconds of last compass update
+    uint32_t _compass_last_update;
+
     // time in millis when we last got a GPS heading
     uint32_t _gps_last_update;
 

libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp

@@ -50,10 +50,7 @@ void setup(void)
     vehicle.init();
     serial_manager.init();
     AP::compass().init();
-    if(AP::compass().read()) {
+    if (!AP::compass().read()) {
-        hal.console->printf("Enabling compass\n");
-        ahrs.set_compass(&AP::compass());
-    } else {
         hal.console->printf("No compass detected\n");
     }
     AP::gps().init(serial_manager);