From 7f4a4a99d64d0393e3c266b1641dda85ae2a8ed4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 25 Jul 2021 09:08:28 +1000 Subject: [PATCH] AP_AHRS: move home and origin methods to frontend --- libraries/AP_AHRS/AP_AHRS.cpp | 11 +++++-- libraries/AP_AHRS/AP_AHRS.h | 44 +++++++++++++++++++++++++-- libraries/AP_AHRS/AP_AHRS_Backend.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_Backend.h | 40 ------------------------ libraries/AP_AHRS/AP_AHRS_DCM.cpp | 16 +++------- libraries/AP_AHRS/AP_AHRS_DCM.h | 2 -- 6 files changed, 56 insertions(+), 59 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 20b16255fb..eb250ac658 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -270,7 +270,13 @@ void AP_AHRS::update(bool skip_ins_update) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); - + + // see if we have to restore home after a watchdog reset: + if (!_checked_watchdog_home) { + load_watchdog_home(); + _checked_watchdog_home = true; + } + // drop back to normal priority if we were boosted by the INS // calling delay_microseconds_boost() hal.scheduler->boost_end(); @@ -654,9 +660,10 @@ void AP_AHRS::reset(bool recover_eulers) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); - + AP_AHRS_DCM::reset(recover_eulers); _dcm_attitude = {roll, pitch, yaw}; + #if HAL_NAVEKF2_AVAILABLE if (_ekf2_started) { _ekf2_started = EKF2.InitialiseFilter(); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 1cb35c82ce..4cac672440 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -144,10 +144,10 @@ public: // set the EKF's origin location in 10e7 degrees. This should only // be called when the EKF has no absolute position reference (i.e. GPS) // from which to decide the origin on its own - bool set_origin(const Location &loc) override WARN_IF_UNUSED; + bool set_origin(const Location &loc) WARN_IF_UNUSED; // returns the inertial navigation origin in lat/lon/alt - bool get_origin(Location &ret) const override WARN_IF_UNUSED; + bool get_origin(Location &ret) const WARN_IF_UNUSED; bool have_inertial_nav() const override; @@ -337,6 +337,37 @@ public: // return SSA float getSSA(void) const { return _SSA; } + /* + * home-related functionality + */ + + // get the home location. This is const to prevent any changes to + // home without telling AHRS about the change + const struct Location &get_home(void) const { + return _home; + } + + // functions to handle locking of home. Some vehicles use this to + // allow GCS to lock in a home location. + void lock_home() { + _home_locked = true; + } + bool home_is_locked() const { + return _home_locked; + } + + // returns true if home is set + bool home_is_set(void) const { + return _home_is_set; + } + + // set the home location in 10e7 degrees. This should be called + // when the vehicle is at this position. It is assumed that the + // current barometer and GPS altitudes correspond to this altitude + bool set_home(const Location &loc) WARN_IF_UNUSED; + + void Log_Write_Home_And_Origin(); + protected: // optional view class AP_AHRS_View *_view; @@ -396,6 +427,15 @@ private: // get the index of the current primary IMU uint8_t get_primary_IMU_index(void) const; + /* + * home-related state + */ + void load_watchdog_home(); + bool _checked_watchdog_home; + struct Location _home; + bool _home_is_set :1; + bool _home_locked :1; + // avoid setting current state repeatedly across all cores on all EKFs: enum class TriState { False = 0, diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.cpp b/libraries/AP_AHRS/AP_AHRS_Backend.cpp index 3dca5905e5..57492383ee 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Backend.cpp @@ -304,7 +304,7 @@ Vector2f AP_AHRS_Backend::body_to_earth2D(const Vector2f &bf) const } // log ahrs home and EKF origin -void AP_AHRS_Backend::Log_Write_Home_And_Origin() +void AP_AHRS::Log_Write_Home_And_Origin() { AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 21a557aa94..5895cb4de5 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -386,41 +386,6 @@ public: return false; } - // get the home location. This is const to prevent any changes to - // home without telling AHRS about the change - const struct Location &get_home(void) const { - return _home; - } - - // functions to handle locking of home. Some vehicles use this to - // allow GCS to lock in a home location. - void lock_home() { - _home_locked = true; - } - bool home_is_locked() const { - return _home_locked; - } - - // returns true if home is set - bool home_is_set(void) const { - return _home_is_set; - } - - // set the home location in 10e7 degrees. This should be called - // when the vehicle is at this position. It is assumed that the - // current barometer and GPS altitudes correspond to this altitude - virtual bool set_home(const Location &loc) WARN_IF_UNUSED = 0; - - // set the EKF's origin location in 10e7 degrees. This should only - // be called when the EKF has no absolute position reference (i.e. GPS) - // from which to decide the origin on its own - virtual bool set_origin(const Location &loc) WARN_IF_UNUSED { return false; } - - // returns the inertial navigation origin in lat/lon/alt - virtual bool get_origin(Location &ret) const WARN_IF_UNUSED { return false; } - - void Log_Write_Home_And_Origin(); - // return true if the AHRS object supports inertial navigation, // with very accurate position and velocity virtual bool have_inertial_nav(void) const { @@ -629,11 +594,6 @@ protected: Vector2f _hp; // ground vector high-pass filter Vector2f _lastGndVelADS; // previous HPF input - // reference position for NED positions - struct Location _home; - bool _home_is_set :1; - bool _home_locked :1; - // helper trig variables float _cos_roll{1.0f}; float _cos_pitch{1.0f}; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index 20ad3369ef..01bc25e566 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -50,7 +50,7 @@ AP_AHRS_DCM::reset_gyro_drift(void) /* if this was a watchdog reset then get home from backup registers */ -void AP_AHRS_DCM::load_watchdog_home() +void AP_AHRS::load_watchdog_home() { const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data; if (hal.util->was_watchdog_reset() && (pd.home_lat != 0 || pd.home_lon != 0)) { @@ -70,11 +70,6 @@ AP_AHRS_DCM::update(bool skip_ins_update) // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); - if (_last_startup_ms == 0) { - _last_startup_ms = AP_HAL::millis(); - load_watchdog_home(); - } - AP_InertialSensor &_ins = AP::ins(); if (!skip_ins_update) { @@ -230,9 +225,6 @@ AP_AHRS_DCM::reset(bool recover_eulers) } - if (_last_startup_ms == 0) { - load_watchdog_home(); - } _last_startup_ms = AP_HAL::millis(); } @@ -1038,7 +1030,7 @@ bool AP_AHRS_DCM::get_position(struct Location &loc) const gps.status() >= AP_GPS::GPS_OK_FIX_3D) { loc.alt = gps.location().alt; } else { - loc.alt = baro.get_altitude() * 100 + _home.alt; + loc.alt = baro.get_altitude() * 100 + AP::ahrs().get_home().alt; } loc.relative_alt = 0; loc.terrain_alt = 0; @@ -1094,7 +1086,7 @@ bool AP_AHRS_DCM::airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) return true; } -bool AP_AHRS_DCM::set_home(const Location &loc) +bool AP_AHRS::set_home(const Location &loc) { WITH_SEMAPHORE(_rsem); // check location is valid @@ -1135,7 +1127,7 @@ void AP_AHRS_DCM::get_relative_position_D_home(float &posD) const const auto &gps = AP::gps(); if (_gps_use == GPSUse::EnableWithHeight && gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - posD = (_home.alt - gps.location().alt) * 0.01; + posD = (AP::ahrs().get_home().alt - gps.location().alt) * 0.01; } else { posD = -AP::baro().get_altitude(); } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index b29c721d7d..05b68ad228 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -102,7 +102,6 @@ public: // return the quaternion defining the rotation from NED to XYZ (body) axes bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED; - bool set_home(const Location &loc) override WARN_IF_UNUSED; void estimate_wind(void); // is the AHRS subsystem healthy? @@ -132,7 +131,6 @@ private: void euler_angles(void); bool have_gps(void) const; bool use_fast_gains(void) const; - void load_watchdog_home(); void backup_attitude(void); // primary representation of attitude of board used for all inertial calculations