From daf427f6d612614f08a1ad770c42109f989e8dc5 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 26 Sep 2023 16:44:13 +1000 Subject: [PATCH] Sub: accept MAV_CMD_DO_MOTOR_TEST as both command-int and command-long --- ArduSub/GCS_Mavlink.cpp | 17 ++++++++++------- ArduSub/GCS_Mavlink.h | 1 + ArduSub/Sub.h | 2 +- ArduSub/motors.cpp | 6 +++--- 4 files changed, 15 insertions(+), 11 deletions(-) diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 955b831078..8bbaba74cf 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_MOTOR_TEST: + return handle_MAV_CMD_DO_MOTOR_TEST(packet); + case MAV_CMD_NAV_LOITER_UNLIM: return handle_MAV_CMD_NAV_LOITER_UNLIM(packet); @@ -526,7 +529,13 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon } return MAV_RESULT_FAILED; - case MAV_CMD_DO_MOTOR_TEST: + default: + return GCS_MAVLINK::handle_command_long_packet(packet, msg); + } +} + +MAV_RESULT GCS_MAVLINK_Sub::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet) +{ // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle) // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum) // param3 : throttle (range depends upon param2) @@ -535,14 +544,8 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon return MAV_RESULT_FAILED; } return MAV_RESULT_ACCEPTED; - - default: - return GCS_MAVLINK::handle_command_long_packet(packet, msg); - } } - - void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg) { switch (msg.msgid) { diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index e24be72af7..17a3e77a8d 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -53,6 +53,7 @@ private: int16_t vfr_hud_throttle() const override; + 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); diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 351949b33a..2e4c3a4136 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -530,7 +530,7 @@ private: uint16_t get_pilot_speed_dn() const; void convert_old_parameters(void); - bool handle_do_motor_test(mavlink_command_long_t command); + bool handle_do_motor_test(mavlink_command_int_t command); bool init_motor_test(); bool verify_motor_test(); diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 9e25f51513..895ade8c8c 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -78,7 +78,7 @@ bool Sub::verify_motor_test() return true; } -bool Sub::handle_do_motor_test(mavlink_command_long_t command) { +bool Sub::handle_do_motor_test(mavlink_command_int_t command) { last_do_motor_test_ms = AP_HAL::millis(); // If we are not already testing motors, initialize test @@ -103,9 +103,9 @@ bool Sub::handle_do_motor_test(mavlink_command_long_t command) { float throttle = command.param3; // float timeout_s = command.param4; // not used // float motor_count = command.param5; // not used - float test_type = command.param6; + const uint32_t test_type = command.y; - if (!is_equal(test_type, (float)MOTOR_TEST_ORDER_BOARD)) { + if (test_type != MOTOR_TEST_ORDER_BOARD) { gcs().send_text(MAV_SEVERITY_WARNING, "bad test type %0.2f", (double)test_type); return false; // test type not supported here }