From 504794eddcd91f263431901e4b86c684987d4743 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Wed, 20 May 2020 21:54:31 +0200 Subject: [PATCH] VTOL: reject transition command in certain flight modes --- .../vtol_att_control_main.cpp | 21 ++++++++++++++----- .../vtol_att_control/vtol_att_control_main.h | 3 ++- 2 files changed, 18 insertions(+), 6 deletions(-) diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index c8ee3a1456..e30cdbb22b 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -156,18 +156,29 @@ VtolAttitudeControl::vehicle_cmd_poll() void VtolAttitudeControl::handle_command() { - // update transition command if necessary if (_vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { - _transition_command = int(_vehicle_cmd.param1 + 0.5f); - // Report that we have received the command no matter what we actually do with it. - // This might not be optimal but is better than no response at all. + vehicle_status_s vehicle_status = {}; + _vehicle_status_sub.copy(&vehicle_status); + + uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + + // deny any transition in auto takeoff mode, plus transition from RW to FW in land or RTL mode + if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF + || (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND + || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL))) { + result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED; + + } else { + _transition_command = int(_vehicle_cmd.param1 + 0.5f); + } if (_vehicle_cmd.from_external) { vehicle_command_ack_s command_ack{}; command_ack.timestamp = hrt_absolute_time(); command_ack.command = _vehicle_cmd.command; - command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED; + command_ack.result = result; command_ack.target_system = _vehicle_cmd.source_system; command_ack.target_component = _vehicle_cmd.source_component; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 08b1ea7d36..093bc4cca8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -79,7 +79,7 @@ #include #include #include - +#include #include "standard.h" #include "tailsitter.h" #include "tiltrotor.h" @@ -146,6 +146,7 @@ private: uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)}; //vehicle attitude subscription uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; //vehicle control mode subscription uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _actuators_0_pub{ORB_ID(actuator_controls_0)}; //input for the mixer (roll,pitch,yaw,thrust) uORB::Publication _actuators_1_pub{ORB_ID(actuator_controls_1)};