From 024b29858da7fd8a683dc23f0c815f27b88585f6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 6 Mar 2018 07:34:26 +1100 Subject: [PATCH] AP_AHRS: use baro singleton --- libraries/AP_AHRS/AP_AHRS.h | 8 +------- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 4 ++-- libraries/AP_AHRS/AP_AHRS_DCM.h | 4 ++-- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 5 ++--- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 2 +- libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 6 +++--- 6 files changed, 11 insertions(+), 18 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index f6cec6c4ab..e9fb153105 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_Baro &baro) : + AP_AHRS(AP_InertialSensor &ins) : roll(0.0f), pitch(0.0f), yaw(0.0f), @@ -67,7 +67,6 @@ public: _beacon(nullptr), _compass_last_update(0), _ins(ins), - _baro(baro), _cos_roll(1.0f), _cos_pitch(1.0f), _cos_yaw(1.0f), @@ -211,10 +210,6 @@ public: return _ins; } - const AP_Baro &get_baro() const { - return _baro; - } - // get the index of the current primary accelerometer sensor virtual uint8_t get_primary_accel_index(void) const { return _ins.get_primary_accel(); @@ -639,7 +634,6 @@ protected: // 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; - AP_Baro &_baro; // 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 8a25dba490..275dfba810 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -956,7 +956,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const { loc.lat = _last_lat; loc.lng = _last_lng; - loc.alt = _baro.get_altitude() * 100 + _home.alt; + loc.alt = AP::baro().get_altitude() * 100 + _home.alt; loc.flags.relative_alt = 0; loc.flags.terrain_alt = 0; location_offset(loc, _position_offset_north, _position_offset_east); @@ -1010,7 +1010,7 @@ void AP_AHRS_DCM::set_home(const Location &loc) // a relative ground position to home in meters, Down void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const { - posD = -_baro.get_altitude(); + posD = -AP::baro().get_altitude(); } /* diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 2200423ec6..561433a9f4 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_Baro &baro) - : AP_AHRS(ins, baro) + AP_AHRS_DCM(AP_InertialSensor &ins) + : AP_AHRS(ins) , _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 307304d877..5246b3c0a0 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -31,11 +31,10 @@ extern const AP_HAL::HAL& hal; // constructor AP_AHRS_NavEKF::AP_AHRS_NavEKF(AP_InertialSensor &ins, - AP_Baro &baro, NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags) : - AP_AHRS_DCM(ins, baro), + AP_AHRS_DCM(ins), EKF2(_EKF2), EKF3(_EKF3), _ekf2_started(false), @@ -903,7 +902,7 @@ void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const float originD; if (!get_relative_position_D_origin(originD) || !get_origin(originLLH)) { - posD = -_baro.get_altitude(); + posD = -AP::baro().get_altitude(); return; } diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 5c45f91f52..2cb51c5071 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -44,7 +44,7 @@ public: }; // Constructor - AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, + AP_AHRS_NavEKF(AP_InertialSensor &ins, NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE); /* Do not allow copies */ diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 2f81a72f65..22590e5aea 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp @@ -26,9 +26,9 @@ static AP_SerialManager serial_manager; class DummyVehicle { public: RangeFinder sonar{serial_manager, ROTATION_PITCH_270}; - NavEKF2 EKF2{&ahrs, barometer, sonar}; - NavEKF3 EKF3{&ahrs, barometer, sonar}; - AP_AHRS_NavEKF ahrs{ins, barometer, EKF2, EKF3, + NavEKF2 EKF2{&ahrs, sonar}; + NavEKF3 EKF3{&ahrs, sonar}; + AP_AHRS_NavEKF ahrs{ins, EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; };