From 5a1a4acab244b4a6d31f3d9a9f465fbcee70e0e6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 4 Oct 2023 09:32:50 +1100 Subject: [PATCH] Sub: accept DO_CHANGE_SPEED as both command_long and command_int --- ArduSub/GCS_Mavlink.cpp | 15 ++++++++++----- ArduSub/GCS_Mavlink.h | 1 + 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 8f027b034c..75cf28f88b 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -465,6 +465,9 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_ { switch(packet.command) { + case MAV_CMD_DO_CHANGE_SPEED: + return handle_MAV_CMD_DO_CHANGE_SPEED(packet); + case MAV_CMD_DO_MOTOR_TEST: return handle_MAV_CMD_DO_MOTOR_TEST(packet); @@ -515,7 +518,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon } return MAV_RESULT_FAILED; - case MAV_CMD_DO_CHANGE_SPEED: + default: + return GCS_MAVLINK::handle_command_long_packet(packet, msg); + } +} + +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet) +{ // param1 : unused // param2 : new speed in m/s // param3 : unused @@ -525,10 +534,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; - - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } } MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet) diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 2177c917b0..6d043d5ba4 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -54,6 +54,7 @@ private: int16_t vfr_hud_throttle() const override; MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet); + MAV_RESULT handle_MAV_CMD_DO_CHANGE_SPEED(const mavlink_command_int_t &packet); MAV_RESULT handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet); MAV_RESULT handle_MAV_CMD_NAV_LOITER_UNLIM(const mavlink_command_int_t &packet); MAV_RESULT handle_MAV_CMD_NAV_LAND(const mavlink_command_int_t &packet);