From ee316f04ed847e17c2f7f784cfb2c97dc0a0d778 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 12 Sep 2023 12:46:10 +1000 Subject: [PATCH] Plane: allow DO_VTOL_TRANSITION as both LONG and INT commands --- ArduPlane/GCS_Mavlink.cpp | 21 +++++++++++++++------ ArduPlane/GCS_Mavlink.h | 1 + 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 07afcfdeeb..9fed6ba017 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -984,6 +984,11 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in case MAV_CMD_DO_CHANGE_SPEED: return handle_command_DO_CHANGE_SPEED(packet); +#if HAL_QUADPLANE_ENABLED + case MAV_CMD_DO_VTOL_TRANSITION: + return handle_command_DO_VTOL_TRANSITION(packet); +#endif + default: return GCS_MAVLINK::handle_command_int_packet(packet, msg); } @@ -1094,12 +1099,6 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l (uint16_t)packet.param3, packet.param4, (uint8_t)packet.param5); - - case MAV_CMD_DO_VTOL_TRANSITION: - if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { - return MAV_RESULT_FAILED; - } - return MAV_RESULT_ACCEPTED; #endif default: @@ -1107,6 +1106,16 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l } } +#if HAL_QUADPLANE_ENABLED +MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet) +{ + if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) { + return MAV_RESULT_FAILED; + } + return MAV_RESULT_ACCEPTED; +} +#endif + // this is called on receipt of a MANUAL_CONTROL packet and is // expected to call manual_override to override RC input on desired // axes. diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index ba0d0c8f65..469644c1b6 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -55,6 +55,7 @@ private: MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); MAV_RESULT handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet); MAV_RESULT handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet); bool try_send_message(enum ap_message id) override; void packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) override;