From 6604c52c981a6c3292d0f1775d901a6fb483fc79 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Wed, 18 Jun 2025 13:26:45 +0200 Subject: [PATCH] Commander: Adjust home position altitude after GNSS altitude correction (#25003) Within the first 2min of a flight, check if the integrated GNSS vertical velocity and the baro measurements disagree with the reported GNSS altitude, and in that case update the set home position altitude to cancel out GNSS altitude drift. * prevent re-init when reaching negative altitudes * only allow correction during the first 120 second after takeoff * add dependency to COM_HOME_EN parameter. reset vel-integral for multiple takeoffs --- src/modules/commander/Commander.cpp | 1 + src/modules/commander/HomePosition.cpp | 80 ++++++++++++++++++++---- src/modules/commander/HomePosition.hpp | 24 ++++++- src/modules/commander/commander_params.c | 3 +- 4 files changed, 94 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index df445166ab..5e6a9082d4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2110,6 +2110,7 @@ void Commander::landDetectorUpdate() events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected"); _vehicle_status.takeoff_time = hrt_absolute_time(); _have_taken_off_since_arming = true; + _home_position.setTakeoffTime(_vehicle_status.takeoff_time); } // automatically set or update home position diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 5597b22391..2f30a93be4 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -39,12 +39,8 @@ #include #include "commander_helper.h" -using namespace time_literals; - -HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags) - : _failsafe_flags(failsafe_flags) -{ -} +HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr), + _failsafe_flags(failsafe_flags) {} bool HomePosition::hasMovedFromCurrentHomeLocation() { @@ -308,6 +304,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) _local_position_sub.update(); _global_position_sub.update(); + if (_vehicle_air_data_sub.updated()) { + vehicle_air_data_s baro_data; + _vehicle_air_data_sub.copy(&baro_data); + const float baro_alt = baro_data.baro_alt_meter; + + if (_last_baro_timestamp != 0) { + const float dt = baro_data.timestamp - _last_baro_timestamp; + _lpf_baro.update(baro_alt, dt); + + } else { + _lpf_baro.reset(baro_alt); + } + + _last_baro_timestamp = baro_data.timestamp; + } + if (_vehicle_gps_position_sub.updated()) { sensor_gps_s vehicle_gps_position; _vehicle_gps_position_sub.copy(&vehicle_gps_position); @@ -319,12 +331,56 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) _gps_epv = vehicle_gps_position.epv; const hrt_abstime now = hrt_absolute_time(); - const bool time = (now < vehicle_gps_position.timestamp + 1_s); - const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType; - const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH; - const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV; - const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH; - _gps_position_for_home_valid = time && fix && eph && epv && evh; + const bool time_valid = now < (vehicle_gps_position.timestamp + 1_s); + const bool fix_valid = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType; + const bool eph_valid = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH; + const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV; + const bool evh_valid = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH; + + _gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid; + + if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0 + && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) { + + const float gps_alt = static_cast(_gps_alt); + + if (!PX4_ISFINITE(_gps_vel_integral)) { + _gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos + _baro_gps_static_offset = gps_alt - _lpf_baro.getState(); + } + + _gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s); + + // correct baro_alt with offset from GPS alt from when the drift integral was initialized + const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset; + const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied; + + // Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude + // are more consistent with each other than either is with the GPS altitude (with home offset). + if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) && + fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) { + + const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied; + + if (fabsf(offset_new) > kAltitudeDifferenceThreshold) { + + home_position_s home = _home_position_pub.get(); + home.alt -= offset_new; + home.z += offset_new; + home.timestamp = now; + home.manual_home = false; + home.update_count = _home_position_pub.get().update_count + 1U; + + _home_position_pub.update(home); + _home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected + } + } + + } else { + _gps_vel_integral = NAN; + } + + _last_gps_timestamp = vehicle_gps_position.timestamp; } const vehicle_local_position_s &lpos = _local_position_sub.get(); diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 07d6b1a632..27a53c642c 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -40,6 +40,11 @@ #include #include #include +#include +#include +#include + +using namespace time_literals; static constexpr int kHomePositionGPSRequiredFixType = 2; static constexpr float kHomePositionGPSRequiredEPH = 5.f; @@ -47,8 +52,11 @@ static constexpr float kHomePositionGPSRequiredEPV = 10.f; static constexpr float kHomePositionGPSRequiredEVH = 1.f; static constexpr float kMinHomePositionChangeEPH = 1.f; static constexpr float kMinHomePositionChangeEPV = 1.5f; +static constexpr float kLpfBaroTimeConst = 5.f; +static constexpr float kAltitudeDifferenceThreshold = 1.f; // altitude difference after which home position gets updated +static constexpr uint64_t kHomePositionCorrectionTimeWindow = 120_s; -class HomePosition +class HomePosition: public ModuleParams { public: HomePosition(const failsafe_flags_s &failsafe_flags); @@ -57,6 +65,7 @@ public: bool setHomePosition(bool force = false); void setInAirHomePosition(); bool setManually(double lat, double lon, float alt, float yaw); + void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; } void update(bool set_automatically, bool check_if_changed); @@ -76,6 +85,15 @@ private: uORB::SubscriptionData _global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uint64_t _last_gps_timestamp{0}; + uint64_t _last_baro_timestamp{0}; + AlphaFilter _lpf_baro{kLpfBaroTimeConst}; + float _gps_vel_integral{NAN}; + float _home_altitude_offset_applied{0.f}; + float _baro_gps_static_offset{0.f}; + uint64_t _takeoff_time{0}; uORB::PublicationData _home_position_pub{ORB_ID(home_position)}; @@ -88,4 +106,8 @@ private: double _gps_alt{0}; float _gps_eph{0.f}; float _gps_epv{0.f}; + + DEFINE_PARAMETERS( + (ParamBool) _param_com_home_en + ) }; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 025d1a48cb..e0822148d2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -143,8 +143,9 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f); * * Set home position automatically if possible. * - * During missions, the home position is locked and will not reset during intermediate landings. + * During missions, the latitude/longitude of the home position is locked and will not reset during intermediate landings. * It will only update once the mission is complete or landed outside of a mission. + * However, the altitude is still being adjusted to correct for GNSS vertical drift in the first 2 minutes after takeoff. * * @group Commander * @reboot_required true