diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 87089fcea3..0dfb69affc 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1626,7 +1626,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_CMD_DO_VTOL_TRANSITION: - result = plane.quadplane.handle_do_vtol_transition(packet); + if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { + result = MAV_RESULT_FAILED; + } else { + result = MAV_RESULT_ACCEPTED; + } break; default: diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index ce8454ef8a..49db030bdc 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -218,6 +218,10 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd) cmd.content.mount_control.yaw); break; #endif + + case MAV_CMD_DO_VTOL_TRANSITION: + plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)cmd.content.do_vtol_transition.target_state); + break; } return true; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index e077a87702..7be9cadcf5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -948,33 +948,38 @@ bool QuadPlane::init_mode(void) /* handle a MAVLink DO_VTOL_TRANSITION */ -bool QuadPlane::handle_do_vtol_transition(const mavlink_command_long_t &packet) +bool QuadPlane::handle_do_vtol_transition(enum MAV_VTOL_STATE state) { if (!available()) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL not available"); - return MAV_RESULT_FAILED; + return false; } if (plane.control_mode != AUTO) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "VTOL transition only in AUTO"); - return MAV_RESULT_FAILED; + return false; } - switch ((uint8_t)packet.param1) { + switch (state) { case MAV_VTOL_STATE_MC: if (!plane.auto_state.vtol_mode) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Entered VTOL mode"); } plane.auto_state.vtol_mode = true; - return MAV_RESULT_ACCEPTED; + return true; + case MAV_VTOL_STATE_FW: if (plane.auto_state.vtol_mode) { plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Exited VTOL mode"); } plane.auto_state.vtol_mode = false; - return MAV_RESULT_ACCEPTED; + + return true; + + default: + break; } plane.gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Invalid VTOL mode"); - return MAV_RESULT_FAILED; + return false; } /* diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f0c8525e60..d60d82653b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -42,7 +42,7 @@ public: return available() && assisted_flight; } - bool handle_do_vtol_transition(const mavlink_command_long_t &packet); + bool handle_do_vtol_transition(enum MAV_VTOL_STATE state); bool do_vtol_takeoff(const AP_Mission::Mission_Command& cmd); bool do_vtol_land(const AP_Mission::Mission_Command& cmd);