diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 0b6a770e07..d8dd883332 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1539,6 +1539,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; #endif + 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 = plane.quadplane.mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); + break; + case MAV_CMD_DO_VTOL_TRANSITION: result = plane.quadplane.handle_do_vtol_transition(packet); break; diff --git a/ArduPlane/motor_test.cpp b/ArduPlane/motor_test.cpp new file mode 100644 index 0000000000..8fcce692f2 --- /dev/null +++ b/ArduPlane/motor_test.cpp @@ -0,0 +1,118 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Plane.h" + +/* + mavlink motor test - implements the MAV_CMD_DO_MOTOR_TEST mavlink + command so that the quadplane pilot can test an + individual motor to ensure proper wiring, rotation. + */ + +// motor test definitions +#define MOTOR_TEST_PWM_MIN 800 // min pwm value accepted by the test +#define MOTOR_TEST_PWM_MAX 2200 // max pwm value accepted by the test +#define MOTOR_TEST_TIMEOUT_MS_MAX 30000 // max timeout is 30 seconds + +// motor_test_output - checks for timeout and sends updates to motors objects +void QuadPlane::motor_test_output() +{ + // exit immediately if the motor test is not running + if (!motor_test.running) { + return; + } + + // check for test timeout + if ((AP_HAL::millis() - motor_test.start_ms) >= motor_test.timeout_ms) { + // stop motor test + motor_test_stop(); + return; + } + + int16_t pwm = 0; // pwm that will be output to the motors + + // 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) { + pwm = plane.channel_throttle->radio_min + (plane.channel_throttle->radio_max - plane.channel_throttle->radio_min) * (float)motor_test.throttle_value/100.0f; + } + break; + + case MOTOR_TEST_THROTTLE_PWM: + pwm = motor_test.throttle_value; + break; + + case MOTOR_TEST_THROTTLE_PILOT: + pwm = plane.channel_throttle->radio_in; + break; + + default: + motor_test_stop(); + return; + } + + // sanity check throttle values + if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) { + // turn on motor to specified pwm vlaue + motors->output_test(motor_test.seq, pwm); + } else { + motor_test_stop(); + } +} + +// 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 QuadPlane::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type, + uint16_t throttle_value, float timeout_sec) +{ + if (motors->armed()) { + plane.gcs_send_text(MAV_SEVERITY_INFO, "Must be disarmed for motor test"); + return MAV_RESULT_FAILED; + } + // if test has not started try to start it + if (!motor_test.running) { + // start test + motor_test.running = true; + + // enable and arm motors + set_armed(true); + + // 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 QuadPlane::motor_test_stop() +{ + // exit immediately if the test is not running + if (!motor_test.running) { + return; + } + + // flag test is complete + motor_test.running = false; + + // disarm motors + set_armed(false); + + // reset timeout + motor_test.start_ms = 0; + motor_test.timeout_ms = 0; + + // turn off notify leds + AP_Notify::flags.esc_calibration = false; +} diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 445d7e0757..7414348b45 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -335,7 +335,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Param: FRAME_CLASS // @DisplayName: Frame Class // @Description: Controls major frame class for multicopter component - // @Values: 0:Quad, 1:Hexa, 2:Octa + // @Values: 0:Quad, 1:Hexa, 2:Octa, 3:OctaQuad // @User: Standard AP_GROUPINFO("FRAME_CLASS", 30, QuadPlane, frame_class, 0), @@ -875,6 +875,11 @@ void QuadPlane::update(void) return; } + if (motor_test.running) { + motor_test_output(); + return; + } + if (!in_vtol_mode()) { update_transition(); } else { diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 4dcaa80b1d..03e83438db 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -200,5 +200,22 @@ private: FRAME_CLASS_QUAD=0, FRAME_CLASS_HEXA=1, FRAME_CLASS_OCTA=2, + FRAME_CLASS_OCTAQUAD=3, }; + + struct { + bool running; + uint32_t start_ms; // system time the motor test began + uint32_t timeout_ms = 0; // test will timeout this many milliseconds after the motor_test_start_ms + uint8_t seq = 0; // motor sequence number of motor being tested + uint8_t throttle_type = 0; // motor throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through) + uint16_t throttle_value = 0; // throttle to be sent to motor, value depends upon it's type + } motor_test; + +public: + void motor_test_output(); + 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); +private: + void motor_test_stop(); };