ardupilot/ArduCopter/motor_test.cpp

178 lines
5.9 KiB
C++
Raw Normal View History

#include "Copter.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
#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
void Copter::motor_test_output()
{
// exit immediately if the motor test is not running
if (!ap.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 {
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 FRAME_CONFIG != HELI_FRAME
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;
}
#endif
break;
case MOTOR_TEST_THROTTLE_PWM:
pwm = motor_test_throttle_value;
break;
case MOTOR_TEST_THROTTLE_PILOT:
ArduCopter: Fix up after refactoring RC_Channel class Further to refactor of RC_Channel class which included adding get_xx set_xx methods, change reads and writes to the public members to calls to get and set functionsss old public member(int16_t) get function -> int16_t set function (int16_t) (expression where c is an object of type RC_Channel) c.radio_in c.get_radio_in() c.set_radio_in(v) c.control_in c.get_control_in() c.set_control_in(v) c.servo_out c.get_servo_out() c.set_servo_out(v) c.pwm_out c.get_pwm_out() // use existing c.radio_out c.get_radio_out() c.set_radio_out(v) c.radio_max c.get_radio_max() c.set_radio_max(v) c.radio_min c.get_radio_min() c.set_radio_min(v) c.radio_trim c.get_radio_trim() c.set_radio_trim(v); c.min_max_configured() // return true if min and max are configured Because data members of RC_Channels are now private and so cannot be written directly some overloads are provided in the Plane classes to provide the old functionality new overload Plane::stick_mix_channel(RC_Channel *channel) which forwards to the previously existing void stick_mix_channel(RC_Channel *channel, int16_t &servo_out); new overload Plane::channel_output_mixer(Rc_Channel* , RC_Channel*)const which forwards to (uint8_t mixing_type, int16_t & chan1, int16_t & chan2)const; Rename functions RC_Channel_aux::set_radio_trim(Aux_servo_function_t function) to RC_Channel_aux::set_trim_to_radio_in_for(Aux_servo_function_t function) RC_Channel_aux::set_servo_out(Aux_servo_function_t function, int16_t value) to RC_Channel_aux::set_servo_out_for(Aux_servo_function_t function, int16_t value) Rationale: RC_Channel is a complicated class, which combines several functionalities dealing with stick inputs in pwm and logical units, logical and actual actuator outputs, unit conversion etc, etc The intent of this PR is to clarify existing use of the class. At the basic level it should now be possible to grep all places where private variable is set by searching for the set_xx function. (The wider purpose is to provide a more generic and logically simpler method of output mixing. This is a small step)
2016-05-08 05:46:59 -03:00
pwm = channel_throttle->get_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_check - perform checks before motor tests can begin
// return true if tests can continue, false if not
bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
{
GCS_MAVLINK_Copter &gcs_chan = gcs().chan(chan-MAVLINK_COMM_0);
// check board has initialised
if (!ap.initialised) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Board initialising");
return false;
}
// check rc has been calibrated
arming.pre_arm_rc_checks(true);
if(check_rc && !ap.pre_arm_rc_check) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
return false;
}
// ensure we are landed
if (!ap.land_complete) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"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.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 Copter::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 (!ap.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
ap.motor_test = true;
// enable and arm motors
if (!motors->armed()) {
init_rc_out();
enable_motor_output();
motors->armed(true);
}
// disable throttle, battery and gps failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED;
// 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 Copter::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_gcs.load();
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
}