From ca58aa9c5fbc4b5b9bb6bf2acd58035682526d07 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 26 Jul 2021 10:32:10 +1000 Subject: [PATCH] AP_AHRS: stop using AHRS as conduit for Compass pointer --- libraries/AP_AHRS/AP_AHRS.cpp | 4 ++-- libraries/AP_AHRS/AP_AHRS.h | 4 ++++ libraries/AP_AHRS/AP_AHRS_Backend.cpp | 11 +++------ libraries/AP_AHRS/AP_AHRS_Backend.h | 23 +------------------ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 10 ++++++-- libraries/AP_AHRS/AP_AHRS_DCM.h | 5 +++- .../AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 5 +--- 7 files changed, 23 insertions(+), 39 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index c946325046..f6cd0353e8 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 6ac536a7e3..b74ec32637 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/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; diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 028aa04703..3dca5905e5 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/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) { - _compass->set_board_orientation(orientation); - } + AP::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) { - _compass->set_board_orientation(orientation, &_custom_rotation); - } + AP::compass().set_board_orientation(orientation, &_custom_rotation); } } diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index d0f3f3f723..8b90e502c6 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/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) { - return _compass && _compass->use_for_yaw(); - } + virtual bool use_compass(void) = 0; // 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; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5b7e334dfc..9e6435e2ba 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/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 diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index f4f5c52cce..c30cb3047a 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/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; diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 521d6a7e29..38a4eaefa2 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/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()) { - hal.console->printf("Enabling compass\n"); - ahrs.set_compass(&AP::compass()); - } else { + if (!AP::compass().read()) { hal.console->printf("No compass detected\n"); } AP::gps().init(serial_manager);