diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 324ae14741..92586064f1 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1039,6 +1039,16 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) } break; + case MAV_CMD_DO_MOTOR_TEST: + // 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) + // param4 : timeout (in seconds) + result = rover.mavlink_motor_test_start(chan, static_cast(packet.param1), + static_cast(packet.param2), + static_cast(packet.param3), + packet.param4); + break; default: result = handle_command_long_message(packet); diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index a57316a6f5..391c5765ba 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -419,6 +419,9 @@ private: // last visual odometry update time uint32_t visual_odom_last_update_ms; + // True when we are doing motor test + bool motor_test; + private: // private member functions void ahrs_update(); @@ -630,6 +633,11 @@ public: void dataflash_periodic(void); void update_soft_armed(); + // Motor test + void motor_test_output(); + bool mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc); + uint8_t mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec); + void motor_test_stop(); }; #define MENU_FUNC(func) FUNCTOR_BIND(&rover, &Rover::func, int8_t, uint8_t, const Menu::arg *) diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index a8ad95b7a8..93e5764f47 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -254,5 +254,10 @@ void Rover::set_servos(void) { } // send output signals to motors - g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt); + if (motor_test) { + g2.motors.slew_limit_throttle(false); + motor_test_output(); + } else { + g2.motors.output(arming.is_armed() && hal.util->get_soft_armed(), G_Dt); + } } diff --git a/APMrover2/motor_test.cpp b/APMrover2/motor_test.cpp new file mode 100644 index 0000000000..6cb022ec6b --- /dev/null +++ b/APMrover2/motor_test.cpp @@ -0,0 +1,158 @@ +#include "Rover.h" + +/* + mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink command so that the GCS/pilot can test an individual motor or flaps + to ensure proper wiring, rotation. + */ + +// motor test definitions +static const int16_t MOTOR_TEST_PWM_MIN = 1000; // min pwm value accepted by the test +static const int16_t MOTOR_TEST_PWM_MAX = 2000; // max pwm value accepted by the test +static const int16_t MOTOR_TEST_TIMEOUT_MS_MAX = 30000; // max timeout is 30 seconds + +static uint32_t motor_test_start_ms = 0; // system time the motor test began +static uint32_t motor_test_timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms +static uint8_t motor_test_seq = 0; // motor sequence number of motor being tested +static uint8_t motor_test_throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through) +static uint16_t motor_test_throttle_value = 0; // 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 Rover::motor_test_output() +{ + // exit immediately if the motor test is not running + if (!motor_test) { + return; + } + + // check for test timeout + if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) { + // stop motor test + motor_test_stop(); + } else { + // calculate pwm based on throttle type + switch (motor_test_throttle_type) { + case MOTOR_TEST_THROTTLE_PERCENT: + // sanity check motor_test_throttle value + if (motor_test_throttle_value <= 100) { + g2.motors.set_throttle(motor_test_throttle_value); + } + break; + + case MOTOR_TEST_THROTTLE_PWM: + channel_throttle->set_pwm(motor_test_throttle_value); + g2.motors.set_throttle(channel_throttle->get_control_in()); + break; + + case MOTOR_TEST_THROTTLE_PILOT: + g2.motors.set_throttle(channel_throttle->get_control_in()); + break; + + default: + motor_test_stop(); + return; + } + const bool test_result = g2.motors.output_test((AP_MotorsUGV::motor_test_order)motor_test_seq); + if (!test_result) { + motor_test_stop(); + } + } +} + +// mavlink_motor_test_check - perform checks before motor tests can begin +// return true if tests can continue, false if not +bool Rover::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc) +{ + GCS_MAVLINK_Rover &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0); + + // check board has initialised + if (!initialised) { + gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Board initialising"); + return false; + } + + // check rc has been calibrated + if (check_rc && !arming.pre_arm_rc_checks(true)) { + gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: RC not calibrated"); + return false; + } + + // check if safety switch has been pushed + if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { + gcs_chan.send_text(MAV_SEVERITY_CRITICAL, "Motor Test: Safety switch"); + return false; + } + + // if we got this far the check was successful and the motor test can continue + return true; +} + +// 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 +uint8_t Rover::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, uint16_t throttle_value, float timeout_sec) +{ + // if test has not started try to start it + if (!motor_test) { + /* perform checks that it is ok to start test + The RC calibrated check can be skipped if direct pwm is + supplied + */ + if (!mavlink_motor_test_check(chan, throttle_type != 1)) { + return MAV_RESULT_FAILED; + } else { + // start test + motor_test = true; + + // arm motors + if (!arming.is_armed()) { + arm_motors(AP_Arming::NONE); + } + + // disable failsafes + g.fs_gcs_enabled = 0; + g.fs_throttle_enabled = 0; + g.fs_crash_check = 0; + + // turn on notify leds + AP_Notify::flags.esc_calibration = true; + } + } + + // set timeout + motor_test_start_ms = AP_HAL::millis(); + motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX); + + // store required output + motor_test_seq = motor_seq; + motor_test_throttle_type = throttle_type; + motor_test_throttle_value = throttle_value; + + // return success + return MAV_RESULT_ACCEPTED; +} + +// motor_test_stop - stops the motor test +void Rover::motor_test_stop() +{ + // exit immediately if the test is not running + if (!motor_test) { + return; + } + + // disarm motors + disarm_motors(); + + // reset timeout + motor_test_start_ms = 0; + motor_test_timeout_ms = 0; + + // re-enable failsafes + g.fs_gcs_enabled.load(); + g.fs_throttle_enabled.load(); + g.fs_crash_check.load(); + + // turn off notify leds + AP_Notify::flags.esc_calibration = false; + + // flag test is complete + motor_test = false; +}