From 2c6b3eeb028eeb050cb661b31e3d46b99c5218d2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 6 Jan 2021 18:50:32 +0100 Subject: [PATCH] Navigator: NAV_CMD_DO_VTOL_TRANSITION: accept once in correct vtol state Signed-off-by: Silvan Fuhrer --- src/modules/navigator/mission_block.cpp | 17 +++++++---------- src/modules/navigator/mission_block.h | 1 - 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6230d8141f..01910affba 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -112,18 +112,16 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_DO_VTOL_TRANSITION: - /* - * We wait half a second to give the transition command time to propagate. - * Then monitor the transition status for completion. - */ - // TODO: check desired transition state achieved and drop _action_start - if (hrt_absolute_time() - _action_start > 500000 && - !_navigator->get_vstatus()->in_transition_mode) { + if (int(_mission_item.params[0]) == 3) { + // transition to RW requested, only accept waypoint if vehicle state has changed accordingly + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - _action_start = 0; - return true; + } else if (int(_mission_item.params[0]) == 4) { + // transition to FW requested, only accept waypoint if vehicle state has changed accordingly + return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; } else { + // invalid vtol transition request return false; } @@ -473,7 +471,6 @@ MissionBlock::issue_command(const mission_item_s &item) _actuator_pub.publish(actuators); } else { - _action_start = hrt_absolute_time(); // This is to support legacy DO_MOUNT_CONTROL as part of a mission. if (item.nav_cmd == NAV_CMD_DO_MOUNT_CONTROL) { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index be87ea307a..2de76f1f8f 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -149,7 +149,6 @@ protected: bool _waypoint_position_reached{false}; bool _waypoint_yaw_reached{false}; - hrt_abstime _action_start{0}; hrt_abstime _time_wp_reached{0}; uORB::Publication _actuator_pub{ORB_ID(actuator_controls_2)};