diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4d4feb0c36..3e876426a5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -520,7 +520,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi // if already flying (armed and !landed) treat TAKEOFF like regular POSITION if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) - && !_navigator->get_land_detected()->landed) { + && !_navigator->get_land_detected()->landed && !_navigator->get_land_detected()->maybe_landed) { sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 7732fad241..86ce3b9b7b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -72,8 +72,9 @@ RTL::rtl_type() const void RTL::on_activation() { - if (_navigator->get_land_detected()->landed) { - // for safety reasons don't go into RTL if landed + if (_navigator->get_land_detected()->landed + || (_navigator->get_position_setpoint_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) { + // for safety reasons don't go into RTL if landed or currently in landing _rtl_state = RTL_STATE_LANDED; } else if ((rtl_type() == RTL_LAND) && _navigator->on_mission_landing()) {