From 9be519396506ec777fc5f77d7d5294c732b1fe64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 May 2017 23:54:54 +0200 Subject: [PATCH] Navigator: Fix RTL command lnd logic for missions The navigator was sending RTL commands in the wrong circumstances leading to a cycle between Navigator and Commander. --- src/modules/navigator/mission.cpp | 3 +-- src/modules/navigator/navigator_main.cpp | 10 ---------- src/modules/navigator/rtl.cpp | 5 ----- 3 files changed, 1 insertion(+), 17 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c6e31fad3a..18052b60d2 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -858,8 +858,7 @@ Mission::do_need_vertical_takeoff() _mission_item.nav_cmd == NAV_CMD_WAYPOINT || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { _need_takeoff = false; return true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a1deb2b2dc..ff64c52b39 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -481,16 +481,6 @@ Navigator::task_main() PX4_WARN("planned landing not available"); } - } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH) { - - vehicle_command_s vcmd = {}; - vcmd.target_system = get_vstatus()->system_id; - vcmd.target_component = get_vstatus()->component_id; - vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH; - - orb_advert_t pub = orb_advertise_queue(ORB_ID(vehicle_command), &vcmd, vehicle_command_s::ORB_QUEUE_LENGTH); - (void)orb_unadvertise(pub); - } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (get_mission_result()->valid && diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index e9418335eb..c1f705f66f 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -300,11 +300,6 @@ RTL::set_rtl_item() reset_mission_item_reached(); - /* execute command if set */ - if (!item_contains_position(&_mission_item)) { - issue_command(&_mission_item); - } - /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false;