From 376494027b531ec0dc9e4e45a5406a5fc0a959b9 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 14 Jul 2020 00:22:35 -0700 Subject: [PATCH] Copter: Support higher resolution percent based motor tests --- ArduCopter/Copter.h | 2 +- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/motor_test.cpp | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index e274208012..a4d1773fef 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -801,7 +801,7 @@ private: // motor_test.cpp void motor_test_output(); bool mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc); - MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec, uint8_t motor_count); + MAV_RESULT mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count); void motor_test_stop(); // motors.cpp diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b7bc4d17c0..f1dfbb66c8 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -808,7 +808,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_ return copter.mavlink_motor_test_start(*this, (uint8_t)packet.param1, (uint8_t)packet.param2, - (uint16_t)packet.param3, + packet.param3, packet.param4, (uint8_t)packet.param5); diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index c403852b3c..1cea7014cb 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -15,7 +15,7 @@ static uint32_t motor_test_timeout_ms; // test will timeout this many milli static uint8_t motor_test_seq; // motor sequence number of motor being tested static uint8_t motor_test_count; // number of motors to test static uint8_t motor_test_throttle_type; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through) -static uint16_t motor_test_throttle_value; // throttle to be sent to motor, value depends upon it's type +static float motor_test_throttle_value; // throttle to be sent to motor, value depends upon it's type // motor_test_output - checks for timeout and sends updates to motors objects void Copter::motor_test_output() @@ -65,13 +65,13 @@ void Copter::motor_test_output() if (motor_test_throttle_value <= 100) { int16_t pwm_min = motors->get_pwm_output_min(); int16_t pwm_max = motors->get_pwm_output_max(); - pwm = pwm_min + (pwm_max - pwm_min) * (float)motor_test_throttle_value/100.0f; + pwm = (int16_t) (pwm_min + (pwm_max - pwm_min) * motor_test_throttle_value * 1e-2f); } #endif break; case MOTOR_TEST_THROTTLE_PWM: - pwm = motor_test_throttle_value; + pwm = (int16_t)motor_test_throttle_value; break; case MOTOR_TEST_THROTTLE_PILOT: @@ -127,7 +127,7 @@ bool Copter::mavlink_motor_test_check(const GCS_MAVLINK &gcs_chan, bool check_rc // mavlink_motor_test_start - start motor test - spin a single motor at a specified pwm // returns MAV_RESULT_ACCEPTED on success, MAV_RESULT_FAILED on failure -MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, +MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t motor_seq, uint8_t throttle_type, float throttle_value, float timeout_sec, uint8_t motor_count) { if (motor_count == 0) {