From b6c21846576d1e20df36262dca46f04d733a2b65 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:37 +1000 Subject: [PATCH 01/34] AC_Avoidance: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AC_Avoidance/AC_Avoid.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libraries/AC_Avoidance/AC_Avoid.cpp b/libraries/AC_Avoidance/AC_Avoid.cpp index 6169ad161d1fe1..066d5d1c504bfa 100644 --- a/libraries/AC_Avoidance/AC_Avoid.cpp +++ b/libraries/AC_Avoidance/AC_Avoid.cpp @@ -430,7 +430,7 @@ void AC_Avoid::adjust_velocity_z(float kP, float accel_cmss, float& climb_rate_c float alt_limit; float curr_alt; if (_ahrs.get_hgt_ctrl_limit(alt_limit) && - _ahrs.get_relative_position_D_origin(curr_alt)) { + _ahrs.get_relative_position_D_origin_float(curr_alt)) { // alt_limit is UP, curr_alt is DOWN: const float ctrl_alt_diff = alt_limit + curr_alt; if (!limit_max_alt || ctrl_alt_diff < max_alt_diff) { @@ -917,7 +917,7 @@ void AC_Avoid::adjust_velocity_inclusion_circles(float kP, float accel_cmss, Vec // get vehicle position Vector2f position_NE; - if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(position_NE)) { // do not limit velocity if we don't have a position estimate return; } @@ -1054,7 +1054,7 @@ void AC_Avoid::adjust_velocity_exclusion_circles(float kP, float accel_cmss, Vec // get vehicle position Vector2f position_NE; - if (!AP::ahrs().get_relative_position_NE_origin(position_NE)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(position_NE)) { // do not limit velocity if we don't have a position estimate return; } @@ -1344,7 +1344,7 @@ void AC_Avoid::adjust_velocity_polygon(float kP, float accel_cmss, Vector2f &des // do not adjust velocity if vehicle is outside the polygon fence Vector2f position_xy; - if (!_ahrs.get_relative_position_NE_origin(position_xy)) { + if (!_ahrs.get_relative_position_NE_origin_float(position_xy)) { // boundary is in earth frame but we have no idea // where we are return; From 0108cd332bda39274aaa13f9f20d4ec4846b7ae7 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 02/34] AC_PrecLand: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AC_PrecLand/AC_PrecLand.cpp | 6 +++--- libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index c97742a855c2db..3b2dccb4b470f3 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -350,7 +350,7 @@ void AC_PrecLand::check_target_status(float rangefinder_alt_m, bool rangefinder_ if (_current_target_state == TargetState::TARGET_RECENTLY_LOST) { // check if it's nearby/found recently, else the status will be demoted to "TARGET_LOST" Vector2f curr_pos; - if (AP::ahrs().get_relative_position_NE_origin(curr_pos)) { + if (AP::ahrs().get_relative_position_NE_origin_float(curr_pos)) { const float dist_to_last_target_loc_xy = (curr_pos - Vector2f{_last_target_pos_rel_origin_NED.x, _last_target_pos_rel_origin_NED.y}).length(); const float dist_to_last_loc_xy = (curr_pos - Vector2f{_last_vehicle_pos_NED.x, _last_vehicle_pos_NED.y}).length(); if ((AP_HAL::millis() - _last_valid_target_ms) > LANDING_TARGET_LOST_TIMEOUT_MS) { @@ -418,7 +418,7 @@ bool AC_PrecLand::get_target_position_cm(Vector2f& ret) return false; } Vector2f curr_pos; - if (!AP::ahrs().get_relative_position_NE_origin(curr_pos)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos)) { return false; } ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm @@ -681,7 +681,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, // store the current relative down position so that if we need to retry landing, we know at this height landing target can be found const AP_AHRS &_ahrs = AP::ahrs(); Vector3f pos_NED; - if (_ahrs.get_relative_position_NED_origin(pos_NED)) { + if (_ahrs.get_relative_position_NED_origin_float(pos_NED)) { _last_target_pos_rel_origin_NED.z = pos_NED.z; _last_vehicle_pos_NED = pos_NED; } diff --git a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp index c25c51e8d03606..cd46ff37efe6c4 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp @@ -195,7 +195,7 @@ AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3 // continue converging towards the target till we are close by retry_pos_m = go_to_pos; Vector3f pos; - if (!AP::ahrs().get_relative_position_NED_origin(pos)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(pos)) { return Status::ERROR; } const float dist_to_target = (go_to_pos-pos).length(); @@ -210,7 +210,7 @@ AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3 // descend a little bit before completing the retry // This will descend to the original height of where landing target was first detected Vector3f pos; - if (!AP::ahrs().get_relative_position_NED_origin(pos)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(pos)) { return Status::ERROR; } // z_target is in "D" frame From 7253fafe8cbe0f04149c61f4e249ec6ce0c501fc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 03/34] AP_AHRS: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_AHRS/AP_AHRS.cpp | 8 ++++---- libraries/AP_AHRS/AP_AHRS.h | 6 +++--- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 2 +- libraries/AP_AHRS/AP_AHRS_View.h | 12 ++++++------ 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 4e7ae679c3e5b6..3fbd6916f5d4c3 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1712,7 +1712,7 @@ bool AP_AHRS::get_hagl(float &height) const /* return a relative NED position from the origin in meters */ -bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const +bool AP_AHRS::get_relative_position_NED_origin_float(Vector3f &vec) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1780,7 +1780,7 @@ bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const /* return a relative position estimate from the origin in meters */ -bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const +bool AP_AHRS::get_relative_position_NE_origin_float(Vector2f &posNE) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1836,7 +1836,7 @@ bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const /* return a relative ground position from the origin in meters, down */ -bool AP_AHRS::get_relative_position_D_origin(float &posD) const +bool AP_AHRS::get_relative_position_D_origin_float(float &posD) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1884,7 +1884,7 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const Location originLLH; float originD; - if (!get_relative_position_D_origin(originD) || + if (!get_relative_position_D_origin_float(originD) || !_get_origin(originLLH)) { #if AP_GPS_ENABLED const auto &gps = AP::gps(); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 30d6f0f72d5029..981cf50b392fbe 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -269,17 +269,17 @@ class AP_AHRS { // return the relative position NED from either home or origin // return true if the estimate is valid bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED; - bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED; + bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED; // return the relative position NE from home or origin // return true if the estimate is valid bool get_relative_position_NE_home(Vector2f &posNE) const WARN_IF_UNUSED; - bool get_relative_position_NE_origin(Vector2f &posNE) const WARN_IF_UNUSED; + bool get_relative_position_NE_origin_float(Vector2f &posNE) const WARN_IF_UNUSED; // return the relative position down from home or origin // baro will be used for the _home relative one if the EKF isn't void get_relative_position_D_home(float &posD) const; - bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED; + bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED; // return location corresponding to vector relative to the // vehicle's origin diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 6498495ee526a8..8a3fc727d08178 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -98,7 +98,7 @@ void AP_AHRS::Write_POS() const const auto nanf = AP::logger().quiet_nanf(); float origin_pos_up; - if (get_relative_position_D_origin(origin_pos_up)) { + if (get_relative_position_D_origin_float(origin_pos_up)) { origin_pos_up *= -1; // down -> up } else { origin_pos_up = nanf; diff --git a/libraries/AP_AHRS/AP_AHRS_View.h b/libraries/AP_AHRS/AP_AHRS_View.h index 1090e112f89beb..d3426a76cbce0b 100644 --- a/libraries/AP_AHRS/AP_AHRS_View.h +++ b/libraries/AP_AHRS/AP_AHRS_View.h @@ -116,24 +116,24 @@ class AP_AHRS_View return ahrs.get_relative_position_NED_home(vec); } - bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED { - return ahrs.get_relative_position_NED_origin(vec); + bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED { + return ahrs.get_relative_position_NED_origin_float(vec); } bool get_relative_position_NE_home(Vector2f &vecNE) const WARN_IF_UNUSED { return ahrs.get_relative_position_NE_home(vecNE); } - bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED { - return ahrs.get_relative_position_NE_origin(vecNE); + bool get_relative_position_NE_origin_float(Vector2f &vecNE) const WARN_IF_UNUSED { + return ahrs.get_relative_position_NE_origin_float(vecNE); } void get_relative_position_D_home(float &posD) const { ahrs.get_relative_position_D_home(posD); } - bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED { - return ahrs.get_relative_position_D_origin(posD); + bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED { + return ahrs.get_relative_position_D_origin_float(posD); } float groundspeed(void) { From 84bc6beb310378bb750418a6cf2ffe4e9d98f962 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 04/34] AP_Follow: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_Follow/AP_Follow.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Follow/AP_Follow.cpp b/libraries/AP_Follow/AP_Follow.cpp index 72ee496826d95f..ea8fa4e6ecedf0 100644 --- a/libraries/AP_Follow/AP_Follow.cpp +++ b/libraries/AP_Follow/AP_Follow.cpp @@ -217,7 +217,7 @@ bool AP_Follow::get_target_position_and_velocity_NED_m(Vector3p &pos_ned_m, Vect bool AP_Follow::get_target_dist_and_vel_NED_m(Vector3f &dist_ned_m, Vector3f &dist_with_offs_m, Vector3f &vel_ned_ms) { Vector3f current_position_NED; - if (!AP::ahrs().get_relative_position_NED_origin(current_position_NED)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(current_position_NED)) { clear_dist_and_bearing_to_target(); return false; } From a0173dad472286f6a9ebb20babd167e893c67d91 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 05/34] AP_ICEngine: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_ICEngine/AP_ICEngine.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp index 797854cf86d414..cd7f8acfe84e44 100644 --- a/libraries/AP_ICEngine/AP_ICEngine.cpp +++ b/libraries/AP_ICEngine/AP_ICEngine.cpp @@ -377,7 +377,7 @@ void AP_ICEngine::update(void) Vector3f pos; if (!should_run) { state = ICE_OFF; - } else if (AP::ahrs().get_relative_position_NED_origin(pos)) { + } else if (AP::ahrs().get_relative_position_NED_origin_float(pos)) { if (height_pending) { height_pending = false; initial_height = -pos.z; @@ -447,7 +447,7 @@ void AP_ICEngine::update(void) if (state == ICE_START_HEIGHT_DELAY) { // when disarmed we can be waiting for takeoff Vector3f pos; - if (AP::ahrs().get_relative_position_NED_origin(pos)) { + if (AP::ahrs().get_relative_position_NED_origin_float(pos)) { // reset initial height while disarmed initial_height = -pos.z; } From 1a151e10abc1190292030fee11515787bf0e8de9 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 06/34] AP_InertialNav: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_InertialNav/AP_InertialNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 326fdf0746310d..1da9a56f5ef2e4 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -15,14 +15,14 @@ void AP_InertialNav::update(bool high_vibes) { // get the NE position relative to the local earth frame origin Vector2f posNE; - if (_ahrs_ekf.get_relative_position_NE_origin(posNE)) { + if (_ahrs_ekf.get_relative_position_NE_origin_float(posNE)) { _relpos_cm.x = posNE.x * 100; // convert from m to cm _relpos_cm.y = posNE.y * 100; // convert from m to cm } // get the D position relative to the local earth frame origin float posD; - if (_ahrs_ekf.get_relative_position_D_origin(posD)) { + if (_ahrs_ekf.get_relative_position_D_origin_float(posD)) { _relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU } From 2975dc87ab1557c2027570f346584340dca17dc8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 07/34] APM_Control: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/APM_Control/AR_PosControl.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index 029f20719f0265..d785310c366916 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -116,7 +116,7 @@ void AR_PosControl::update(float dt) // exit immediately if no current location, destination or disarmed Vector2f curr_pos_NE; Vector3f curr_vel_NED; - if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || + if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { _desired_speed = _atc.get_desired_speed_accel_limited(0.0f, dt); _desired_lat_accel = 0.0f; @@ -250,7 +250,7 @@ bool AR_PosControl::init() // get current position and velocity from AHRS Vector2f pos_NE; Vector3f vel_NED; - if (!AP::ahrs().get_relative_position_NE_origin(pos_NE) || !AP::ahrs().get_velocity_NED(vel_NED)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(pos_NE) || !AP::ahrs().get_velocity_NED(vel_NED)) { return false; } @@ -351,7 +351,7 @@ Vector2p AR_PosControl::get_pos_error() const { // return zero error is not active or no position estimate Vector2f curr_pos_NE; - if (!is_active() ||!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + if (!is_active() ||!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) { return Vector2p{}; } @@ -371,7 +371,7 @@ void AR_PosControl::write_log() // exit immediately if no position or velocity estimate Vector3f curr_pos_NED; Vector3f curr_vel_NED; - if (!AP::ahrs().get_relative_position_NED_origin(curr_pos_NED) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(curr_pos_NED) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { return; } @@ -418,7 +418,7 @@ void AR_PosControl::handle_ekf_xy_reset() uint32_t reset_ms = AP::ahrs().getLastPosNorthEastReset(pos_shift); if (reset_ms != _ekf_xy_reset_ms) { Vector2f pos_NE; - if (!AP::ahrs().get_relative_position_NE_origin(pos_NE)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(pos_NE)) { return; } _pos_target = (pos_NE + _p_pos.get_error()).topostype(); From a85549035421b7e042ecd46285830bcaed433296 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:38 +1000 Subject: [PATCH 08/34] AP_Module: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_Module/AP_Module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Module/AP_Module.cpp b/libraries/AP_Module/AP_Module.cpp index 7e6c5ce6a236d5..d1551d8183ae29 100644 --- a/libraries/AP_Module/AP_Module.cpp +++ b/libraries/AP_Module/AP_Module.cpp @@ -187,7 +187,7 @@ void AP_Module::call_hook_AHRS_update(const AP_AHRS &ahrs) } Vector3f pos; - if (ahrs.get_relative_position_NED_origin(pos)) { + if (ahrs.get_relative_position_NED_origin_float(pos)) { state.relative_position[0] = pos[0]; state.relative_position[1] = pos[1]; state.relative_position[2] = pos[2]; From 6bfc1e47e5b50fad74e4e057522faefc3ea40ff2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 09/34] AP_Proximity: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_Proximity/AP_Proximity_Backend.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Proximity/AP_Proximity_Backend.cpp b/libraries/AP_Proximity/AP_Proximity_Backend.cpp index d5467ce01b7447..57d95d68a77688 100644 --- a/libraries/AP_Proximity/AP_Proximity_Backend.cpp +++ b/libraries/AP_Proximity/AP_Proximity_Backend.cpp @@ -94,7 +94,7 @@ bool AP_Proximity_Backend::database_prepare_for_push(Vector3f ¤t_pos, Matr return false; } - if (!AP::ahrs().get_relative_position_NED_origin(current_pos)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(current_pos)) { return false; } From 48198d0058cc6704889b43379d87e45948f6a589 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 10/34] AP_Scripting: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_Scripting/generator/description/bindings.desc | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 2793df7c25b2df..3fa886aa20b14a 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -48,7 +48,9 @@ singleton AP_AHRS method head_wind float'skip_check singleton AP_AHRS method groundspeed_vector Vector2f singleton AP_AHRS method get_velocity_NED boolean Vector3f'Null singleton AP_AHRS method get_relative_position_NED_home boolean Vector3f'Null -singleton AP_AHRS method get_relative_position_NED_origin boolean Vector3f'Null +singleton AP_AHRS method get_relative_position_NED_origin_float boolean Vector3f'Null +singleton AP_AHRS method get_relative_position_NED_origin_float rename get_relative_position_NED_origin + singleton AP_AHRS method get_relative_position_D_home void float'Ref singleton AP_AHRS method home_is_set boolean singleton AP_AHRS method healthy boolean From 8d2c4b14476a5d113d8b885d1ecb3aa2a7d1bc9d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 11/34] AP_SmartRTL: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_SmartRTL/AP_SmartRTL.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp index ed25eee43b4c30..c7fb855ede8067 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp +++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp @@ -201,7 +201,7 @@ bool AP_SmartRTL::peek_point(Vector3f& point) void AP_SmartRTL::set_home(bool position_ok) { Vector3f current_pos; - position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos); + position_ok &= AP::ahrs().get_relative_position_NED_origin_float(current_pos); set_home(position_ok, current_pos); } @@ -250,7 +250,7 @@ void AP_SmartRTL::update(bool position_ok, bool save_position) } Vector3f current_pos; - position_ok &= AP::ahrs().get_relative_position_NED_origin(current_pos); + position_ok &= AP::ahrs().get_relative_position_NED_origin_float(current_pos); update(position_ok, current_pos); } From c0dd0d9922b5765558c3b63b3887d02d4553fffd Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 12/34] AP_VisualOdom: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp index 744fac017b5147..a770bec29cd312 100644 --- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp +++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp @@ -224,7 +224,7 @@ bool AP_VisualOdom_IntelT265::align_position_to_ahrs(const Vector3f &sensor_pos, { // fail immediately if ahrs cannot provide position Vector3f ahrs_pos_ned; - if (!AP::ahrs().get_relative_position_NED_origin(ahrs_pos_ned)) { + if (!AP::ahrs().get_relative_position_NED_origin_float(ahrs_pos_ned)) { return false; } From 77f185ef8b00c933431f911e90eb98cf98122eb3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 13/34] AR_WPNav: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/AR_WPNav/AR_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index a7adbc78238698..bb540b857cfc41 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -405,7 +405,7 @@ void AR_WPNav::advance_wp_target_along_track(const Location ¤t_loc, float // exit immediately if no current location, destination or disarmed Vector2f curr_pos_NE; Vector3f curr_vel_NED; - if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { return; } From e6fc2188ce78de65cf82317666f14302743898ac Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 14/34] GCS_MAVLink: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- libraries/GCS_MAVLink/GCS_Common.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e4f5ce57afd0ba..43403b84b09ebc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -2981,7 +2981,7 @@ void GCS_MAVLINK::send_local_position() const const AP_AHRS &ahrs = AP::ahrs(); Vector3f local_position, velocity; - if (!ahrs.get_relative_position_NED_origin(local_position) || + if (!ahrs.get_relative_position_NED_origin_float(local_position) || !ahrs.get_velocity_NED(velocity)) { // we don't know the position and velocity return; From 733c61c2240926e1107f2d4c3067488c1bee903b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:39 +1000 Subject: [PATCH 15/34] ArduSub: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- ArduSub/Sub.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index d1a14aeb8e8565..2c581d30285da8 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -430,7 +430,7 @@ float Sub::get_alt_rel() const // get relative position float posD; - if (ahrs.get_relative_position_D_origin(posD)) { + if (ahrs.get_relative_position_D_origin_float(posD)) { if (ahrs.home_is_set()) { // adjust to the home position auto home = ahrs.get_home(); @@ -459,7 +459,7 @@ float Sub::get_alt_msl() const // get relative position float posD; - if (!ahrs.get_relative_position_D_origin(posD)) { + if (!ahrs.get_relative_position_D_origin_float(posD)) { // fall back to the barometer reading posD = -AP::baro().get_altitude(); } From 892a16c6c0614e47a8719a251841e175b6dbcdc2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:40 +1000 Subject: [PATCH 16/34] Blimp: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- Blimp/Blimp.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Blimp/Blimp.cpp b/Blimp/Blimp.cpp index 8ae8c0b704aa39..33daed95511bbb 100644 --- a/Blimp/Blimp.cpp +++ b/Blimp/Blimp.cpp @@ -223,7 +223,7 @@ void Blimp::read_AHRS(void) ahrs.update(true); IGNORE_RETURN(ahrs.get_velocity_NED(vel_ned)); - IGNORE_RETURN(ahrs.get_relative_position_NED_origin(pos_ned)); + IGNORE_RETURN(ahrs.get_relative_position_NED_origin_float(pos_ned)); vel_yaw = ahrs.get_yaw_rate_earth(); Vector2f vel_xy_filtd = vel_xy_filter.apply({vel_ned.x, vel_ned.y}); From a3976e78d914d4b7cb39a4442a175233c39a6178 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 4 May 2025 09:29:40 +1000 Subject: [PATCH 17/34] Rover: rename get_relative_position_NED_origin methods to include 'float' can't have both postype and these methods, something has to shift names --- Rover/mode_circle.cpp | 8 ++++---- Rover/mode_dock.cpp | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/Rover/mode_circle.cpp b/Rover/mode_circle.cpp index da4ce7f64a681f..71f65193575760 100644 --- a/Rover/mode_circle.cpp +++ b/Rover/mode_circle.cpp @@ -82,7 +82,7 @@ bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir bool ModeCircle::_enter() { // capture starting point and yaw - if (!AP::ahrs().get_relative_position_NE_origin(config.center_pos)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(config.center_pos)) { return false; } config.radius = MAX(fabsf(radius), AR_CIRCLE_RADIUS_MIN); @@ -129,7 +129,7 @@ void ModeCircle::init_target_yaw_rad() { // if no position estimate use vehicle yaw Vector2f curr_pos_NE; - if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) { target.yaw_rad = AP::ahrs().get_yaw(); return; } @@ -150,7 +150,7 @@ void ModeCircle::update() { // get current position Vector2f curr_pos; - const bool position_ok = AP::ahrs().get_relative_position_NE_origin(curr_pos); + const bool position_ok = AP::ahrs().get_relative_position_NE_origin_float(curr_pos); // if no position estimate stop vehicle if (!position_ok) { @@ -242,7 +242,7 @@ void ModeCircle::update_circling() float ModeCircle::wp_bearing() const { Vector2f curr_pos_NE; - if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE)) { return 0; } // calc vector from circle center to vehicle diff --git a/Rover/mode_dock.cpp b/Rover/mode_dock.cpp index 52de65a55f4692..e89071414e1eb8 100644 --- a/Rover/mode_dock.cpp +++ b/Rover/mode_dock.cpp @@ -248,7 +248,7 @@ float ModeDock::apply_slowdown(float desired_speed) // we can calculate it based on most recent value from precland because the dock is assumed stationary wrt ekf origin bool ModeDock::calc_dock_pos_rel_vehicle_NE(Vector2f &dock_pos_rel_vehicle) const { Vector2f current_pos_m; - if (!AP::ahrs().get_relative_position_NE_origin(current_pos_m)) { + if (!AP::ahrs().get_relative_position_NE_origin_float(current_pos_m)) { return false; } From 46f3d8c799c1d583aa8d5c29f755384959c6832a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 May 2025 12:54:32 +1000 Subject: [PATCH 18/34] AP_AHRS: allow AHRS to return postype for position-relative-to-origin --- libraries/AP_AHRS/AP_AHRS.cpp | 18 ++++++++-------- libraries/AP_AHRS/AP_AHRS.h | 30 +++++++++++++++++++++++--- libraries/AP_AHRS/AP_AHRS_Backend.h | 6 +++--- libraries/AP_AHRS/AP_AHRS_DCM.cpp | 12 +++++------ libraries/AP_AHRS/AP_AHRS_DCM.h | 6 +++--- libraries/AP_AHRS/AP_AHRS_External.cpp | 10 ++++----- libraries/AP_AHRS/AP_AHRS_External.h | 6 +++--- libraries/AP_AHRS/AP_AHRS_SIM.cpp | 12 +++++------ libraries/AP_AHRS/AP_AHRS_SIM.h | 6 +++--- 9 files changed, 65 insertions(+), 41 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index 3fbd6916f5d4c3..a1847ab63b0392 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -1712,7 +1712,7 @@ bool AP_AHRS::get_hagl(float &height) const /* return a relative NED position from the origin in meters */ -bool AP_AHRS::get_relative_position_NED_origin_float(Vector3f &vec) const +bool AP_AHRS::get_relative_position_NED_origin(Vector3p &vec) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1721,8 +1721,8 @@ bool AP_AHRS::get_relative_position_NED_origin_float(Vector3f &vec) const #endif #if HAL_NAVEKF2_AVAILABLE case EKFType::TWO: { - Vector2f posNE; - float posD; + Vector2p posNE; + postype_t posD; if (EKF2.getPosNE(posNE) && EKF2.getPosD(posD)) { // position is valid vec.x = posNE.x; @@ -1736,8 +1736,8 @@ bool AP_AHRS::get_relative_position_NED_origin_float(Vector3f &vec) const #if HAL_NAVEKF3_AVAILABLE case EKFType::THREE: { - Vector2f posNE; - float posD; + Vector2p posNE; + postype_t posD; if (EKF3.getPosNE(posNE) && EKF3.getPosD(posD)) { // position is valid vec.x = posNE.x; @@ -1780,7 +1780,7 @@ bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const /* return a relative position estimate from the origin in meters */ -bool AP_AHRS::get_relative_position_NE_origin_float(Vector2f &posNE) const +bool AP_AHRS::get_relative_position_NE_origin(Vector2p &posNE) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1836,7 +1836,7 @@ bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const /* return a relative ground position from the origin in meters, down */ -bool AP_AHRS::get_relative_position_D_origin_float(float &posD) const +bool AP_AHRS::get_relative_position_D_origin(postype_t &posD) const { switch (active_EKF_type()) { #if AP_AHRS_DCM_ENABLED @@ -1883,8 +1883,8 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const } Location originLLH; - float originD; - if (!get_relative_position_D_origin_float(originD) || + postype_t originD; + if (!get_relative_position_D_origin(originD) || !_get_origin(originLLH)) { #if AP_GPS_ENABLED const auto &gps = AP::gps(); diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 981cf50b392fbe..b0915a597dcc6b 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -269,17 +269,41 @@ class AP_AHRS { // return the relative position NED from either home or origin // return true if the estimate is valid bool get_relative_position_NED_home(Vector3f &vec) const WARN_IF_UNUSED; - bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED; + bool get_relative_position_NED_origin(Vector3p &vec) const WARN_IF_UNUSED; + bool get_relative_position_NED_origin_float(Vector3f &vec) const WARN_IF_UNUSED { + Vector3p tmp_posNED; + if (!get_relative_position_NED_origin(tmp_posNED)) { + return false; + } + vec = tmp_posNED.tofloat(); + return true; + } // return the relative position NE from home or origin // return true if the estimate is valid bool get_relative_position_NE_home(Vector2f &posNE) const WARN_IF_UNUSED; - bool get_relative_position_NE_origin_float(Vector2f &posNE) const WARN_IF_UNUSED; + bool get_relative_position_NE_origin(Vector2p &posNE) const WARN_IF_UNUSED; + bool get_relative_position_NE_origin_float(Vector2f &posNE) const WARN_IF_UNUSED { + Vector2p tmp_posNE; + if (!get_relative_position_NE_origin(tmp_posNE)) { + return false; + } + posNE = tmp_posNE.tofloat(); + return true; + } // return the relative position down from home or origin // baro will be used for the _home relative one if the EKF isn't void get_relative_position_D_home(float &posD) const; - bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED; + bool get_relative_position_D_origin(postype_t &posD) const WARN_IF_UNUSED; + bool get_relative_position_D_origin_float(float &posD) const WARN_IF_UNUSED { + postype_t tmp_posD; + if (!get_relative_position_D_origin(tmp_posD)) { + return false; + } + posD = float(tmp_posD); + return true; + } // return location corresponding to vector relative to the // vehicle's origin diff --git a/libraries/AP_AHRS/AP_AHRS_Backend.h b/libraries/AP_AHRS/AP_AHRS_Backend.h index fc5d1ace5a81c2..cf7e4f4bd5ba91 100644 --- a/libraries/AP_AHRS/AP_AHRS_Backend.h +++ b/libraries/AP_AHRS/AP_AHRS_Backend.h @@ -185,19 +185,19 @@ class AP_AHRS_Backend // return a position relative to origin in meters, North/East/Down // order. This will only be accurate if have_inertial_nav() is // true - virtual bool get_relative_position_NED_origin(Vector3f &vec) const WARN_IF_UNUSED { + virtual bool get_relative_position_NED_origin(Vector3p &vec) const WARN_IF_UNUSED { return false; } // return a position relative to origin in meters, North/East // order. Return true if estimate is valid - virtual bool get_relative_position_NE_origin(Vector2f &vecNE) const WARN_IF_UNUSED { + virtual bool get_relative_position_NE_origin(Vector2p &vecNE) const WARN_IF_UNUSED { return false; } // return a Down position relative to origin in meters // Return true if estimate is valid - virtual bool get_relative_position_D_origin(float &posD) const WARN_IF_UNUSED { + virtual bool get_relative_position_D_origin(postype_t &posD) 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 e11e08953097a9..cf38e7c897495f 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1282,7 +1282,7 @@ bool AP_AHRS_DCM::get_origin(Location &ret) const return !ret.is_zero(); } -bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3f &posNED) const +bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3p &posNED) const { Location origin; if (!AP_AHRS_DCM::get_origin(origin)) { @@ -1292,13 +1292,13 @@ bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3f &posNED) const if (!AP_AHRS_DCM::get_location(loc)) { return false; } - posNED = origin.get_distance_NED(loc); + posNED = origin.get_distance_NED_postype(loc); return true; } -bool AP_AHRS_DCM::get_relative_position_NE_origin(Vector2f &posNE) const +bool AP_AHRS_DCM::get_relative_position_NE_origin(Vector2p &posNE) const { - Vector3f posNED; + Vector3p posNED; if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) { return false; } @@ -1306,9 +1306,9 @@ bool AP_AHRS_DCM::get_relative_position_NE_origin(Vector2f &posNE) const return true; } -bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const +bool AP_AHRS_DCM::get_relative_position_D_origin(postype_t &posD) const { - Vector3f posNED; + Vector3p posNED; if (!AP_AHRS_DCM::get_relative_position_NED_origin(posNED)) { return false; } diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 8f9288d014ba87..5db9a276ffe3d4 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -122,9 +122,9 @@ class AP_AHRS_DCM : public AP_AHRS_Backend { // relative-origin functions for fallback in AP_InertialNav bool get_origin(Location &ret) const override; - bool get_relative_position_NED_origin(Vector3f &vec) const override; - bool get_relative_position_NE_origin(Vector2f &posNE) const override; - bool get_relative_position_D_origin(float &posD) const override; + bool get_relative_position_NED_origin(Vector3p &vec) const override; + bool get_relative_position_NE_origin(Vector2p &posNE) const override; + bool get_relative_position_D_origin(postype_t &posD) const override; void send_ekf_status_report(class GCS_MAVLINK &link) const override; diff --git a/libraries/AP_AHRS/AP_AHRS_External.cpp b/libraries/AP_AHRS/AP_AHRS_External.cpp index 435bda69b471a9..c7b2cf70f39bfb 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.cpp +++ b/libraries/AP_AHRS/AP_AHRS_External.cpp @@ -59,21 +59,21 @@ Vector2f AP_AHRS_External::groundspeed_vector() } -bool AP_AHRS_External::get_relative_position_NED_origin(Vector3f &vec) const +bool AP_AHRS_External::get_relative_position_NED_origin(Vector3p &vec) const { auto &extahrs = AP::externalAHRS(); Location loc, orgn; if (extahrs.get_origin(orgn) && extahrs.get_location(loc)) { const Vector2f diff2d = orgn.get_distance_NE(loc); - vec = Vector3f(diff2d.x, diff2d.y, + vec = Vector3p(diff2d.x, diff2d.y, -(loc.alt - orgn.alt)*0.01); return true; } return false; } -bool AP_AHRS_External::get_relative_position_NE_origin(Vector2f &posNE) const +bool AP_AHRS_External::get_relative_position_NE_origin(Vector2p &posNE) const { auto &extahrs = AP::externalAHRS(); @@ -82,11 +82,11 @@ bool AP_AHRS_External::get_relative_position_NE_origin(Vector2f &posNE) const !extahrs.get_origin(orgn)) { return false; } - posNE = orgn.get_distance_NE(loc); + posNE = orgn.get_distance_NE_postype(loc); return true; } -bool AP_AHRS_External::get_relative_position_D_origin(float &posD) const +bool AP_AHRS_External::get_relative_position_D_origin(postype_t &posD) const { auto &extahrs = AP::externalAHRS(); diff --git a/libraries/AP_AHRS/AP_AHRS_External.h b/libraries/AP_AHRS/AP_AHRS_External.h index 6ca69f73f8f4ac..c0e421aeedb017 100644 --- a/libraries/AP_AHRS/AP_AHRS_External.h +++ b/libraries/AP_AHRS/AP_AHRS_External.h @@ -79,9 +79,9 @@ class AP_AHRS_External : public AP_AHRS_Backend { // relative-origin functions for fallback in AP_InertialNav bool get_origin(Location &ret) const override; - bool get_relative_position_NED_origin(Vector3f &vec) const override; - bool get_relative_position_NE_origin(Vector2f &posNE) const override; - bool get_relative_position_D_origin(float &posD) const override; + bool get_relative_position_NED_origin(Vector3p &vec) const override; + bool get_relative_position_NE_origin(Vector2p &posNE) const override; + bool get_relative_position_D_origin(postype_t &posD) const override; bool get_filter_status(nav_filter_status &status) const override; void send_ekf_status_report(class GCS_MAVLINK &link) const override; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.cpp b/libraries/AP_AHRS/AP_AHRS_SIM.cpp index 82fb6ead9823ea..734ff7364bef77 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_SIM.cpp @@ -102,7 +102,7 @@ bool AP_AHRS_SIM::get_hagl(float &height) const return true; } -bool AP_AHRS_SIM::get_relative_position_NED_origin(Vector3f &vec) const +bool AP_AHRS_SIM::get_relative_position_NED_origin(Vector3p &vec) const { if (_sitl == nullptr) { return false; @@ -114,27 +114,27 @@ bool AP_AHRS_SIM::get_relative_position_NED_origin(Vector3f &vec) const return false; } - const Vector2f diff2d = orgn.get_distance_NE(loc); + const Vector2p diff2d = orgn.get_distance_NE_postype(loc); const struct SITL::sitl_fdm &fdm = _sitl->state; - vec = Vector3f(diff2d.x, diff2d.y, + vec = Vector3p(diff2d.x, diff2d.y, -(fdm.altitude - orgn.alt*0.01f)); return true; } -bool AP_AHRS_SIM::get_relative_position_NE_origin(Vector2f &posNE) const +bool AP_AHRS_SIM::get_relative_position_NE_origin(Vector2p &posNE) const { Location loc, orgn; if (!get_location(loc) || !get_origin(orgn)) { return false; } - posNE = orgn.get_distance_NE(loc); + posNE = orgn.get_distance_NE_postype(loc); return true; } -bool AP_AHRS_SIM::get_relative_position_D_origin(float &posD) const +bool AP_AHRS_SIM::get_relative_position_D_origin(postype_t &posD) const { if (_sitl == nullptr) { return false; diff --git a/libraries/AP_AHRS/AP_AHRS_SIM.h b/libraries/AP_AHRS/AP_AHRS_SIM.h index 9fa8506414c3ec..4da08df41cab00 100644 --- a/libraries/AP_AHRS/AP_AHRS_SIM.h +++ b/libraries/AP_AHRS/AP_AHRS_SIM.h @@ -100,9 +100,9 @@ class AP_AHRS_SIM : public AP_AHRS_Backend { // relative-origin functions for fallback in AP_InertialNav bool get_origin(Location &ret) const override; - bool get_relative_position_NED_origin(Vector3f &vec) const override; - bool get_relative_position_NE_origin(Vector2f &posNE) const override; - bool get_relative_position_D_origin(float &posD) const override; + bool get_relative_position_NED_origin(Vector3p &vec) const override; + bool get_relative_position_NE_origin(Vector2p &posNE) const override; + bool get_relative_position_D_origin(postype_t &posD) const override; void send_ekf_status_report(class GCS_MAVLINK &link) const override; From b9eb6a74d585d8228269346d37dd5b7be4802b27 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 May 2025 12:54:32 +1000 Subject: [PATCH 19/34] AP_Common: allow AHRS to return postype for position-relative-to-origin --- libraries/AP_Common/Location.cpp | 13 +++++++++++++ libraries/AP_Common/Location.h | 2 ++ 2 files changed, 15 insertions(+) diff --git a/libraries/AP_Common/Location.cpp b/libraries/AP_Common/Location.cpp index 112e6182665b79..330a9709b7a11c 100644 --- a/libraries/AP_Common/Location.cpp +++ b/libraries/AP_Common/Location.cpp @@ -375,6 +375,13 @@ Vector3f Location::get_distance_NED(const Location &loc2) const (alt - loc2.alt) * 0.01); } +Vector3p Location::get_distance_NED_postype(const Location &loc2) const +{ + return Vector3p((loc2.lat - lat) * LOCATION_SCALING_FACTOR, + diff_longitude(loc2.lng,lng) * LOCATION_SCALING_FACTOR * longitude_scale((lat+loc2.lat)/2), + (alt - loc2.alt) * 0.01); +} + // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 Vector3d Location::get_distance_NED_double(const Location &loc2) const { @@ -402,6 +409,12 @@ Vector2d Location::get_distance_NE_double(const Location &loc2) const diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2)); } +Vector2p Location::get_distance_NE_postype(const Location &loc2) const +{ + return Vector2p((loc2.lat - lat) * double(LOCATION_SCALING_FACTOR), + diff_longitude(loc2.lng,lng) * double(LOCATION_SCALING_FACTOR) * longitude_scale((lat+loc2.lat)/2)); +} + Vector2F Location::get_distance_NE_ftype(const Location &loc2) const { return Vector2F((loc2.lat - lat) * ftype(LOCATION_SCALING_FACTOR), diff --git a/libraries/AP_Common/Location.h b/libraries/AP_Common/Location.h index 2e09b956328aa7..66f3d03a4aa764 100644 --- a/libraries/AP_Common/Location.h +++ b/libraries/AP_Common/Location.h @@ -101,6 +101,7 @@ class Location // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 // NOT CONSIDERING ALT FRAME! Vector3f get_distance_NED(const Location &loc2) const; + Vector3p get_distance_NED_postype(const Location &loc2) const; Vector3d get_distance_NED_double(const Location &loc2) const; // return the distance in meters in North/East/Down plane as a N/E/D vector to loc2 considering alt frame, if altitude cannot be resolved down distance is 0 @@ -108,6 +109,7 @@ class Location // return the distance in meters in North/East plane as a N/E vector to loc2 Vector2f get_distance_NE(const Location &loc2) const; + Vector2p get_distance_NE_postype(const Location &loc2) const; Vector2d get_distance_NE_double(const Location &loc2) const; Vector2F get_distance_NE_ftype(const Location &loc2) const; From d72bab6a5c15af7e738854280d8f030c1c85c79a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 May 2025 17:54:24 +1000 Subject: [PATCH 20/34] AP_NavEKF2: allow AHRS to return postype for position-relative-to-origin --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 12 +++++++----- libraries/AP_NavEKF2/AP_NavEKF2.h | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp | 4 ++-- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 10 ++++------ libraries/AP_NavEKF2/AP_NavEKF2_core.h | 4 ++-- 5 files changed, 17 insertions(+), 17 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index be95fcbc742a7c..4fc4dbc69d6b1b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -925,7 +925,7 @@ int8_t NavEKF2::getPrimaryCoreIMUIndex(void) const // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF2::getPosNE(Vector2f &posNE) const +bool NavEKF2::getPosNE(Vector2p &posNE) const { if (!core) { return false; @@ -936,7 +936,7 @@ bool NavEKF2::getPosNE(Vector2f &posNE) const // Write the last calculated D position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF2::getPosD(float &posD) const +bool NavEKF2::getPosD(postype_t &posD) const { if (!core) { return false; @@ -1460,7 +1460,8 @@ void NavEKF2::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_prim // update the position reset data to capture changes due to a lane switch void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary) { - Vector2f pos_old_primary, pos_new_primary, old_pos_delta; + Vector2p pos_old_primary, pos_new_primary; + Vector2f old_pos_delta; // If core position reset data has been consumed reset delta to zero if (!pos_reset_data.core_changed) { @@ -1476,7 +1477,7 @@ void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim // Add current delta in case it hasn't been consumed yet core[old_primary].getPosNE(pos_old_primary); core[new_primary].getPosNE(pos_new_primary); - pos_reset_data.core_delta = pos_new_primary - pos_old_primary + pos_reset_data.core_delta; + pos_reset_data.core_delta = (pos_new_primary - pos_old_primary).tofloat() + pos_reset_data.core_delta; pos_reset_data.last_primary_change = imuSampleTime_us / 1000; pos_reset_data.core_changed = true; @@ -1487,7 +1488,8 @@ void NavEKF2::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim // new primary EKF update has been run void NavEKF2::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary) { - float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta; + postype_t posDownOldPrimary, posDownNewPrimary; + float oldPosDownDelta; // If core position reset data has been consumed reset delta to zero if (!pos_down_reset_data.core_changed) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index dbf1a51dbc0374..f508c06d7ea18e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -69,12 +69,12 @@ class NavEKF2 { // Write the last calculated NE position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(Vector2f &posNE) const; + bool getPosNE(Vector2p &posNE) const; // Write the last calculated D position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(float &posD) const; + bool getPosD(postype_t &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp index 73874b775f95cf..2560b97d32d7fc 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Logging.cpp @@ -12,8 +12,8 @@ void NavEKF2_core::Log_Write_NKF1(uint64_t time_us) const { // Write first EKF packet Vector3f euler; - Vector2f posNE; - float posD; + Vector2p posNE; + postype_t posD; Vector3f velNED; Vector3f gyroBias; float posDownDeriv; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 7f209b6b28d25e..df4e87cb4af2e0 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -209,7 +209,7 @@ void NavEKF2_core::getAccelZBias(float &zbias) const { // Write the last estimated NE position of the body frame origin relative to the reference point (m). // Return true if the estimate is valid -bool NavEKF2_core::getPosNE(Vector2f &posNE) const +bool NavEKF2_core::getPosNE(Vector2p &posNE) const { // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) if (PV_AidingMode != AID_NONE) { @@ -225,9 +225,7 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const if ((dal.gps().status(dal.gps().primary_sensor()) >= AP_DAL_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const Location &gpsloc = dal.gps().location(); - const Vector2F tempPosNE = EKF_origin.get_distance_NE_ftype(gpsloc); - posNE.x = tempPosNE.x; - posNE.y = tempPosNE.y; + posNE = EKF_origin.get_distance_NE_postype(gpsloc); return false; } else if (rngBcnAlignmentStarted) { // If we are attempting alignment using range beacon data, then report the position @@ -252,7 +250,7 @@ bool NavEKF2_core::getPosNE(Vector2f &posNE) const // Write the last calculated D position of the body frame origin relative to the EKF origin (m). // Return true if the estimate is valid -bool NavEKF2_core::getPosD(float &posD) const +bool NavEKF2_core::getPosD(postype_t &posD) const { // The EKF always has a height estimate regardless of mode of operation // Correct for the IMU offset in body frame (EKF calculations are at the IMU) @@ -288,7 +286,7 @@ bool NavEKF2_core::getLLH(Location &loc) const { const auto &gps = dal.gps(); Location origin; - float posD; + postype_t posD; if(getPosD(posD) && getOriginLLH(origin)) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 0dc9c6881908f8..8ef86948fa8621 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -108,12 +108,12 @@ class NavEKF2_core : public NavEKF_core_common // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(Vector2f &posNE) const; + bool getPosNE(Vector2p &posNE) const; // Write the last calculated D position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(float &posD) const; + bool getPosD(postype_t &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; From 626f1c522f9b5804736c62054cc3b71618d78ab6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 May 2025 17:54:24 +1000 Subject: [PATCH 21/34] AP_NavEKF3: allow AHRS to return postype for position-relative-to-origin --- libraries/AP_NavEKF3/AP_NavEKF3.cpp | 12 +++++++----- libraries/AP_NavEKF3/AP_NavEKF3.h | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp | 4 ++-- libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp | 14 +++++++------- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 6 +++--- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 2240ac46164b72..d8eaaecccf7caf 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1206,7 +1206,7 @@ int8_t NavEKF3::getPrimaryCoreIMUIndex(void) const // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF3::getPosNE(Vector2f &posNE) const +bool NavEKF3::getPosNE(Vector2p &posNE) const { if (!core) { return false; @@ -1217,7 +1217,7 @@ bool NavEKF3::getPosNE(Vector2f &posNE) const // Write the last calculated D position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control -bool NavEKF3::getPosD(float &posD) const +bool NavEKF3::getPosD(postype_t &posD) const { if (!core) { return false; @@ -2019,7 +2019,8 @@ void NavEKF3::updateLaneSwitchYawResetData(uint8_t new_primary, uint8_t old_prim // update the position reset data to capture changes due to a lane switch void NavEKF3::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_primary) { - Vector2f pos_old_primary, pos_new_primary, old_pos_delta; + Vector2p pos_old_primary, pos_new_primary; + Vector2f old_pos_delta; // If core position reset data has been consumed reset delta to zero if (!pos_reset_data.core_changed) { @@ -2035,7 +2036,7 @@ void NavEKF3::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim // Add current delta in case it hasn't been consumed yet core[old_primary].getPosNE(pos_old_primary); core[new_primary].getPosNE(pos_new_primary); - pos_reset_data.core_delta = pos_new_primary - pos_old_primary + pos_reset_data.core_delta; + pos_reset_data.core_delta = (pos_new_primary - pos_old_primary).tofloat() + pos_reset_data.core_delta; pos_reset_data.last_primary_change = imuSampleTime_us / 1000; pos_reset_data.core_changed = true; @@ -2046,7 +2047,8 @@ void NavEKF3::updateLaneSwitchPosResetData(uint8_t new_primary, uint8_t old_prim // new primary EKF update has been run void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_primary) { - float posDownOldPrimary, posDownNewPrimary, oldPosDownDelta; + postype_t posDownOldPrimary, posDownNewPrimary; + float oldPosDownDelta; // If core position reset data has been consumed reset delta to zero if (!pos_down_reset_data.core_changed) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index ee3984aa2d9f1e..846fbf2a1c42d5 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -69,12 +69,12 @@ class NavEKF3 { // Write the last calculated NE position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(Vector2f &posNE) const; + bool getPosNE(Vector2p &posNE) const; // Write the last calculated D position relative to the reference point (m) // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(float &posD) const; + bool getPosD(postype_t &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp index 50fa12558d2969..bcdd24065bf651 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp @@ -16,8 +16,8 @@ void NavEKF3_core::Log_Write_XKF1(uint64_t time_us) const { // Write first EKF packet Vector3f euler; - Vector2f posNE; - float posD; + Vector2p posNE; + postype_t posD; Vector3f velNED; Vector3f gyroBias; float posDownDeriv; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index b91a2327f7cdb0..ec8a8c0322e8af 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -246,13 +246,13 @@ float NavEKF3_core::getPosDownDerivative(void) const // Write the last estimated NE position of the body frame origin relative to the reference point (m). // Return true if the estimate is valid -bool NavEKF3_core::getPosNE(Vector2f &posNE) const +bool NavEKF3_core::getPosNE(Vector2p &posNE) const { // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) if (PV_AidingMode != AID_NONE) { // This is the normal mode of operation where we can use the EKF position states // correct for the IMU offset (EKF calculations are at the IMU) - posNE = (outputDataNew.position.xy() + posOffsetNED.xy() + public_origin.get_distance_NE_ftype(EKF_origin)).tofloat(); + posNE = outputDataNew.position.xy().topostype() + posOffsetNED.xy().topostype() + public_origin.get_distance_NE_postype(EKF_origin); return true; } else { @@ -262,7 +262,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const if ((gps.status(selected_gps) >= AP_DAL_GPS::GPS_OK_FIX_2D)) { // If the origin has been set and we have GPS, then return the GPS position relative to the origin const Location &gpsloc = gps.location(selected_gps); - posNE = public_origin.get_distance_NE_ftype(gpsloc).tofloat(); + posNE = public_origin.get_distance_NE_postype(gpsloc); return false; #if EK3_FEATURE_BEACON_FUSION } else if (rngBcn.alignmentStarted) { @@ -273,7 +273,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const #endif } else { // If no GPS fix is available, all we can do is provide the last known position - posNE = outputDataNew.position.xy().tofloat(); + posNE = outputDataNew.position.xy().topostype(); return false; } } else { @@ -287,7 +287,7 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const // Write the last calculated D position of the body frame origin relative to the EKF local origin // Return true if the estimate is valid -bool NavEKF3_core::getPosD_local(float &posD) const +bool NavEKF3_core::getPosD_local(postype_t &posD) const { posD = outputDataNew.position.z + posOffsetNED.z; @@ -298,7 +298,7 @@ bool NavEKF3_core::getPosD_local(float &posD) const // Write the last calculated D position of the body frame origin relative to the public origin // Return true if the estimate is valid -bool NavEKF3_core::getPosD(float &posD) const +bool NavEKF3_core::getPosD(postype_t &posD) const { bool ret = getPosD_local(posD); @@ -327,7 +327,7 @@ bool NavEKF3_core::getLLH(Location &loc) const { Location origin; if (getOriginLLH(origin)) { - float posD; + postype_t posD; if (getPosD_local(posD) && PV_AidingMode != AID_NONE) { // Altitude returned is an absolute altitude relative to the WGS-84 spherioid loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 2212dfda0e5f9b..d2c575405d9062 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -151,15 +151,15 @@ class NavEKF3_core : public NavEKF_core_common // Write the last calculated NE position relative to the reference point (m). // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosNE(Vector2f &posNE) const; + bool getPosNE(Vector2p &posNE) const; // get position D from local origin - bool getPosD_local(float &posD) const; + bool getPosD_local(postype_t &posD) const; // Write the last calculated D position relative to the public origin // If a calculated solution is not available, use the best available data and return false // If false returned, do not use for flight control - bool getPosD(float &posD) const; + bool getPosD(postype_t &posD) const; // return NED velocity in m/s void getVelNED(Vector3f &vel) const; From 830ba07e4373647892436885365e336cc519eca3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 11 May 2025 21:10:30 +0930 Subject: [PATCH 22/34] AC_PID: AC_P_+2D: use Vector2p input squash PID --- libraries/AC_PID/AC_P_2D.cpp | 7 +++---- libraries/AC_PID/AC_P_2D.h | 8 +------- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp index faac93b7d0795a..ebb96ec4e1b57e 100644 --- a/libraries/AC_PID/AC_P_2D.cpp +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -21,16 +21,15 @@ AC_P_2D::AC_P_2D(float initial_p) : } // update_all - set target and measured inputs to P controller and calculate outputs -Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) +Vector2f AC_P_2D::update_all(Vector2p &target, const Vector2p &measurement) { // calculate distance _error - _error = (Vector2p{target_x, target_y} - measurement.topostype()).tofloat(); + _error = (target - measurement).tofloat(); // Constrain _error and target position // Constrain the maximum length of _vel_target to the maximum position correction velocity if (is_positive(_error_max) && _error.limit_length(_error_max)) { - target_x = measurement.x + _error.x; - target_y = measurement.y + _error.y; + target = measurement + _error.topostype(); } // MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h index dad0168e25bf1a..4b7710d34af17f 100644 --- a/libraries/AC_PID/AC_P_2D.h +++ b/libraries/AC_PID/AC_P_2D.h @@ -17,13 +17,7 @@ class AC_P_2D { CLASS_NO_COPY(AC_P_2D); // set target and measured inputs to P controller and calculate outputs - Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) WARN_IF_UNUSED; - - // set target and measured inputs to P controller and calculate outputs - // measurement is provided as 3-axis vector but only x and y are used - Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector3f &measurement) WARN_IF_UNUSED { - return update_all(target_x, target_y, Vector2f{measurement.x, measurement.y}); - } + Vector2f update_all(Vector2p &target, const Vector2p &measurement) WARN_IF_UNUSED; // set_limits - sets the maximum error to limit output and first and second derivative of output void set_limits(float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f); From 2a98b223512d77c7399f94eb187dc6fab4500750 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 11 May 2025 21:12:09 +0930 Subject: [PATCH 23/34] APM_Control: Use NED Vector2p input to AC_P_2D --- libraries/APM_Control/AR_PosControl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/APM_Control/AR_PosControl.cpp b/libraries/APM_Control/AR_PosControl.cpp index d785310c366916..c359649b95b146 100644 --- a/libraries/APM_Control/AR_PosControl.cpp +++ b/libraries/APM_Control/AR_PosControl.cpp @@ -114,9 +114,9 @@ AR_PosControl::AR_PosControl(AR_AttitudeControl& atc) : void AR_PosControl::update(float dt) { // exit immediately if no current location, destination or disarmed - Vector2f curr_pos_NE; + Vector3p curr_pos_NED_m; Vector3f curr_vel_NED; - if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NE_origin_float(curr_pos_NE) || + if (!hal.util->get_soft_armed() || !AP::ahrs().get_relative_position_NED_origin(curr_pos_NED_m) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) { _desired_speed = _atc.get_desired_speed_accel_limited(0.0f, dt); _desired_lat_accel = 0.0f; @@ -138,7 +138,7 @@ void AR_PosControl::update(float dt) _vel_target.zero(); if (_pos_target_valid) { Vector2p pos_target = _pos_target; - _vel_target = _p_pos.update_all(pos_target.x, pos_target.y, curr_pos_NE); + _vel_target = _p_pos.update_all(pos_target, curr_pos_NED_m.xy()); } // calculation velocity error From c2e57ad8ef96c1ca6337f71eb28566a3e398b2c5 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 11 May 2025 21:13:06 +0930 Subject: [PATCH 24/34] AC_AttitudeControl: AC_PosControl: Remove INAV --- .../AC_AttitudeControl/AC_PosControl.cpp | 99 ++++++++++++------- libraries/AC_AttitudeControl/AC_PosControl.h | 15 ++- 2 files changed, 71 insertions(+), 43 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 9a88631a799d53..547e89bd2804d2 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -330,10 +330,8 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const AP_Motors& motors, AC_AttitudeControl& attitude_control) : +AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_AttitudeControl& attitude_control) : _ahrs(ahrs), - _inav(inav), _motors(motors), _attitude_control(attitude_control), _p_pos_ne(POSCONTROL_POS_XY_P), @@ -416,7 +414,7 @@ float AC_PosControl::pos_terrain_U_scaler(float pos_terrain_u_cm, float pos_terr if (is_zero(pos_terrain_u_buffer_cm)) { return 1.0; } - float pos_offset_error_u_cm = _inav.get_position_z_up_cm() - (_pos_target_neu_cm.z + (pos_terrain_u_cm - _pos_terrain_u_cm)); + float pos_offset_error_u_cm = _pos_estimate_neu_cm.z - (_pos_target_neu_cm.z + (pos_terrain_u_cm - _pos_terrain_u_cm)); return constrain_float((1.0 - (fabsf(pos_offset_error_u_cm) - 0.5 * pos_terrain_u_buffer_cm) / (0.5 * pos_terrain_u_buffer_cm)), 0.01, 1.0); } @@ -490,7 +488,7 @@ void AC_PosControl::soften_for_landing_NE() { // decay position error to zero if (is_positive(_dt)) { - _pos_target_neu_cm.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target_neu_cm.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); + _pos_target_neu_cm.xy() += (_pos_estimate_neu_cm.xy() - _pos_target_neu_cm.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); } @@ -514,10 +512,10 @@ void AC_PosControl::init_NE_controller() _yaw_rate_target_cds = 0.0f; _angle_max_override_cd = 0.0; - _pos_target_neu_cm.xy() = _inav.get_position_xy_cm().topostype(); + _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); - _vel_target_neu_cms.xy() = _inav.get_velocity_xy_cms(); + _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy(); _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); // Set desired acceleration to zero because raw acceleration is prone to noise @@ -611,17 +609,17 @@ void AC_PosControl::update_offsets_NE() /// stop_pos_NE_stabilisation - sets the target to the current position to remove any position corrections from the system void AC_PosControl::stop_pos_NE_stabilisation() { - _pos_target_neu_cm.xy() = _inav.get_position_xy_cm().topostype(); + _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); } /// stop_vel_NE_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system void AC_PosControl::stop_vel_NE_stabilisation() { - _pos_target_neu_cm.xy() = _inav.get_position_xy_cm().topostype(); + _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); - _vel_target_neu_cms.xy() = _inav.get_velocity_xy_cms();; + _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy(); _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); // initialise I terms from lean angles @@ -666,11 +664,11 @@ void AC_PosControl::update_NE_controller() _pos_target_neu_cm.xy() = _pos_desired_neu_cm.xy() + _pos_offset_neu_cm.xy(); // determine the combined position of the actual position and the disturbance from system ID mode - const Vector3f &curr_pos = _inav.get_position_neu_cm(); - Vector3f comb_pos = curr_pos; - comb_pos.xy() += _disturb_pos_ne_cm; + // calculate the target velocity correction + Vector2p comb_pos = _pos_estimate_neu_cm.xy(); + comb_pos += _disturb_pos_ne_cm.topostype(); - Vector2f vel_target = _p_pos_ne.update_all(_pos_target_neu_cm.x, _pos_target_neu_cm.y, comb_pos); + Vector2f vel_target = _p_pos_ne.update_all(_pos_target_neu_cm.xy(), comb_pos); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); // Velocity Controller @@ -683,8 +681,8 @@ void AC_PosControl::update_NE_controller() _vel_target_neu_cms.xy() += _vel_desired_neu_cms.xy() + _vel_offset_neu_cms.xy(); // determine the combined velocity of the actual velocity and the disturbance from system ID mode - const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); - Vector2f comb_vel = curr_vel; + // Velocity Controller + Vector2f comb_vel = _vel_estimate_neu_cms.xy(); comb_vel += _disturb_vel_ne_cms; Vector2f accel_target_ne_cmss = _pid_vel_ne.update_all(_vel_target_neu_cms.xy(), comb_vel, _dt, _limit_vector.xy()); @@ -821,10 +819,10 @@ void AC_PosControl::init_U_controller() // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. init_offsets_U(); - _pos_target_neu_cm.z = _inav.get_position_z_up_cm(); + _pos_target_neu_cm.z = _pos_estimate_neu_cm.z; _pos_desired_neu_cm.z = _pos_target_neu_cm.z - _pos_offset_neu_cm.z; - _vel_target_neu_cms.z = _inav.get_velocity_z_up_cms(); + _vel_target_neu_cms.z = _vel_estimate_neu_cms.z; _vel_desired_neu_cms.z = _vel_target_neu_cms.z - _vel_offset_neu_cms.z; // Reset I term of velocity PID @@ -1003,7 +1001,7 @@ void AC_PosControl::update_U_controller() // calculate the target velocity correction float pos_target_zf = _pos_target_neu_cm.z; - _vel_target_neu_cms.z = _p_pos_u.update_all(pos_target_zf, _inav.get_position_z_up_cm()); + _vel_target_neu_cms.z = _p_pos_u.update_all(pos_target_zf, _pos_estimate_neu_cm.z); _vel_target_neu_cms.z *= AP::ahrs().getControlScaleZ(); _pos_target_neu_cm.z = pos_target_zf; @@ -1014,8 +1012,7 @@ void AC_PosControl::update_U_controller() // Velocity Controller - const float curr_vel_u_cms = _inav.get_velocity_z_up_cms(); - _accel_target_neu_cmss.z = _pid_vel_u.update_all(_vel_target_neu_cms.z, curr_vel_u_cms, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper); + _accel_target_neu_cmss.z = _pid_vel_u.update_all(_vel_target_neu_cms.z, _vel_estimate_neu_cms.z, _dt, _motors.limit.throttle_lower, _motors.limit.throttle_upper); _accel_target_neu_cmss.z *= AP::ahrs().getControlScaleZ(); // add feed forward component @@ -1277,10 +1274,10 @@ Vector3f AC_PosControl::get_thrust_vector() const void AC_PosControl::get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) const { // todo: we should use the current target position and velocity if we are currently running the position controller - stopping_point_neu_cm = _inav.get_position_xy_cm().topostype(); + stopping_point_neu_cm = _pos_estimate_neu_cm.xy(); stopping_point_neu_cm -= _pos_offset_neu_cm.xy(); - Vector2f curr_vel = _inav.get_velocity_xy_cms(); + Vector2f curr_vel = _vel_estimate_neu_cms.xy(); curr_vel -= _vel_offset_neu_cms.xy(); // calculate current velocity @@ -1305,10 +1302,10 @@ void AC_PosControl::get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) co /// get_stopping_point_U_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration void AC_PosControl::get_stopping_point_U_cm(postype_t &stopping_point_u_cm) const { - float curr_pos_u_cm = _inav.get_position_z_up_cm(); + float curr_pos_u_cm = _pos_estimate_neu_cm.z; curr_pos_u_cm -= _pos_offset_neu_cm.z; - float curr_vel_u_cms = _inav.get_velocity_z_up_cms(); + float curr_vel_u_cms = _vel_estimate_neu_cms.z; curr_vel_u_cms -= _vel_offset_neu_cms.z; // avoid divide by zero by using current position if kP is very low or acceleration is zero @@ -1323,7 +1320,7 @@ void AC_PosControl::get_stopping_point_U_cm(postype_t &stopping_point_u_cm) cons /// get_bearing_to_target_cd - get bearing to target position in centi-degrees int32_t AC_PosControl::get_bearing_to_target_cd() const { - return get_bearing_cd(_inav.get_position_xy_cm(), _pos_target_neu_cm.tofloat().xy()); + return get_bearing_cd(Vector2f{0.0, 0.0}, (_pos_target_neu_cm.xy() - _pos_estimate_neu_cm.xy()).tofloat()); } @@ -1331,6 +1328,32 @@ int32_t AC_PosControl::get_bearing_to_target_cd() const /// System methods /// +// Updates internal position and velocity estimates in the NED frame. +// Falls back to vertical-only estimates if full NED data is unavailable. +// When high_vibes is true, forces use of vertical fallback for velocity. +void AC_PosControl::update_estimates(bool high_vibes) +{ + Vector3p pos_estimate_ned_m; + if (!AP::ahrs().get_relative_position_NED_origin(pos_estimate_ned_m)) { + float posD; + if (AP::ahrs().get_relative_position_D_origin_float(posD)) { + pos_estimate_ned_m.z = posD; + } + } + _pos_estimate_neu_cm.xy() = pos_estimate_ned_m.xy() * 100.0; + _pos_estimate_neu_cm.z = -pos_estimate_ned_m.z * 100.0; + + Vector3f vel_estimate_ned_ms; + if (!AP::ahrs().get_velocity_NED(vel_estimate_ned_ms) || high_vibes) { + float rate_z; + if (AP::ahrs().get_vert_pos_rate_D(rate_z)) { + vel_estimate_ned_ms.z = rate_z; + } + } + _vel_estimate_neu_cms.xy() = vel_estimate_ned_ms.xy() * 100.0; + _vel_estimate_neu_cms.z = -vel_estimate_ned_ms.z * 100.0; +} + // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) float AC_PosControl::get_throttle_with_vibration_override() { @@ -1353,7 +1376,7 @@ void AC_PosControl::standby_NEU_reset() _pid_accel_u.set_integrator(0.0f); // Set the target position to the current pos. - _pos_target_neu_cm = _inav.get_position_neu_cm().topostype(); + _pos_target_neu_cm = _pos_estimate_neu_cm; // Set _pid_vel_ne integrator and derivative to zero. _pid_vel_ne.reset_filter(); @@ -1369,11 +1392,11 @@ void AC_PosControl::write_log() if (is_active_NE()) { float accel_n_cmss, accel_e_cmss; lean_angles_to_accel_NE_cmss(accel_n_cmss, accel_e_cmss); - Write_PSCN(_pos_desired_neu_cm.x, _pos_target_neu_cm.x, _inav.get_position_neu_cm().x, - _vel_desired_neu_cms.x, _vel_target_neu_cms.x, _inav.get_velocity_neu_cms().x, + Write_PSCN(_pos_desired_neu_cm.x, _pos_target_neu_cm.x, _pos_estimate_neu_cm.x , + _vel_desired_neu_cms.x, _vel_target_neu_cms.x, _vel_estimate_neu_cms.x, _accel_desired_neu_cmss.x, _accel_target_neu_cmss.x, accel_n_cmss); - Write_PSCE(_pos_desired_neu_cm.y, _pos_target_neu_cm.y, _inav.get_position_neu_cm().y, - _vel_desired_neu_cms.y, _vel_target_neu_cms.y, _inav.get_velocity_neu_cms().y, + Write_PSCE(_pos_desired_neu_cm.y, _pos_target_neu_cm.y, _pos_estimate_neu_cm.y, + _vel_desired_neu_cms.y, _vel_target_neu_cms.y, _vel_estimate_neu_cms.y, _accel_desired_neu_cmss.y, _accel_target_neu_cmss.y, accel_e_cmss); // log offsets if they are being used @@ -1384,8 +1407,8 @@ void AC_PosControl::write_log() } if (is_active_U()) { - Write_PSCD(-_pos_desired_neu_cm.z, -_pos_target_neu_cm.z, -_inav.get_position_z_up_cm(), - -_vel_desired_neu_cms.z, -_vel_target_neu_cms.z, -_inav.get_velocity_z_up_cms(), + Write_PSCD(-_pos_desired_neu_cm.z, -_pos_target_neu_cm.z, -_pos_estimate_neu_cm.z, + -_vel_desired_neu_cms.z, -_vel_target_neu_cms.z, -_vel_estimate_neu_cms.z, -_accel_desired_neu_cmss.z, -_accel_target_neu_cmss.z, -get_measured_accel_U_cmss()); // log down and terrain offsets if they are being used @@ -1402,7 +1425,7 @@ void AC_PosControl::write_log() /// crosstrack_error - returns horizontal error to the closest point to the current track float AC_PosControl::crosstrack_error() const { - const Vector2f pos_error = _inav.get_position_xy_cm() - (_pos_target_neu_cm.xy()).tofloat(); + const Vector2f pos_error = (_pos_target_neu_cm.xy() - _pos_estimate_neu_cm.xy()).tofloat(); if (is_zero(_vel_desired_neu_cms.xy().length_squared())) { // crosstrack is the horizontal distance to target when stationary return pos_error.length(); @@ -1545,9 +1568,9 @@ void AC_PosControl::handle_ekf_NE_reset() // for this we need some sort of switch to select what type of EKF handling we want to use // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control. - _pos_target_neu_cm.xy() = (_inav.get_position_xy_cm() + _p_pos_ne.get_error()).topostype(); + _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy() + _p_pos_ne.get_error().topostype(); _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); - _vel_target_neu_cms.xy() = _inav.get_velocity_xy_cms() + _pid_vel_ne.get_error(); + _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy() + _pid_vel_ne.get_error(); _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); _ekf_ne_reset_ms = reset_ms; @@ -1573,9 +1596,9 @@ void AC_PosControl::handle_ekf_U_reset() // for this we need some sort of switch to select what type of EKF handling we want to use // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control. - _pos_target_neu_cm.z = _inav.get_position_z_up_cm() + _p_pos_u.get_error(); + _pos_target_neu_cm.z = _pos_estimate_neu_cm.z + _p_pos_u.get_error(); _pos_desired_neu_cm.z = _pos_target_neu_cm.z - (_pos_offset_neu_cm.z + _pos_terrain_u_cm); - _vel_target_neu_cms.z = _inav.get_velocity_z_up_cms() + _pid_vel_u.get_error(); + _vel_target_neu_cms.z = _vel_estimate_neu_cms.z + _pid_vel_u.get_error(); _vel_desired_neu_cms.z = _vel_target_neu_cms.z - (_vel_offset_neu_cms.z + _vel_terrain_u_cms); _ekf_u_reset_ms = reset_ms; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index b260a3680375bb..235cbc925a0d78 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -41,8 +41,7 @@ class AC_PosControl public: /// Constructor - AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav, - const class AP_Motors& motors, AC_AttitudeControl& attitude_control); + AC_PosControl(AP_AHRS_View& ahrs, const class AP_Motors& motors, AC_AttitudeControl& attitude_control); // do not allow copying CLASS_NO_COPY(AC_PosControl); @@ -53,6 +52,11 @@ class AC_PosControl void set_dt(float dt) { _dt = dt; } float get_dt() const { return _dt; } + // Updates internal position and velocity estimates in the NED frame. + // Falls back to vertical-only estimates if full NED data is unavailable. + // When high_vibes is true, forces use of vertical fallback for velocity. + void update_estimates(bool high_vibes = false); + /// get_shaping_jerk_NE_cmsss - gets the jerk limit of the ne kinematic path generation in cm/s/s/s float get_shaping_jerk_NE_cmsss() const { return _shaping_jerk_ne_msss * 100.0; } @@ -304,7 +308,7 @@ class AC_PosControl void set_vel_desired_NE_cms(const Vector2f &vel_desired_ne_cms) {_vel_desired_neu_cms.xy() = vel_desired_ne_cms; } /// get_vel_desired_NEU_cms - returns desired velocity in cm/s in NEU - const Vector3f& get_vel_desired_NEU_cms() { return _vel_desired_neu_cms; } + const Vector3f& get_vel_desired_NEU_cms() const { return _vel_desired_neu_cms; } // get_vel_target_NEU_cms - returns the target velocity in NEU cm/s const Vector3f& get_vel_target_NEU_cms() const { return _vel_target_neu_cms; } @@ -334,7 +338,7 @@ class AC_PosControl void init_pos_terrain_U_cm(float pos_terrain_u_cm); // get_pos_terrain_U_cm - returns the current terrain altitude in cm - float get_pos_terrain_U_cm() { return _pos_terrain_u_cm; } + float get_pos_terrain_U_cm() const { return _pos_terrain_u_cm; } /// Offset @@ -518,7 +522,6 @@ class AC_PosControl // references to inertial nav and ahrs libraries AP_AHRS_View& _ahrs; - const AP_InertialNav& _inav; const class AP_Motors& _motors; AC_AttitudeControl& _attitude_control; @@ -555,8 +558,10 @@ class AC_PosControl float _yaw_rate_target_cds; // desired yaw rate in centi-degrees per second calculated by position controller // position controller internal variables + Vector3p _pos_estimate_neu_cm; Vector3p _pos_desired_neu_cm; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets Vector3p _pos_target_neu_cm; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired_neu_cm plus offsets + Vector3f _vel_estimate_neu_cms; Vector3f _vel_desired_neu_cms; // desired velocity in NEU cm/s Vector3f _vel_target_neu_cms; // velocity target in NEU cm/s calculated by pos_to_rate step Vector3f _accel_desired_neu_cmss; // desired acceleration in NEU cm/s/s (feed forward) From 732d359f1bbac1095571c28a40f038f42d6c14ab Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 11 May 2025 21:13:53 +0930 Subject: [PATCH 25/34] Copter: AC_PosControl: Remove INAV and update_estimates() --- ArduCopter/mode.cpp | 1 + ArduCopter/system.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 88a6717bfc707c..0c22b76de2a463 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -405,6 +405,7 @@ bool Copter::set_mode(const uint8_t new_mode, const ModeReason reason) // called at 100hz or more void Copter::update_flight_mode() { + pos_control->update_estimates(vibration_check.high_vibes); #if AP_RANGEFINDER_ENABLED surface_tracking.invalidate_for_logging(); // invalidate surface tracking alt, flight mode will set to true if used #endif diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 9f167d2bdbaeaf..5a7f2b76fc5819 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -448,7 +448,7 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(attitude_control, attitude_control_var_info); - pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); + pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, *motors, *attitude_control); if (pos_control == nullptr) { AP_BoardConfig::allocation_error("PosControl"); } From b107b1825d152234251e04d9ce02ae5b7cdc0f25 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 11 May 2025 21:14:15 +0930 Subject: [PATCH 26/34] Sub: AC_PosControl: Remove INAV and update_estimates() --- ArduSub/Sub.cpp | 2 +- ArduSub/mode.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 2c581d30285da8..e6f217c62e1fc5 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -37,7 +37,7 @@ Sub::Sub() inertial_nav(ahrs), ahrs_view(ahrs, ROTATION_NONE), attitude_control(ahrs_view, aparm, motors), - pos_control(ahrs_view, inertial_nav, motors, attitude_control), + pos_control(ahrs_view, motors, attitude_control), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index b540cebe4acfc5..dd71c815128345 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -156,6 +156,7 @@ bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) // called at 100hz or more void Sub::update_flight_mode() { + pos_control.update_estimates(); flightmode->run(); } From 0045df481d74f6caf15a5108fd0005765ff37b4b Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 08:52:29 +0930 Subject: [PATCH 27/34] Plane: AC_PosControl: Remove INAV and update_estimates() --- ArduPlane/Plane.cpp | 3 +++ ArduPlane/quadplane.cpp | 2 +- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/Plane.cpp b/ArduPlane/Plane.cpp index d3900654895a58..f84271d6597df4 100644 --- a/ArduPlane/Plane.cpp +++ b/ArduPlane/Plane.cpp @@ -204,6 +204,9 @@ void Plane::ahrs_update() // update inertial_nav for quadplane quadplane.inertial_nav.update(); + if (quadplane.available()) { + quadplane.pos_control->update_estimates(); + } #endif #if HAL_LOGGING_ENABLED diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 53f9043bf223c2..24d34f048365cf 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -752,7 +752,7 @@ bool QuadPlane::setup(void) } AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); - pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); + pos_control = NEW_NOTHROW AC_PosControl(*ahrs_view, *motors, *attitude_control); if (!pos_control) { AP_BoardConfig::allocation_error("pos_control"); } From 5e0146df4eedb2d96557c3effcb2a2386f797780 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 10:18:47 +0930 Subject: [PATCH 28/34] AC_AttitudeControl: AC_PosControl: Add estimate getters --- libraries/AC_AttitudeControl/AC_PosControl.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 235cbc925a0d78..fdafa337e5db8e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -259,6 +259,9 @@ class AC_PosControl /// Position + /// get_pos_estimate_NEU_cm - returns the current position estimate, frame NEU in cm relative to the EKF origin + const Vector3p& get_pos_estimate_NEU_cm() const { return _pos_estimate_neu_cm; } + /// get_pos_target_NEU_cm - returns the position target, frame NEU in cm relative to the EKF origin const Vector3p& get_pos_target_NEU_cm() const { return _pos_target_neu_cm; } @@ -301,6 +304,9 @@ class AC_PosControl /// Velocity + /// get_vel_estimate_NEU_cms - returns current velocity estimate in cm/s in NEU + const Vector3f& get_vel_estimate_NEU_cms() const { return _vel_estimate_neu_cms; } + /// set_vel_desired_NEU_cms - sets desired velocity in NEU cm/s void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { _vel_desired_neu_cms = vel_desired_neu_cms; } From edce10d9872a45fc437fcce162f0db7f2623fcf2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 09:11:47 +0930 Subject: [PATCH 29/34] AC_WPNav: Remove INAV --- libraries/AC_WPNav/AC_Circle.cpp | 9 ++++----- libraries/AC_WPNav/AC_Circle.h | 3 +-- libraries/AC_WPNav/AC_Loiter.cpp | 3 +-- libraries/AC_WPNav/AC_Loiter.h | 3 +-- libraries/AC_WPNav/AC_WPNav.cpp | 20 ++++++++------------ libraries/AC_WPNav/AC_WPNav.h | 3 +-- libraries/AC_WPNav/AC_WPNav_OA.cpp | 8 ++++---- libraries/AC_WPNav/AC_WPNav_OA.h | 2 +- 8 files changed, 21 insertions(+), 30 deletions(-) diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index 8e6563eb5542d8..d8b737a4d60a32 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -40,8 +40,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control) : - _inav(inav), +AC_Circle::AC_Circle(const AP_AHRS_View& ahrs, AC_PosControl& pos_control) : _ahrs(ahrs), _pos_control(pos_control) { @@ -115,7 +114,7 @@ void AC_Circle::set_center(const Location& center) set_center_NEU_cm(Vector3f(center_ne_cm.x, center_ne_cm.y, terr_alt_cm), true); } else { // failed to convert location so set to current position and log error - set_center_NEU_cm(_inav.get_position_neu_cm(), false); + set_center_NEU_cm(_pos_control.get_pos_estimate_NEU_cm().tofloat(), false); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } } else { @@ -123,7 +122,7 @@ void AC_Circle::set_center(const Location& center) Vector3f circle_center_neu_cm; if (!center.get_vector_from_origin_NEU_cm(circle_center_neu_cm)) { // default to current position and log error - circle_center_neu_cm = _inav.get_position_neu_cm(); + circle_center_neu_cm = _pos_control.get_pos_estimate_NEU_cm().tofloat(); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } set_center_NEU_cm(circle_center_neu_cm, false); @@ -364,7 +363,7 @@ bool AC_Circle::get_terrain_offset_cm(float& offset_cm) float terr_alt = 0.0f; AP_Terrain *terrain = AP_Terrain::get_singleton(); if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) { - offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0); + offset_cm = _pos_control.get_pos_estimate_NEU_cm().tofloat().z - (terr_alt * 100.0); return true; } #endif diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 8ac7857fb804c8..b50468f4eb4e1e 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -17,7 +17,7 @@ class AC_Circle public: /// Constructor - AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control); + AC_Circle(const AP_AHRS_View& ahrs, AC_PosControl& pos_control); /// init - initialise circle controller setting center specifically /// set terrain_alt to true if center_neu_cm.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn @@ -131,7 +131,6 @@ class AC_Circle } _flags; // references to inertial nav and ahrs libraries - const AP_InertialNav& _inav; const AP_AHRS_View& _ahrs; AC_PosControl& _pos_control; diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 60e14ee5c02050..7b288a6318121c 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -79,8 +79,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_Loiter::AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : - _inav(inav), +AC_Loiter::AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : _ahrs(ahrs), _pos_control(pos_control), _attitude_control(attitude_control) diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index 7f1de2edc39b9a..4b079b0742f84c 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -14,7 +14,7 @@ class AC_Loiter public: /// Constructor - AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); + AC_Loiter(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); /// initialise loiter target to a position in cm from ekf origin void init_target_cm(const Vector2f& position_neu_cm); @@ -72,7 +72,6 @@ class AC_Loiter void calc_desired_velocity(bool avoidance_on = true); // references and pointers to external libraries - const AP_InertialNav& _inav; const AP_AHRS_View& _ahrs; AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 9dbc6b9d7f0a4e..c40d51e6613cdd 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -108,8 +108,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : - _inav(inav), +AC_WPNav::AC_WPNav(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : _ahrs(ahrs), _pos_control(pos_control), _attitude_control(attitude_control) @@ -409,12 +408,9 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_NE() // Reset position controller to current location _pos_control.init_NE_controller(); - // get current and target locations - const Vector2f& curr_pos_neu_cm = _inav.get_position_xy_cm(); - // shift origin and destination_neu_cm horizontally - _origin_neu_cm.xy() = curr_pos_neu_cm; - _destination_neu_cm.xy() = curr_pos_neu_cm; + _origin_neu_cm.xy() = _pos_control.get_pos_estimate_NEU_cm().tofloat().xy(); + _destination_neu_cm.xy() = _pos_control.get_pos_estimate_NEU_cm().tofloat().xy(); } /// shifts the origin and destination_neu_cm horizontally to the achievable stopping point @@ -472,7 +468,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) const Vector3p& psc_pos_offset_neu_cm = _pos_control.get_pos_offset_NEU_cm(); // get current position and adjust altitude to origin and destination_neu_cm's frame (i.e. _frame) - Vector3f curr_pos_neu_cm = _inav.get_position_neu_cm() - psc_pos_offset_neu_cm.tofloat(); + Vector3f curr_pos_neu_cm = _pos_control.get_pos_estimate_NEU_cm().tofloat() - psc_pos_offset_neu_cm.tofloat(); curr_pos_neu_cm.z -= terr_offset_u_cm; Vector3f curr_target_vel_neu_cms = _pos_control.get_vel_desired_NEU_cms(); curr_target_vel_neu_cms.z -= _pos_control.get_vel_offset_U_cms(); @@ -484,7 +480,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) if (is_positive(curr_target_vel_neu_cms.length_squared())) { Vector3f track_direction_neu = curr_target_vel_neu_cms.normalized(); const float track_error_neu_cm = _pos_control.get_pos_error_NEU_cm().dot(track_direction_neu); - const float track_velocity_neu_cms = _inav.get_velocity_neu_cms().dot(track_direction_neu); + const float track_velocity_neu_cms = _pos_control.get_vel_estimate_NEU_cms().dot(track_direction_neu); // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation. track_scaler_dt = constrain_float(0.05f + (track_velocity_neu_cms - _pos_control.get_pos_NE_p().kP() * track_error_neu_cm) / curr_target_vel_neu_cms.length(), 0.0f, 1.0f); } @@ -578,13 +574,13 @@ void AC_WPNav::update_track_with_speed_accel_limits() /// get_wp_distance_to_destination - get horizontal distance to destination_neu_cm in cm float AC_WPNav::get_wp_distance_to_destination_cm() const { - return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_neu_cm.xy()); + return get_horizontal_distance_cm(_pos_control.get_pos_estimate_NEU_cm().tofloat().xy(), _destination_neu_cm.xy()); } /// get_wp_bearing_to_destination_cd - get bearing to next waypoint in centi-degrees int32_t AC_WPNav::get_wp_bearing_to_destination_cd() const { - return get_bearing_cd(_inav.get_position_xy_cm(), _destination_neu_cm.xy()); + return get_bearing_cd(_pos_control.get_pos_estimate_NEU_cm().tofloat().xy(), _destination_neu_cm.xy()); } /// update_wpnav - run the wp controller - should be called at 100hz or higher @@ -668,7 +664,7 @@ bool AC_WPNav::get_terrain_offset_cm(float& offset_cm) AP_Terrain *terrain = AP::terrain(); if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) { - offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0); + offset_cm = _pos_control.get_pos_estimate_NEU_cm().tofloat().z - (terr_alt * 100.0); return true; } #endif diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 0f714c2df9e831..9ca86df7fa2fdd 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -20,7 +20,7 @@ class AC_WPNav public: /// Constructor - AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); + AC_WPNav(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); /// provide rangefinder based terrain offset /// terrain offset is the terrain's height above the EKF origin @@ -236,7 +236,6 @@ class AC_WPNav void calc_scurve_jerk_and_snap(); // references and pointers to external libraries - const AP_InertialNav& _inav; const AP_AHRS_View& _ahrs; AC_PosControl& _pos_control; const AC_AttitudeControl& _attitude_control; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 2d030c2329404c..19c784968633e1 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -6,8 +6,8 @@ #include #include "AC_WPNav_OA.h" -AC_WPNav_OA::AC_WPNav_OA(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : - AC_WPNav(inav, ahrs, pos_control, attitude_control) +AC_WPNav_OA::AC_WPNav_OA(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) : + AC_WPNav(ahrs, pos_control, attitude_control) { } @@ -48,7 +48,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination_cm() const return AC_WPNav::get_wp_distance_to_destination_cm(); } - return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy()); + return get_horizontal_distance_cm(_pos_control.get_pos_estimate_NEU_cm().tofloat().xy(), _destination_oabak_neu_cm.xy()); } /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees @@ -59,7 +59,7 @@ int32_t AC_WPNav_OA::get_wp_bearing_to_destination_cd() const return AC_WPNav::get_wp_bearing_to_destination_cd(); } - return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy()); + return get_bearing_cd(_pos_control.get_pos_estimate_NEU_cm().tofloat().xy(), _destination_oabak_neu_cm.xy()); } /// true when we have come within RADIUS cm of the waypoint diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index 55de9195b83d90..56b79371373920 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -13,7 +13,7 @@ class AC_WPNav_OA : public AC_WPNav public: /// Constructor - AC_WPNav_OA(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); + AC_WPNav_OA(const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); // returns object avoidance adjusted wp location using location class // returns false if unable to convert from target vector to global coordinates From ad8bacade6f05f74c5312579a44b0a9909719c00 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 09:09:19 +0930 Subject: [PATCH 30/34] Copter: Remove INAV --- ArduCopter/system.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 5a7f2b76fc5819..16bf82261db451 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -455,23 +455,23 @@ void Copter::allocate_motors(void) AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); #if AP_OAPATHPLANNER_ENABLED - wp_nav = NEW_NOTHROW AC_WPNav_OA(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + wp_nav = NEW_NOTHROW AC_WPNav_OA(*ahrs_view, *pos_control, *attitude_control); #else - wp_nav = NEW_NOTHROW AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + wp_nav = NEW_NOTHROW AC_WPNav(*ahrs_view, *pos_control, *attitude_control); #endif if (wp_nav == nullptr) { AP_BoardConfig::allocation_error("WPNav"); } AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); - loiter_nav = NEW_NOTHROW AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + loiter_nav = NEW_NOTHROW AC_Loiter(*ahrs_view, *pos_control, *attitude_control); if (loiter_nav == nullptr) { AP_BoardConfig::allocation_error("LoiterNav"); } AP_Param::load_object_from_eeprom(loiter_nav, loiter_nav->var_info); #if MODE_CIRCLE_ENABLED - circle_nav = NEW_NOTHROW AC_Circle(inertial_nav, *ahrs_view, *pos_control); + circle_nav = NEW_NOTHROW AC_Circle(*ahrs_view, *pos_control); if (circle_nav == nullptr) { AP_BoardConfig::allocation_error("CircleNav"); } From 0cb51b739100e6f950d5ec6d208d3d803be8ee19 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 09:10:19 +0930 Subject: [PATCH 31/34] Plane: Remove INAV --- ArduPlane/quadplane.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 24d34f048365cf..778d4787600703 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -757,13 +757,13 @@ bool QuadPlane::setup(void) AP_BoardConfig::allocation_error("pos_control"); } AP_Param::load_object_from_eeprom(pos_control, pos_control->var_info); - wp_nav = NEW_NOTHROW AC_WPNav(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + wp_nav = NEW_NOTHROW AC_WPNav(*ahrs_view, *pos_control, *attitude_control); if (!wp_nav) { AP_BoardConfig::allocation_error("wp_nav"); } AP_Param::load_object_from_eeprom(wp_nav, wp_nav->var_info); - loiter_nav = NEW_NOTHROW AC_Loiter(inertial_nav, *ahrs_view, *pos_control, *attitude_control); + loiter_nav = NEW_NOTHROW AC_Loiter(*ahrs_view, *pos_control, *attitude_control); if (!loiter_nav) { AP_BoardConfig::allocation_error("loiter_nav"); } From 7ca4b3dcbda44c580bcd51da761b68f09ab7c66c Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 12:43:57 +0930 Subject: [PATCH 32/34] Sub: Remove INAV --- ArduSub/Sub.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index e6f217c62e1fc5..54c5959d5f7e08 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -38,9 +38,9 @@ Sub::Sub() ahrs_view(ahrs, ROTATION_NONE), attitude_control(ahrs_view, aparm, motors), pos_control(ahrs_view, motors, attitude_control), - wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), - loiter_nav(inertial_nav, ahrs_view, pos_control, attitude_control), - circle_nav(inertial_nav, ahrs_view, pos_control), + wp_nav(ahrs_view, pos_control, attitude_control), + loiter_nav(ahrs_view, pos_control, attitude_control), + circle_nav(ahrs_view, pos_control), param_loader(var_info), flightmode(&mode_manual), auto_mode(Auto_WP), From 0b4a3acbda9dc1d8dcc2ca0f832cbc0e6cc9f590 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 12:53:28 +0930 Subject: [PATCH 33/34] Copter: Remove inertial_nav from flight modes --- ArduCopter/mode.cpp | 6 +++--- ArduCopter/mode_auto.cpp | 4 ++-- ArduCopter/mode_brake.cpp | 4 ++-- ArduCopter/mode_drift.cpp | 2 +- ArduCopter/mode_flowhold.cpp | 6 +++--- ArduCopter/mode_guided.cpp | 8 ++++---- ArduCopter/mode_loiter.cpp | 4 ++-- ArduCopter/mode_poshold.cpp | 6 +++--- ArduCopter/mode_rtl.cpp | 2 +- ArduCopter/mode_systemid.cpp | 2 +- ArduCopter/mode_throw.cpp | 20 ++++++++++---------- ArduCopter/takeoff.cpp | 12 +++++------- 12 files changed, 37 insertions(+), 39 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 0c22b76de2a463..612c4c20e55beb 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -655,7 +655,7 @@ void Mode::land_run_vertical_control(bool pause_descent) Vector2f target_pos; float target_error_cm = 0.0f; if (copter.precland.get_target_position_cm(target_pos)) { - const Vector2f current_pos = inertial_nav.get_position_xy_cm(); + const Vector2f current_pos = pos_control->get_pos_estimate_NEU_cm().xy().tofloat(); // target is this many cm away from the vehicle target_error_cm = (target_pos - current_pos).length(); } @@ -739,10 +739,10 @@ void Mode::land_run_horizontal_control() if (copter.ap.prec_land_active) { Vector2f target_pos, target_vel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos = inertial_nav.get_position_xy_cm(); + target_pos = pos_control->get_pos_estimate_NEU_cm().xy().tofloat(); } // get the velocity of the target - copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel); + copter.precland.get_target_velocity_cms(pos_control->get_vel_estimate_NEU_cms().xy(), target_vel); Vector2f zero; Vector2p landing_pos = target_pos.topostype(); diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index d425ab6c809c72..ede105dd1ae62c 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -378,7 +378,7 @@ void ModeAuto::takeoff_start(const Location& dest_loc) // by default current_alt_cm and alt_target_cm are alt-above-EKF-origin int32_t alt_target_cm; bool alt_target_terrain = false; - float current_alt_cm = inertial_nav.get_position_z_up_cm(); + float current_alt_cm = pos_control->get_pos_estimate_NEU_cm().tofloat().z; float terrain_offset; // terrain's altitude in cm above the ekf origin if ((dest_loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) && wp_nav->get_terrain_offset_cm(terrain_offset)) { // subtract terrain offset to convert vehicle's alt-above-ekf-origin to alt-above-terrain @@ -535,7 +535,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi } // if we are outside the circle, point at the edge, otherwise hold yaw - const float dist_to_center = get_horizontal_distance_cm(inertial_nav.get_position_xy_cm().topostype(), copter.circle_nav->get_center_NEU_cm().xy()); + const float dist_to_center = get_horizontal_distance_cm(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), copter.circle_nav->get_center_NEU_cm().tofloat().xy()); // initialise yaw // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI if (auto_yaw.mode() != AutoYaw::Mode::ROI) { diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index d76e505482dd54..b6ccb5a0bc67b6 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -10,8 +10,8 @@ bool ModeBrake::init(bool ignore_checks) { // initialise pos controller speed and acceleration - pos_control->set_max_speed_accel_NE_cm(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE); - pos_control->set_correction_speed_accel_NE_cm(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE); + pos_control->set_max_speed_accel_NE_cm(pos_control->get_vel_estimate_NEU_cms().length(), BRAKE_MODE_DECEL_RATE); + pos_control->set_correction_speed_accel_NE_cm(pos_control->get_vel_estimate_NEU_cms().length(), BRAKE_MODE_DECEL_RATE); // initialise position controller pos_control->init_NE_controller(); diff --git a/ArduCopter/mode_drift.cpp b/ArduCopter/mode_drift.cpp index ae1ee140d416d4..f74bcd1b83588b 100644 --- a/ArduCopter/mode_drift.cpp +++ b/ArduCopter/mode_drift.cpp @@ -46,7 +46,7 @@ void ModeDrift::run() get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // Grab inertial velocity - const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); + const Vector3f& vel = pos_control->get_vel_estimate_NEU_cms(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 71dc6b9959cb2d..3751f93f2c4525 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -106,7 +106,7 @@ bool ModeFlowHold::init(bool ignore_checks) flow_pi_xy.set_dt(1.0/copter.scheduler.get_loop_rate_hz()); // start with INS height - last_ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01; + last_ins_height = pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01; height_offset = 0; return true; @@ -130,7 +130,7 @@ void ModeFlowHold::flowhold_flow_to_angle(Vector2f &bf_angles, bool stick_input) Vector2f sensor_flow = flow_filter.apply(raw_flow); // scale by height estimate, limiting it to height_min to height_max - float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01; + float ins_height = pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01; float height_estimate = ins_height + height_offset; // compensate for height, this converts to (approx) m/s @@ -351,7 +351,7 @@ void ModeFlowHold::run() */ void ModeFlowHold::update_height_estimate(void) { - float ins_height = copter.inertial_nav.get_position_z_up_cm() * 0.01; + float ins_height = copter.pos_control->get_pos_estimate_NEU_cm().tofloat().z * 0.01; #if 1 // assume on ground when disarmed, or if we have only just started spooling the motors up diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 9fa40631bae5b1..ef665c1d429fdc 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -1062,7 +1062,7 @@ void ModeGuided::limit_init_time_and_pos() guided_limit.start_time = AP_HAL::millis(); // initialise start position from current position - guided_limit.start_pos = inertial_nav.get_position_neu_cm(); + guided_limit.start_pos = pos_control->get_pos_estimate_NEU_cm().tofloat(); } // limit_check - returns true if guided mode has breached a limit @@ -1075,7 +1075,7 @@ bool ModeGuided::limit_check() } // get current location - const Vector3f& curr_pos = inertial_nav.get_position_neu_cm(); + const Vector3f& curr_pos = pos_control->get_pos_estimate_NEU_cm().tofloat(); // check if we have gone below min alt if (!is_zero(guided_limit.alt_min_cm) && (curr_pos.z < guided_limit.alt_min_cm)) { @@ -1120,7 +1120,7 @@ float ModeGuided::wp_distance_m() const case SubMode::WP: return wp_nav->get_wp_distance_to_destination_cm() * 0.01f; case SubMode::Pos: - return get_horizontal_distance_cm(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()) * 0.01f; + return get_horizontal_distance_cm(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), guided_pos_target_cm.tofloat().xy()) * 0.01f; case SubMode::PosVelAccel: return pos_control->get_pos_error_NE_cm() * 0.01f; default: @@ -1134,7 +1134,7 @@ int32_t ModeGuided::wp_bearing() const case SubMode::WP: return wp_nav->get_wp_bearing_to_destination_cd(); case SubMode::Pos: - return get_bearing_cd(inertial_nav.get_position_xy_cm(), guided_pos_target_cm.tofloat().xy()); + return get_bearing_cd(pos_control->get_pos_estimate_NEU_cm().tofloat().xy(), guided_pos_target_cm.tofloat().xy()); case SubMode::PosVelAccel: return pos_control->get_bearing_to_target_cd(); case SubMode::TakeOff: diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 817be90359baa0..71107058c365ad 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -65,10 +65,10 @@ void ModeLoiter::precision_loiter_xy() loiter_nav->clear_pilot_desired_acceleration(); Vector2f target_pos, target_vel; if (!copter.precland.get_target_position_cm(target_pos)) { - target_pos = inertial_nav.get_position_xy_cm(); + target_pos = pos_control->get_pos_estimate_NEU_cm().tofloat().xy(); } // get the velocity of the target - copter.precland.get_target_velocity_cms(inertial_nav.get_velocity_xy_cms(), target_vel); + copter.precland.get_target_velocity_cms(pos_control->get_vel_estimate_NEU_cms().xy(), target_vel); Vector2f zero; Vector2p landing_pos = target_pos.topostype(); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index bd292cc9e85a15..d7b96d8de76de1 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -69,7 +69,7 @@ void ModePosHold::run() { float controller_to_pilot_roll_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls float controller_to_pilot_pitch_mix; // mix of controller and pilot controls. 0 = fully last controller controls, 1 = fully pilot controls - const Vector3f& vel = inertial_nav.get_velocity_neu_cms(); + const Vector3f& vel = pos_control->get_vel_estimate_NEU_cms(); // set vertical speed and acceleration limits pos_control->set_max_speed_accel_U_cm(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); @@ -377,7 +377,7 @@ void ModePosHold::run() pitch_mode = RPMode::BRAKE_TO_LOITER; brake.to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER; // init loiter controller - loiter_nav->init_target_cm(inertial_nav.get_position_xy_cm() - pos_control->get_pos_offset_NEU_cm().xy().tofloat()); + loiter_nav->init_target_cm((pos_control->get_pos_estimate_NEU_cm().xy() - pos_control->get_pos_offset_NEU_cm().xy()).tofloat()); // set delay to start of wind compensation estimate updates wind_comp_start_timer = POSHOLD_WIND_COMP_START_TIMER; } @@ -564,7 +564,7 @@ void ModePosHold::update_wind_comp_estimate() } // check horizontal velocity is low - if (inertial_nav.get_speed_xy_cms() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) { + if (pos_control->get_vel_estimate_NEU_cms().xy().length() > POSHOLD_WIND_COMP_ESTIMATE_SPEED_MAX) { return; } diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9eda4dc2e03e32..8ae3f4babe3d70 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -309,7 +309,7 @@ void ModeRTL::descent_run() attitude_control->input_thrust_vector_heading_cd(pos_control->get_thrust_vector(), auto_yaw.get_heading()); // check if we've reached within 20cm of final altitude - _state_complete = labs(rtl_path.descent_target.alt - copter.inertial_nav.get_position_z_up_cm()) < 20; + _state_complete = labs(rtl_path.descent_target.alt - pos_control->get_pos_estimate_NEU_cm().tofloat().z) < 20; } // land_start - initialise controllers to loiter over home diff --git a/ArduCopter/mode_systemid.cpp b/ArduCopter/mode_systemid.cpp index ae9e57da0f6368..91291daffea07d 100644 --- a/ArduCopter/mode_systemid.cpp +++ b/ArduCopter/mode_systemid.cpp @@ -129,7 +129,7 @@ bool ModeSystemId::init(bool ignore_checks) pos_control->init_U_controller(); } Vector3f curr_pos; - curr_pos = inertial_nav.get_position_neu_cm(); + curr_pos = pos_control->get_pos_estimate_NEU_cm().tofloat(); target_pos = curr_pos.xy(); } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 937c7a1e3e0a57..90a4bc70f3a684 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -72,9 +72,9 @@ void ModeThrow::run() // initialise the demanded height below/above the throw height from user parameters // this allows for rapidly clearing surrounding obstacles if (g2.throw_type == ThrowType::Drop) { - pos_control->set_pos_desired_U_cm(inertial_nav.get_position_z_up_cm() - g.throw_altitude_descend * 100.0f); + pos_control->set_pos_desired_U_cm(pos_control->get_pos_estimate_NEU_cm().z - g.throw_altitude_descend * 100.0f); } else { - pos_control->set_pos_desired_U_cm(inertial_nav.get_position_z_up_cm() + g.throw_altitude_ascend * 100.0f); + pos_control->set_pos_desired_U_cm(pos_control->get_pos_estimate_NEU_cm().z + g.throw_altitude_ascend * 100.0f); } // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum @@ -206,8 +206,8 @@ void ModeThrow::run() if ((stage != prev_stage) || (now - last_log_ms) > 100) { prev_stage = stage; last_log_ms = now; - const float velocity = inertial_nav.get_velocity_neu_cms().length(); - const float velocity_z = inertial_nav.get_velocity_z_up_cms(); + const float velocity = pos_control->get_vel_estimate_NEU_cms().length(); + const float velocity_z = pos_control->get_vel_estimate_NEU_cms().z; const float accel = copter.ins.get_accel().length(); const float ef_accel_z = ahrs.get_accel_ef().z; const bool throw_detect = (stage > Throw_Detecting) || throw_detected(); @@ -258,14 +258,14 @@ bool ModeThrow::throw_detected() } // Check for high speed (>500 cm/s) - bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED); + bool high_speed = pos_control->get_vel_estimate_NEU_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED); // check for upwards or downwards trajectory (airdrop) of 50cm/s bool changing_height; if (g2.throw_type == ThrowType::Drop) { - changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED; + changing_height = pos_control->get_vel_estimate_NEU_cms().z < -THROW_VERTICAL_SPEED; } else { - changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED; + changing_height = pos_control->get_vel_estimate_NEU_cms().z > THROW_VERTICAL_SPEED; } // Check the vertical acceleration is greater than 0.25g @@ -280,7 +280,7 @@ bool ModeThrow::throw_detected() ahrs.get_relative_position_D_home(altitude_above_home); altitude_above_home = -altitude_above_home; // altitude above home is returned as negative } else { - altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters + altitude_above_home = pos_control->get_pos_estimate_NEU_cm().z * 0.01f; // centimeters to meters } // Check that the altitude is within user defined limits @@ -293,11 +293,11 @@ bool ModeThrow::throw_detected() // Record time and vertical velocity when we detect the possible throw if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) { free_fall_start_ms = AP_HAL::millis(); - free_fall_start_velz = inertial_nav.get_velocity_z_up_cms(); + free_fall_start_velz = pos_control->get_vel_estimate_NEU_cms().z; } // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm - bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f)); + bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((pos_control->get_vel_estimate_NEU_cms().z - free_fall_start_velz) < -250.0f)); // start motors and enter the control mode if we are in continuous freefall return throw_condition_confirmed; diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index d518cb38239687..9002b85fd9a9fa 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -115,7 +115,6 @@ void Mode::_TakeOff::do_pilot_takeoff(float& pilot_climb_rate_cm) void _AutoTakeoff::run() { const auto &g2 = copter.g2; - const auto &inertial_nav = copter.inertial_nav; const auto &wp_nav = copter.wp_nav; auto *motors = copter.motors; auto *pos_control = copter.pos_control; @@ -126,7 +125,7 @@ void _AutoTakeoff::run() // do not spool down tradheli when on the ground with motor interlock enabled copter.flightmode->make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock()); // update auto_takeoff_no_nav_alt_cm - no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100; return; } @@ -152,7 +151,7 @@ void _AutoTakeoff::run() attitude_control->reset_rate_controller_I_terms(); attitude_control->input_thrust_vector_rate_heading_cds(pos_control->get_thrust_vector(), 0.0); // update auto_takeoff_no_nav_alt_cm - no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100; return; } @@ -170,7 +169,7 @@ void _AutoTakeoff::run() if (throttle >= MIN(copter.g2.takeoff_throttle_max, 0.9) || (copter.pos_control->get_measured_accel_U_cmss() >= 0.5 * copter.pos_control->get_max_accel_U_cmss()) || (copter.pos_control->get_vel_desired_NEU_cms().z >= 0.1 * copter.pos_control->get_max_speed_up_cms()) || - ( no_nav_active && (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm))) { + ( no_nav_active && (pos_control->get_pos_estimate_NEU_cm().z >= no_nav_alt_cm))) { // throttle > 90% // acceleration > 50% maximum acceleration // velocity > 10% maximum velocity @@ -183,7 +182,7 @@ void _AutoTakeoff::run() // check if we are not navigating because of low altitude if (no_nav_active) { // check if vehicle has reached no_nav_alt threshold - if (inertial_nav.get_position_z_up_cm() >= no_nav_alt_cm) { + if (pos_control->get_pos_estimate_NEU_cm().z >= no_nav_alt_cm) { no_nav_active = false; } pos_control->relax_velocity_controller_NE(); @@ -229,8 +228,7 @@ void _AutoTakeoff::start(float _complete_alt_cm, bool _terrain_alt) complete = false; // initialise auto_takeoff_no_nav_alt_cm const auto &g2 = copter.g2; - const auto &inertial_nav = copter.inertial_nav; - no_nav_alt_cm = inertial_nav.get_position_z_up_cm() + g2.wp_navalt_min * 100; + no_nav_alt_cm = copter.pos_control->get_pos_estimate_NEU_cm().z + g2.wp_navalt_min * 100; if ((g2.wp_navalt_min > 0) && (copter.flightmode->is_disarmed_or_landed() || !copter.motors->get_interlock())) { // we are not flying, climb with no navigation to current alt-above-ekf-origin + wp_navalt_min no_nav_active = true; From c1efd81220be8c93fc435b097bbc18df737427fd Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 12 May 2025 16:55:27 +0930 Subject: [PATCH 34/34] Sub: Alternative placement of update_estimates() --- ArduSub/inertia.cpp | 1 + ArduSub/mode.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduSub/inertia.cpp b/ArduSub/inertia.cpp index 9c9ec8138c86e3..2b8142024aac39 100644 --- a/ArduSub/inertia.cpp +++ b/ArduSub/inertia.cpp @@ -5,6 +5,7 @@ void Sub::read_inertia() { // inertial altitude estimates inertial_nav.update(); + sub.pos_control.update_estimates(); // pull position from ahrs Location loc; diff --git a/ArduSub/mode.cpp b/ArduSub/mode.cpp index dd71c815128345..b540cebe4acfc5 100644 --- a/ArduSub/mode.cpp +++ b/ArduSub/mode.cpp @@ -156,7 +156,6 @@ bool Sub::set_mode(const uint8_t new_mode, const ModeReason reason) // called at 100hz or more void Sub::update_flight_mode() { - pos_control.update_estimates(); flightmode->run(); }