diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index c801d21fd6..5912874bf2 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -703,7 +703,7 @@ bool AP_AHRS::_get_location(Location &loc) const { switch (active_EKF_type()) { case EKFType::NONE: - return dcm.get_location(loc); + return dcm_estimates.get_location(loc); #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: @@ -723,19 +723,18 @@ bool AP_AHRS::_get_location(Location &loc) const #if AP_AHRS_SIM_ENABLED case EKFType::SIM: - return sim.get_location(loc); + return sim_estimates.get_location(loc); #endif #if HAL_EXTERNAL_AHRS_ENABLED - case EKFType::EXTERNAL: { - return external.get_location(loc); - } + case EKFType::EXTERNAL: + return external_estimates.get_location(loc); #endif } // fall back to position from DCM if (!always_use_EKF()) { - return dcm.get_location(loc); + return dcm_estimates.get_location(loc); } return false; } @@ -1178,7 +1177,10 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const case EKFType::NONE: // return DCM position - dcm.get_location(loc); + loc = dcm_estimates.location; + // FIXME: we intentionally do not return whether location is + // actually valid here so we continue to send mavlink messages + // and log data: return true; #if HAL_NAVEKF2_AVAILABLE @@ -1204,7 +1206,7 @@ bool AP_AHRS::_get_secondary_position(Location &loc) const #if HAL_EXTERNAL_AHRS_ENABLED case EKFType::EXTERNAL: // External is secondary - return external.get_location(loc); + return external_estimates.get_location(loc); #endif } diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 9caeea17e7..9c1f3033a6 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -812,25 +812,6 @@ private: // poke AP_Notify based on values from status void update_notify_from_filter_status(const nav_filter_status &status); - /* - * backends (and their results) - */ - AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats}; - struct AP_AHRS_Backend::Estimates dcm_estimates; -#if AP_AHRS_SIM_ENABLED -#if HAL_NAVEKF3_AVAILABLE - AP_AHRS_SIM sim{EKF3}; -#else - AP_AHRS_SIM sim; -#endif - struct AP_AHRS_Backend::Estimates sim_estimates; -#endif - -#if HAL_EXTERNAL_AHRS_ENABLED - AP_AHRS_External external; - struct AP_AHRS_Backend::Estimates external_estimates; -#endif - /* * copy results from a backend over AP_AHRS canonical results. * This updates member variables like roll and pitch, as well as @@ -957,6 +938,26 @@ private: Vector3f velocity_NED; bool velocity_NED_ok; } state; + + /* + * backends (and their results) + */ + AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats}; + struct AP_AHRS_Backend::Estimates dcm_estimates; +#if AP_AHRS_SIM_ENABLED +#if HAL_NAVEKF3_AVAILABLE + AP_AHRS_SIM sim{EKF3}; +#else + AP_AHRS_SIM sim; +#endif + struct AP_AHRS_Backend::Estimates sim_estimates; +#endif + +#if HAL_EXTERNAL_AHRS_ENABLED + AP_AHRS_External external; + struct AP_AHRS_Backend::Estimates external_estimates; +#endif + }; namespace AP { diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index 6675a9c288..9d9696ad59 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -24,6 +24,7 @@ #include #include #include +#include #define AP_AHRS_TRIM_LIMIT 10.0f // maximum trim angle in degrees #define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter @@ -57,6 +58,14 @@ public: Vector3f gyro_drift; Vector3f accel_ef; Vector3f accel_bias; + + Location location; + bool location_valid; + + bool get_location(Location &loc) const { + loc = location; + return location_valid; + }; }; // init sets up INS board orientation @@ -109,10 +118,6 @@ public: // reset the current attitude, used on new IMU calibration virtual void reset() = 0; - // get our current position estimate. Return true if a position is available, - // otherwise false. This call fills in lat, lng and alt - virtual bool get_location(class Location &loc) const WARN_IF_UNUSED = 0; - // get latest altitude estimate above ground level in meters and validity flag virtual bool get_hagl(float &height) const WARN_IF_UNUSED { return false; } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp index bd1ce5d760..eb49fc6179 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -124,6 +124,8 @@ void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results) results.gyro_estimate = _omega; results.gyro_drift = _omega_I; results.accel_ef = _accel_ef; + + results.location_valid = get_location(results.location); } /* diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 168cbbaa33..1a8825a97d 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -60,9 +60,6 @@ public: return have_initial_yaw; } - // dead-reckoning support - virtual bool get_location(Location &loc) const override; - // status reporting float get_error_rp() const { return _error_rp; @@ -132,6 +129,9 @@ public: private: + // dead-reckoning support + bool get_location(Location &loc) const; + // settable parameters AP_Float &_kp_yaw; AP_Float &_kp; diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 0c1a01b2c7..0c4be3d5bd 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -36,12 +36,8 @@ void AP_AHRS_External::get_results(AP_AHRS_Backend::Estimates &results) const Vector3f accel = AP::externalAHRS().get_accel(); const Vector3f accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel; results.accel_ef = accel_ef; -} - -bool AP_AHRS_External::get_location(struct Location &loc) const -{ - return AP::externalAHRS().get_location(loc); + results.location_valid = AP::externalAHRS().get_location(results.location); } bool AP_AHRS_External::get_quaternion(Quaternion &quat) const diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h index 999ed05fed..4d63a07029 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.h +++ b/libraries/AP_AHRS/AP_AHRS_External.h @@ -43,9 +43,6 @@ public: void get_results(Estimates &results) override; void reset() override {} - // dead-reckoning support - virtual bool get_location(struct Location &loc) const override; - // return a wind estimation vector, in m/s bool wind_estimate(Vector3f &ret) const override { return false; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 3d191da785..ff7e042d47 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -255,6 +255,8 @@ void AP_AHRS_SIM::get_results(AP_AHRS_Backend::Estimates &results) const Vector3f &accel = _ins.get_accel(); results.accel_ef = results.dcm_matrix * AP::ahrs().get_rotation_autopilot_body_to_vehicle_body() * accel; + results.location_valid = get_location(results.location); + #if HAL_NAVEKF3_AVAILABLE if (_sitl->odom_enable) { // use SITL states to write body frame odometry data at 20Hz diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 340a8877b6..037b64c388 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -60,9 +60,6 @@ public: void get_results(Estimates &results) override; void reset() override { return; } - // dead-reckoning support - virtual bool get_location(Location &loc) const override; - // get latest altitude estimate above ground level in meters and validity flag bool get_hagl(float &hagl) const override WARN_IF_UNUSED; @@ -119,6 +116,9 @@ public: private: + // dead-reckoning support + bool get_location(Location &loc) const; + #if HAL_NAVEKF3_AVAILABLE // a reference to the EKF3 backend that we can use to send in // body-frame-odometry data into the EKF. Rightfully there should