From ae9477600c1cfef115a3ce4be352b47b971be359 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Apr 2014 16:27:27 +0900 Subject: [PATCH] Copter: add mavlink motor_test Based on original work by Nils Hogberg --- ArduCopter/ArduCopter.pde | 3 +- ArduCopter/GCS_Mavlink.pde | 8 ++ ArduCopter/motor_test.pde | 168 +++++++++++++++++++++++++++++++++++++ ArduCopter/motors.pde | 11 ++- 4 files changed, 186 insertions(+), 4 deletions(-) create mode 100644 ArduCopter/motor_test.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 992ca70b31..ce2503fb49 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -402,8 +402,9 @@ static union { uint8_t usb_connected : 1; // 13 // true if APM is powered from USB connection uint8_t rc_receiver_present : 1; // 14 // true if we have an rc receiver present (i.e. if we've ever received an update uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration + uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test }; - uint16_t value; + uint32_t value; } ap; //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 522dd9e305..3e5c100e3a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1336,6 +1336,14 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif 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 = mavlink_motor_test_start(chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3, packet.param4); + break; + default: result = MAV_RESULT_UNSUPPORTED; break; diff --git a/ArduCopter/motor_test.pde b/ArduCopter/motor_test.pde new file mode 100644 index 0000000000..98d6397457 --- /dev/null +++ b/ArduCopter/motor_test.pde @@ -0,0 +1,168 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/* + 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 +#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 + +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 +static void motor_test_output() +{ + // exit immediately if the motor test is not running + if (!ap.motor_test) { + return; + } + + // check for test timeout + if ((hal.scheduler->millis() - motor_test_start_ms) >= motor_test_timeout_ms) { + // stop motor test + motor_test_stop(); + } else { + 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 >= 0 || motor_test_throttle_value <= 100) { + pwm = g.rc_3.radio_min + (g.rc_3.radio_max - g.rc_3.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 = g.rc_3.radio_in; + break; + + default: + motor_test_stop(); + return; + break; + } + + // 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_check - perform checks before motor tests can begin +// return true if tests can continue, false if not +static bool mavlink_motor_test_check(mavlink_channel_t chan) +{ + // check rc has been calibrated + pre_arm_rc_checks(); + if(!ap.pre_arm_rc_check) { + gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("Motor Test: RC not calibrated")); + return false; + } + + // ensure we are landed + if (!ap.land_complete) { + gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("Motor Test: vehicle not landed")); + return false; + } + + // check if safety switch has been pushed + if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) { + gcs[chan-MAVLINK_COMM_0].send_text_P(SEVERITY_HIGH,PSTR("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 +static 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) +{ + // debug + cliSerial->printf_P(PSTR("\nMotTest Seq:%d TT:%d Thr:%d TimOut:%4.2f"),(int)motor_seq, (int)throttle_type, (int)throttle_value, (float)timeout_sec); + + // if test has not started try to start it + if (!ap.motor_test) { + // perform checks that it is ok to start test + if (!mavlink_motor_test_check(chan)) { + return MAV_RESULT_FAILED; + } else { + // start test + ap.motor_test = true; + + // enable and arm motors + if (!motors.armed()) { + init_rc_out(); + output_min(); + motors.armed(true); + } + + // disable throttle, battery and gps failsafe + g.failsafe_throttle = FS_THR_DISABLED; + g.failsafe_battery_enabled = FS_BATT_DISABLED; + g.failsafe_gps_enabled = FS_GPS_DISABLED; + g.failsafe_gcs = FS_GCS_DISABLED; + + // turn on notify leds + AP_Notify::flags.esc_calibration = true; + } + } + + // set timeout + motor_test_start_ms = hal.scheduler->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 +static void motor_test_stop() +{ + // exit immediately if the test is not running + if (!ap.motor_test) { + return; + } + + // flag test is complete + ap.motor_test = false; + + // disarm motors + motors.armed(false); + + // reset timeout + motor_test_start_ms = 0; + motor_test_timeout_ms = 0; + + // re-enable failsafes + g.failsafe_throttle.load(); + g.failsafe_battery_enabled.load(); + g.failsafe_gps_enabled.load(); + g.failsafe_gcs.load(); + + // turn off notify leds + AP_Notify::flags.esc_calibration = false; +} diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index c6d1e5ad5a..19c9f6d86a 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -556,11 +556,16 @@ static void init_disarm_motors() static void set_servos_4() { + // check if we are performing the motor test + if (ap.motor_test) { + motor_test_output(); + } else { #if FRAME_CONFIG == TRI_FRAME - // To-Do: implement improved stability patch for tri so that we do not need to limit throttle input to motors - g.rc_3.servo_out = min(g.rc_3.servo_out, 800); + // To-Do: implement improved stability patch for tri so that we do not need to limit throttle input to motors + g.rc_3.servo_out = min(g.rc_3.servo_out, 800); #endif - motors.output(); + motors.output(); + } } // servo_write - writes to a servo after checking the channel is not used for a motor