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