diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7bebf1a374..9f8d2f7962 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -46,6 +46,8 @@ static constexpr float DELAY_SIGMA = 0.01f; +using namespace time_literals; + RTL::RTL(Navigator *navigator) : MissionBlock(navigator), ModuleParams(navigator) @@ -73,6 +75,13 @@ RTL::on_inactive() void RTL::find_RTL_destination() { + // don't update RTL destination faster than 1 Hz + if (hrt_elapsed_time(&_destination_check_time) < 1_s) { + return; + } + + _destination_check_time = hrt_absolute_time(); + // get home position: home_position_s &home_landing_position = *_navigator->get_home_position(); // get global position diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index d68a5a1ad3..c5a6694659 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -129,6 +129,8 @@ private: RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) + hrt_abstime _destination_check_time{0}; + float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false};