ardupilot/ArduCopter/motor_test.cpp

229 lines
7.6 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_TIMEOUT_SEC 600 // max timeout is 10 minutes (600 seconds)
static uint32_t motor_test_start_ms; // system time the motor test began
static uint32_t motor_test_timeout_ms; // test will timeout this many milliseconds after the motor_test_start_ms
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 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()
{
// exit immediately if the motor test is not running
if (!ap.motor_test) {
return;
}
2019-05-15 01:10:12 -03:00
EXPECT_DELAY_MS(2000);
2019-05-11 05:18:21 -03:00
// check for test timeout
uint32_t now = AP_HAL::millis();
if ((now - motor_test_start_ms) >= motor_test_timeout_ms) {
if (motor_test_count > 1) {
if (now - motor_test_start_ms < motor_test_timeout_ms*1.5) {
// output zero for 50% of the test time
motors->output_min();
} else {
// move onto next motor
motor_test_seq++;
motor_test_count--;
motor_test_start_ms = now;
if (!motors->armed()) {
motors->armed(true);
hal.util->set_soft_armed(true);
}
}
return;
}
// 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_COMPASS_CAL:
compass.set_voltage(battery.voltage());
compass.per_motor_calibration_update();
2018-03-20 11:52:42 -03:00
FALLTHROUGH;
2018-07-17 01:09:58 -03:00
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 = (int16_t) (pwm_min + (pwm_max - pwm_min) * motor_test_throttle_value * 1e-2f);
}
#endif
break;
case MOTOR_TEST_THROTTLE_PWM:
pwm = (int16_t)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 >= RC_Channel::RC_MIN_LIMIT_PWM && pwm <= RC_Channel::RC_MAX_LIMIT_PWM) {
// turn on motor to specified pwm value
motors->output_test_seq(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_control_check(const GCS_MAVLINK &gcs_chan, bool check_rc, const char* mode)
{
// check board has initialised
if (!ap.initialised) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Board initialising", mode);
return false;
}
// Check Motor test is allowed
char failure_msg[50] {};
if (!motors->motor_test_checks(ARRAY_SIZE(failure_msg), failure_msg)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: %s", mode, failure_msg);
return false;
}
// check rc has been calibrated
if (check_rc && !arming.rc_calibration_checks(true)) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: RC not calibrated", mode);
return false;
}
// ensure we are landed
if (!ap.land_complete) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: vehicle not landed", mode);
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,"%s: Safety switch", mode);
return false;
}
2022-02-08 21:00:43 -04:00
// check E-Stop is not active
if (SRV_Channels::get_emergency_stop()) {
gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Motor Emergency Stopped", mode);
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
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) {
motor_count = 1;
}
// 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_control_check(gcs_chan, throttle_type != 1, "Motor Test")) {
return MAV_RESULT_FAILED;
} else {
// start test
2022-02-08 21:00:43 -04:00
gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
ap.motor_test = true;
2019-05-15 01:10:12 -03:00
EXPECT_DELAY_MS(3000);
// enable and arm motors
if (!motors->armed()) {
motors->output_min(); // output lowest possible value to motors
motors->armed(true);
hal.util->set_soft_armed(true);
}
2018-02-28 19:32:16 -04:00
// disable throttle and gps failsafe
g.failsafe_throttle.set(FS_THR_DISABLED);
g.failsafe_gcs.set(FS_GCS_DISABLED);
g.fs_ekf_action.set(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, MOTOR_TEST_TIMEOUT_SEC) * 1000;
// store required output
motor_test_seq = motor_seq;
motor_test_count = motor_count;
motor_test_throttle_type = throttle_type;
motor_test_throttle_value = throttle_value;
if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
compass.per_motor_calibration_start();
}
2018-07-17 01:09:58 -03:00
// 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;
}
gcs().send_text(MAV_SEVERITY_INFO, "finished motor test");
// flag test is complete
ap.motor_test = false;
// disarm motors
motors->armed(false);
hal.util->set_soft_armed(false);
// reset timeout
motor_test_start_ms = 0;
motor_test_timeout_ms = 0;
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_gcs.load();
g.fs_ekf_action.load();
if (motor_test_throttle_type == MOTOR_TEST_COMPASS_CAL) {
compass.per_motor_calibration_end();
}
2018-07-17 01:09:58 -03:00
// turn off notify leds
AP_Notify::flags.esc_calibration = false;
}