From a4a03e86dad826e6d3fc2c7ace7d0573930dc652 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 9 Oct 2020 13:49:15 +0300 Subject: [PATCH] mission: be more intelligent about saying that we are on a mission landing - previously the decision of being on a landing pattern was taken by just looking at the current mission index. However, even if the current mission index indicates a landing pattern the vehicle could be at an arbitrary location, far from being established on the pattern. Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 23 +++++++++++++++++++++-- src/modules/navigator/mission.h | 2 ++ src/modules/navigator/navigator.h | 8 +++++++- src/modules/navigator/navigator_main.cpp | 9 +++++++-- src/modules/navigator/rtl.cpp | 8 +++++++- src/modules/navigator/rtl.h | 5 +++++ 6 files changed, 49 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f69d9cb9ab..d55575e0e8 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -58,6 +58,9 @@ #include #include #include +#include + +using namespace time_literals; Mission::Mission(Navigator *navigator) : MissionBlock(navigator), @@ -73,6 +76,12 @@ Mission::on_inactive() * is used for missions such as RTL. */ _navigator->set_cruising_speed(); + // if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid + // this prevents RTL to just continue at the current mission index + if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) { + _navigator->setMissionLandingInProgress(false); + } + /* Without home a mission can't be valid yet anyway, let's wait. */ if (!_navigator->home_position_valid()) { return; @@ -147,6 +156,8 @@ Mission::on_inactivation() if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } + + _time_mission_deactivated = hrt_absolute_time(); } void @@ -421,12 +432,15 @@ Mission::land_start() { // if not currently landing, jump to do_land_start if (_land_start_available) { - if (landing()) { + if (_navigator->getMissionLandingInProgress()) { return true; } else { set_current_mission_index(get_land_start_index()); - return landing(); + + const bool can_land_now = landing(); + _navigator->setMissionLandingInProgress(can_land_now); + return can_land_now; } } @@ -1626,6 +1640,11 @@ Mission::set_mission_item_reached() _navigator->get_mission_result()->seq_reached = _current_mission_index; _navigator->set_mission_result_updated(); + // let the navigator know that we are currently executing the mission landing. + // Using the method landing() itself is not accurate as it only give information about the mission index + // but the vehicle could still be very far from the actual landing items + _navigator->setMissionLandingInProgress(landing()); + reset_mission_item_reached(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 29509de4ba..c8b59450e8 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -255,6 +255,8 @@ private: bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + hrt_abstime _time_mission_deactivated{0}; + enum { MISSION_TYPE_NONE, MISSION_TYPE_MISSION diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 430475fdcb..05bb60a831 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -269,7 +269,10 @@ public: void set_mission_failure(const char *reason); - // MISSION + void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; } + + bool getMissionLandingInProgress() { return _mission_landing_in_progress; } + bool is_planned_mission() const { return _navigation_mode == &_mission; } bool on_mission_landing() { return _mission.landing(); } bool start_mission_landing() { return _mission.land_start(); } @@ -396,6 +399,9 @@ private: float _mission_cruising_speed_fw{-1.0f}; float _mission_throttle{NAN}; + bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern + // if mission mode is inactive, this flag will be cleared after 2 seconds + // update subscriptions void params_update(); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 2277ff23c7..8214669d7a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -563,9 +563,14 @@ Navigator::run() } - // if RTL is set to use a mission landing and mission has a planned landing, then use MISSION to fly there directly - if (on_mission_landing() && !get_land_detected()->landed) { + + if (!rtl_activated && _rtl.initialClimbDone() && get_mission_start_land_available()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); + + if (!getMissionLandingInProgress()) { + start_mission_landing(); + } + navigation_mode_new = &_mission; } else { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6d1a3a9b05..d51b817af9 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -225,8 +225,10 @@ void RTL::on_activation() // For safety reasons don't go into RTL if landed. _rtl_state = RTL_STATE_LANDED; - } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->on_mission_landing()) { + } else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) { // RTL straight to RETURN state, but mission will takeover for landing. + _rtl_state = RTL_STATE_RETURN; + } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { @@ -239,7 +241,10 @@ void RTL::on_activation() _rtl_state = RTL_STATE_RETURN; } + setInitialClimbDone(_rtl_state != RTL_STATE_CLIMB); + set_rtl_item(); + } void RTL::on_active() @@ -476,6 +481,7 @@ void RTL::advance_rtl() switch (_rtl_state) { case RTL_STATE_CLIMB: + setInitialClimbDone(true); _rtl_state = RTL_STATE_RETURN; break; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 8586f29557..56418990de 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -83,6 +83,10 @@ public: int rtl_destination(); + void setInitialClimbDone(bool done) { _initial_climb_done = done; } + + bool initialClimbDone() { return _initial_climb_done; } + private: /** * Set the RTL item @@ -134,6 +138,7 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; + bool _initial_climb_done{false}; // this flag is set to true if RTL is active and we are past the climb state DEFINE_PARAMETERS( (ParamFloat) _param_rtl_return_alt,