From 4d7fc1fe464b2ad34b9e562b0e08957d7ff04071 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Wed, 14 Feb 2024 14:14:51 +0100 Subject: [PATCH] AC_PrecLand: remove code duplication and use the same logic as the other backends (<= instead of <) --- libraries/AC_PrecLand/AC_PrecLand_SITL.cpp | 13 ++++--------- libraries/AC_PrecLand/AC_PrecLand_SITL.h | 2 +- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 1d920818fb..c8394948d1 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -39,19 +39,14 @@ void AC_PrecLand_SITL::update() _have_los_meas = _have_los_meas && AP_HAL::millis() - _los_meas_time_ms <= 1000; } -bool AC_PrecLand_SITL::have_los_meas() { - return AP_HAL::millis() - _los_meas_time_ms < 1000; -} - // provides a unit vector towards the target in body frame // returns same as have_los_meas() bool AC_PrecLand_SITL::get_los_body(Vector3f& ret) { - if (AP_HAL::millis() - _los_meas_time_ms > 1000) { - // no measurement for a full second; no vector available - return false; + if (have_los_meas()) { + ret = _los_meas_body; + return true; } - ret = _los_meas_body; - return true; + return false; } #endif // AC_PRECLAND_SITL_ENABLED diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index 53b0c23aa6..736adfbdb1 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -33,7 +33,7 @@ public: uint32_t los_meas_time_ms() override { return _los_meas_time_ms; } // return true if there is a valid los measurement available - bool have_los_meas() override; + bool have_los_meas() override { return _have_los_meas; } // returns distance to target in meters (0 means distance is not known) float distance_to_target() override { return _distance_to_target; }