diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7e375bc57e..8db4482180 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -582,7 +582,7 @@ void Navigator::run() case RTL::RTL_TYPE_MISSION_LANDING: case RTL::RTL_TYPE_CLOSEST: - if (!rtl_activated && _rtl.getLoiterDone() + if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER && _rtl.getShouldEngageMissionForLanding()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index eb787fba88..80c95e219e 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -241,8 +241,6 @@ void RTL::find_RTL_destination() void RTL::on_activation() { - setLoiterDone(false); - // if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission _should_engange_mission_for_landing = (_destination.type == RTL_DESTINATION_MISSION_LANDING) @@ -291,8 +289,6 @@ void RTL::on_activation() _rtl_state = RTL_STATE_RETURN; } - setLoiterDone(_rtl_state > RTL_STATE_LOITER); - // reset cruising speed and throttle to default for RTL _navigator->set_cruising_speed(); _navigator->set_cruising_throttle(); @@ -641,8 +637,6 @@ void RTL::advance_rtl() case RTL_STATE_LOITER: - setLoiterDone(true); - if (vtol_in_fw_mode) { _rtl_state = RTL_STATE_TRANSITION_TO_MC; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 49e2a46e22..d3b0510cf7 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -82,6 +82,19 @@ public: RTL_CURRENT_HEADING, }; + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + }; + void on_inactivation() override; void on_inactive() override; void on_activation() override; @@ -93,13 +106,12 @@ public: int get_rtl_type() const { return _param_rtl_type.get(); } - void setLoiterDone(bool done) { _loiter_done = done; } - - bool getLoiterDone() { return _loiter_done; } - void get_rtl_xy_z_speed(float &xy, float &z); + matrix::Vector2f get_wind(); + RTLState getRTLState() { return _rtl_state; } + bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; } private: @@ -121,18 +133,7 @@ private: float getHoverLandSpeed(); - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - RTL_STATE_HEAD_TO_CENTER, - } _rtl_state{RTL_STATE_NONE}; + RTLState _rtl_state{RTL_STATE_NONE}; struct RTLPosition { double lat; @@ -160,7 +161,6 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending - bool _loiter_done{false}; // this flag is set to true if RTL is active and we are past the loiter state bool _rtl_alt_min{false}; bool _should_engange_mission_for_landing{false};