diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 26e704b0ae..35cd997af7 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +37,8 @@ #include #include "commander_helper.h" +using namespace time_literals; + HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags) : _failsafe_flags(failsafe_flags) { @@ -69,7 +71,7 @@ bool HomePosition::hasMovedFromCurrentHomeLocation() eph = gpos.eph; epv = gpos.epv; - } else if (!_failsafe_flags.gps_position_invalid) { + } else if (_gps_position_for_home_valid) { sensor_gps_s gps; _vehicle_gps_position_sub.copy(&gps); const double lat = static_cast(gps.lat) * 1e-7; @@ -114,7 +116,7 @@ bool HomePosition::setHomePosition(bool force) setHomePosValid(); updated = true; - } else if (!_failsafe_flags.gps_position_invalid) { + } else if (_gps_position_for_home_valid) { // Set home using GNSS position sensor_gps_s gps_pos; _vehicle_gps_position_sub.copy(&gps_pos); @@ -203,7 +205,7 @@ void HomePosition::setInAirHomePosition() home.timestamp = hrt_absolute_time(); _home_position_pub.update(); - } else if (!_failsafe_flags.local_position_invalid && !_failsafe_flags.gps_position_invalid) { + } else if (!_failsafe_flags.local_position_invalid && _gps_position_for_home_valid) { // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS raw) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); @@ -314,6 +316,19 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) _local_position_sub.update(); _global_position_sub.update(); + if (_vehicle_gps_position_sub.updated()) { + sensor_gps_s vehicle_gps_position; + _vehicle_gps_position_sub.copy(&vehicle_gps_position); + + 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 vehicle_local_position_s &lpos = _local_position_sub.get(); const auto &home = _home_position_pub.get(); @@ -328,7 +343,7 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) if (check_if_changed && set_automatically) { const bool can_set_home_lpos_first_time = !home.valid_lpos && !_failsafe_flags.local_position_invalid; const bool can_set_home_gpos_first_time = ((!home.valid_hpos || !home.valid_alt) - && (!_failsafe_flags.global_position_invalid || !_failsafe_flags.gps_position_invalid)); + && (!_failsafe_flags.global_position_invalid || _gps_position_for_home_valid)); const bool can_set_home_alt_first_time = (!home.valid_alt && lpos.z_global); if (can_set_home_lpos_first_time diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 46a683530e..6b883d4e52 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +41,11 @@ #include #include +static constexpr int kHomePositionGPSRequiredFixType = 2; +static constexpr float kHomePositionGPSRequiredEPH = 5.f; +static constexpr float kHomePositionGPSRequiredEPV = 10.f; +static constexpr float kHomePositionGPSRequiredEVH = 1.f; + class HomePosition { public: @@ -75,4 +80,5 @@ private: uint8_t _heading_reset_counter{0}; bool _valid{false}; const failsafe_flags_s &_failsafe_flags; + bool _gps_position_for_home_valid{false}; };