Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: accept MAV_CMD_DO_MOTOR_TEST as both command-int and command-long

This commit is contained in:
Peter Barker 2023-09-26 16:44:13 +10:00 committed by Andrew Tridgell
parent 67b9a50153
commit daf427f6d6
4 changed files with 15 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduSub/GCS_Mavlink.cpp
View File

@ -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) {

1
ArduSub/GCS_Mavlink.h
View File

@ -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);

2
ArduSub/Sub.h
View File

@ -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();

6
ArduSub/motors.cpp
View File

@ -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
}