From 5c021d8fa482f734e929cb9cf56d026035070b02 Mon Sep 17 00:00:00 2001 From: Konrad Date: Wed, 16 Aug 2023 14:35:40 +0200 Subject: [PATCH] RTL: Keep only the selected RTL type in memory. --- src/modules/navigator/rtl.cpp | 158 ++++++++++-------- src/modules/navigator/rtl.h | 28 +++- src/modules/navigator/rtl_base.h | 57 +++++++ src/modules/navigator/rtl_direct.cpp | 4 +- src/modules/navigator/rtl_direct.h | 8 +- .../navigator/rtl_direct_mission_land.cpp | 6 +- .../navigator/rtl_direct_mission_land.h | 12 +- src/modules/navigator/rtl_mission_fast.cpp | 2 +- src/modules/navigator/rtl_mission_fast.h | 6 +- .../navigator/rtl_mission_fast_reverse.cpp | 2 +- .../navigator/rtl_mission_fast_reverse.h | 6 +- 11 files changed, 192 insertions(+), 97 deletions(-) create mode 100644 src/modules/navigator/rtl_base.h diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f5092d0a6d..d28952c9f5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -49,13 +49,12 @@ using namespace time_literals; using namespace math; +static constexpr float MIN_DIST_THRESHOLD = 2.f; + RTL::RTL(Navigator *navigator) : NavigatorMode(navigator), ModuleParams(navigator), - _rtl_direct(navigator), - _rtl_direct_mission_land(navigator), - _rtl_mission(navigator), - _rtl_mission_reverse(navigator) + _rtl_direct(navigator) { } @@ -162,22 +161,16 @@ void RTL::updateDatamanCache() void RTL::on_inactivation() { switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - _rtl_mission.on_inactivation(); - break; - - case RtlType::RTL_MISSION_FAST_REVERSE: - _rtl_mission_reverse.on_inactivation(); + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactivation(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_inactivation(); break; - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_direct_mission_land.on_inactivation(); - break; - default: break; } @@ -194,10 +187,20 @@ void RTL::on_inactive() parameters_update(); - _rtl_mission.on_inactive(); - _rtl_mission_reverse.on_inactive(); - _rtl_direct.on_inactive(); - _rtl_direct_mission_land.on_inactive(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactive(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactive(); + break; + + default: + break; + } // Limit inactive calculation to 1Hz hrt_abstime now{hrt_absolute_time()}; @@ -214,16 +217,14 @@ void RTL::on_inactive() if (_navigator->home_global_position_valid() && global_position_recently_updated) { switch (_rtl_type) { - case RtlType::RTL_DIRECT: estimated_time = _rtl_direct.calc_rtl_time_estimate(); + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); break; - case RtlType::RTL_DIRECT_MISSION_LAND: estimated_time = _rtl_direct_mission_land.calc_rtl_time_estimate(); - break; - - case RtlType::RTL_MISSION_FAST: estimated_time = _rtl_mission.calc_rtl_time_estimate(); - break; - - case RtlType::RTL_MISSION_FAST_REVERSE: estimated_time = _rtl_mission_reverse.calc_rtl_time_estimate(); + case RtlType::RTL_DIRECT_MISSION_LAND: + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); break; default: break; @@ -239,21 +240,18 @@ void RTL::on_activation() setRtlTypeAndDestination(); switch (_rtl_type) { - case RtlType::RTL_MISSION_FAST: - _rtl_mission.on_activation(); - break; - + case RtlType::RTL_DIRECT_MISSION_LAND: // Fall through + case RtlType::RTL_MISSION_FAST: // Fall through case RtlType::RTL_MISSION_FAST_REVERSE: - _rtl_mission_reverse.on_activation(); + _rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt); + _rtl_mission_type_handle->on_activation(); break; case RtlType::RTL_DIRECT: - _rtl_direct.on_activation(_enforce_rtl_alt); + _rtl_direct.setReturnAltMin(_enforce_rtl_alt); + _rtl_direct.on_activation(); break; - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_direct_mission_land.on_activation(_enforce_rtl_alt); - break; default: break; @@ -271,31 +269,13 @@ void RTL::on_active() switch (_rtl_type) { case RtlType::RTL_MISSION_FAST: - _rtl_mission.on_active(); - _rtl_mission_reverse.on_inactive(); - _rtl_direct.on_inactive(); - _rtl_direct_mission_land.on_inactive(); - break; - case RtlType::RTL_MISSION_FAST_REVERSE: - _rtl_mission_reverse.on_active(); - _rtl_mission.on_inactive(); - _rtl_direct.on_inactive(); - _rtl_direct_mission_land.on_inactive(); + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_active(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_active(); - _rtl_direct_mission_land.on_inactive(); - _rtl_mission_reverse.on_inactive(); - _rtl_mission.on_inactive(); - break; - - case RtlType::RTL_DIRECT_MISSION_LAND: - _rtl_direct_mission_land.on_active(); - _rtl_mission_reverse.on_inactive(); - _rtl_mission.on_inactive(); - _rtl_direct.on_inactive(); break; default: @@ -305,15 +285,10 @@ void RTL::on_active() void RTL::setRtlTypeAndDestination() { - if (_param_rtl_type.get() == 2) { - if (hasMissionLandStart()) { - _rtl_type = RtlType::RTL_MISSION_FAST; - } else { - _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE; - } + init_rtl_mission_type(); - } else { + if (_param_rtl_type.get() != 2) { // check the closest allowed destination. bool isMissionLanding{false}; RtlDirect::RtlPosition rtl_position; @@ -321,13 +296,13 @@ void RTL::setRtlTypeAndDestination() findRtlDestination(isMissionLanding, rtl_position, rtl_alt); if (isMissionLanding) { - _rtl_direct_mission_land.setRtlAlt(rtl_alt); _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; + _rtl_mission_type_handle->setRtlAlt(rtl_alt); } else { + _rtl_type = RtlType::RTL_DIRECT; _rtl_direct.setRtlAlt(rtl_alt); _rtl_direct.setRtlPosition(rtl_position); - _rtl_type = RtlType::RTL_DIRECT; } } } @@ -372,7 +347,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; - if (dist < min_dist) { + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { min_dist = dist; setLandPosAsDestination(rtl_position, land_mission_item); isMissionLanding = true; @@ -395,7 +370,7 @@ void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; - if (dist < min_dist) { + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { min_dist = dist; setSafepointAsDestination(rtl_position, mission_safe_point); isMissionLanding = false; @@ -485,6 +460,53 @@ float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPositio return max(return_altitude_amsl, _global_pos_sub.get().alt); } +void RTL::init_rtl_mission_type() +{ + RtlType new_rtl_mission_type{RtlType::RTL_DIRECT_MISSION_LAND}; + + if (_param_rtl_type.get() == 2) { + if (hasMissionLandStart()) { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST; + + } else { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; + } + } + + if (_set_rtl_mission_type == new_rtl_mission_type) { + return; + } + + if (_rtl_mission_type_handle) { + delete _rtl_mission_type_handle; + _rtl_mission_type_handle = nullptr; + _set_rtl_mission_type = RtlType::NONE; + } + + switch (new_rtl_mission_type) { + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle = new RtlDirectMissionLand(_navigator); + _set_rtl_mission_type = RtlType::RTL_DIRECT_MISSION_LAND; + // RTL type is either direct or mission land have to set it later. + break; + + case RtlType::RTL_MISSION_FAST: + _rtl_mission_type_handle = new RtlMissionFast(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST; + _rtl_type = RtlType::RTL_MISSION_FAST; + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;; + break; + + default: + break; + } +} + void RTL::parameters_update() { if (_parameter_update_sub.updated()) { @@ -495,7 +517,9 @@ void RTL::parameters_update() // this class attributes need updating (and do so). updateParams(); - setRtlTypeAndDestination(); + if (!isActive()) { + setRtlTypeAndDestination(); + } } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 76cef15e9f..fab59e65d6 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -45,6 +45,7 @@ #include "navigator_mode.h" #include +#include "rtl_base.h" #include "rtl_direct.h" #include "rtl_direct_mission_land.h" #include "rtl_mission_fast.h" @@ -68,6 +69,7 @@ public: ~RTL() = default; enum class RtlType { + NONE, RTL_DIRECT, RTL_DIRECT_MISSION_LAND, RTL_MISSION_FAST, @@ -115,13 +117,24 @@ private: void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point); /** - * @brief + * @brief calculate return altitude from cone half angle * - * @param cone_half_angle_deg - * @return float + * @param[in] rtl_position landing position of the rtl + * @param[in] cone_half_angle_deg half angle of the cone [deg] + * @return return altitude */ float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); + /** + * @brief initialize RTL mission type + * + */ + void init_rtl_mission_type(); + + /** + * @brief Update parameters + * + */ void parameters_update(); enum class DatamanState { @@ -134,6 +147,9 @@ private: hrt_abstime _destination_check_time{0}; + RtlBase *_rtl_mission_type_handle{nullptr}; + RtlType _set_rtl_mission_type{RtlType::NONE}; + RtlType _rtl_type{RtlType::RTL_DIRECT}; DatamanState _dataman_state{DatamanState::UpdateRequestWait}; @@ -150,12 +166,6 @@ private: RtlDirect _rtl_direct; - RtlDirectMissionLand _rtl_direct_mission_land; - - RtlMissionFast _rtl_mission; - - RtlMissionFastReverse _rtl_mission_reverse; - bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h new file mode 100644 index 0000000000..dc698687fc --- /dev/null +++ b/src/modules/navigator/rtl_base.h @@ -0,0 +1,57 @@ +/*************************************************************************** + * + * Copyright (c) 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file rtl_base.h + * + * Helper class for RTL modes using the mission + * + */ + +#pragma once + +#include "mission_base.h" +#include + +class RtlBase : public MissionBase +{ +public: + RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): + MissionBase(navigator, dataman_cache_size_signed) {}; + virtual ~RtlBase() = default; + + virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; + + virtual void setReturnAltMin(bool min) { (void)min;}; + + virtual void setRtlAlt(float alt) { (void)alt;}; +}; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 0b1d8442f5..22c79f7161 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -74,7 +74,7 @@ void RtlDirect::on_inactivation() } } -void RtlDirect::on_activation(bool enforce_rtl_alt) +void RtlDirect::on_activation() { _global_pos_sub.update(); _local_pos_sub.update(); @@ -87,7 +87,7 @@ void RtlDirect::on_activation(bool enforce_rtl_alt) // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; - } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || enforce_rtl_alt) { + } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) { // If lower than return altitude, climb up first. // If rtl_alt_min is true then forcing altitude change even if above. diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 892421cb22..94ada8f514 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -88,12 +88,11 @@ public: void on_inactivation() override; /** - * @brief On activation. + * @brief on activation. * Initialize the return to launch calculations. * - * @param[in] enforce_rtl_alt boolean if the minimal return to launch altitude should be enforced at the beginning of the return, even when the current vehicle altitude is above. */ - void on_activation(bool enforce_rtl_alt); + void on_activation() override; /** * @brief on active @@ -109,6 +108,7 @@ public: */ rtl_time_estimate_s calc_rtl_time_estimate(); + void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } void setRtlAlt(float alt) {_rtl_alt = alt;}; void setRtlPosition(RtlPosition position) {_destination = position;}; @@ -204,6 +204,8 @@ private: /** Current state in the state machine.*/ RTLState _rtl_state{RTL_STATE_NONE}; + bool _enforce_rtl_alt{false}; + RtlPosition _destination{}; ///< the RTL position to fly to float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index 7eb5c54814..236b7b61c5 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -47,12 +47,12 @@ static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5; RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : - MissionBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE) + RtlBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE) { } -void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt) +void RtlDirectMissionLand::on_activation() { _land_detected_sub.update(); _global_pos_sub.update(); @@ -62,7 +62,7 @@ void RtlDirectMissionLand::on_activation(bool enforce_rtl_alt) if (hasMissionLandStart()) { _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK); - if ((_global_pos_sub.get().alt < _rtl_alt) || enforce_rtl_alt) { + if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { // If lower than return altitude, climb up first. // If enforce_rtl_alt is true then forcing altitude change even if above. diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h index 1642a61b5a..002d26da67 100644 --- a/src/modules/navigator/rtl_direct_mission_land.h +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -41,7 +41,7 @@ #pragma once -#include "mission.h" +#include "rtl_base.h" #include #include @@ -49,17 +49,18 @@ class Navigator; -class RtlDirectMissionLand : public MissionBase +class RtlDirectMissionLand : public RtlBase { public: RtlDirectMissionLand(Navigator *navigator); ~RtlDirectMissionLand() = default; - void on_activation(bool enforce_rtl_alt); + void on_activation() override; - rtl_time_estimate_s calc_rtl_time_estimate(); + rtl_time_estimate_s calc_rtl_time_estimate() override; - void setRtlAlt(float alt) {_rtl_alt = alt;}; + void setReturnAltMin(bool min) override { _enforce_rtl_alt = min; }; + void setRtlAlt(float alt) override {_rtl_alt = alt;}; private: bool setNextMissionItem() override; @@ -68,5 +69,6 @@ private: bool do_need_move_to_land(); bool _needs_climbing{false}; //< Flag if climbing is required at the start + bool _enforce_rtl_alt{false}; float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position }; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index f667e3e79f..924ef8b8ba 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -47,7 +47,7 @@ static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; RtlMissionFast::RtlMissionFast(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) + RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) { } diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index 0b6f689041..576152efe2 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -41,7 +41,7 @@ #pragma once -#include "mission_base.h" +#include "rtl_base.h" #include #include @@ -49,7 +49,7 @@ class Navigator; -class RtlMissionFast : public MissionBase +class RtlMissionFast : public RtlBase { public: RtlMissionFast(Navigator *navigator); @@ -59,7 +59,7 @@ public: void on_active() override; void on_inactive() override; - rtl_time_estimate_s calc_rtl_time_estimate(); + rtl_time_estimate_s calc_rtl_time_estimate() override; private: bool setNextMissionItem() override; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 42c014ac74..1b5feb850d 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -47,7 +47,7 @@ static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : - MissionBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) + RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) { } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index e4dba5312b..acc2893b3a 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -41,7 +41,7 @@ #pragma once -#include "mission_base.h" +#include "rtl_base.h" #include #include @@ -49,7 +49,7 @@ class Navigator; -class RtlMissionFastReverse : public MissionBase +class RtlMissionFastReverse : public RtlBase { public: RtlMissionFastReverse(Navigator *navigator); @@ -59,7 +59,7 @@ public: void on_active() override; void on_inactive() override; - rtl_time_estimate_s calc_rtl_time_estimate(); + rtl_time_estimate_s calc_rtl_time_estimate() override; private: bool setNextMissionItem() override;