From 76c6d537ed56f19ed678b486c6ea0661de738ed5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 25 Aug 2023 10:15:32 +1000 Subject: [PATCH] Rover: support running MAV_CMD_NAV_SET_YAW_SPEED as command_int --- Rover/GCS_Mavlink.cpp | 14 ++++---------- Rover/GCS_Mavlink.h | 2 +- 2 files changed, 5 insertions(+), 11 deletions(-) diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 0c7e16d088..70ace4b5c2 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -680,17 +680,16 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in } return MAV_RESULT_FAILED; + case MAV_CMD_NAV_SET_YAW_SPEED: + return handle_command_nav_set_yaw_speed(packet, msg); + default: return GCS_MAVLINK::handle_command_int_packet(packet, msg); } } -MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) +MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - switch (packet.command) { - - case MAV_CMD_NAV_SET_YAW_SPEED: - { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // param3 : 0 = absolute, 1 = relative @@ -709,11 +708,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l rover.mode_guided.set_desired_heading_and_speed(packet.param1 * 100.0f, packet.param2); } return MAV_RESULT_ACCEPTED; - } - - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } } MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet) diff --git a/Rover/GCS_Mavlink.h b/Rover/GCS_Mavlink.h index 3e7ccacc31..14ab0a8194 100644 --- a/Rover/GCS_Mavlink.h +++ b/Rover/GCS_Mavlink.h @@ -17,8 +17,8 @@ protected: MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg) override; - MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg) override; MAV_RESULT handle_command_int_do_reposition(const mavlink_command_int_t &packet); + MAV_RESULT handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg); void send_position_target_global_int() override;