2015-05-29 23:12:49 -03:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2014-04-28 04:27:27 -03:00
|
|
|
/*
|
|
|
|
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
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::motor_test_output()
|
2014-04-28 04:27:27 -03:00
|
|
|
{
|
|
|
|
// exit immediately if the motor test is not running
|
|
|
|
if (!ap.motor_test) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check for test timeout
|
2015-11-19 23:04:45 -04:00
|
|
|
if ((AP_HAL::millis() - motor_test_start_ms) >= motor_test_timeout_ms) {
|
2014-04-28 04:27:27 -03:00
|
|
|
// 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
|
2016-05-26 02:05:52 -03:00
|
|
|
#if FRAME_CONFIG != HELI_FRAME
|
2014-05-09 01:37:15 -03:00
|
|
|
if (motor_test_throttle_value <= 100) {
|
2017-01-09 03:31:26 -04:00
|
|
|
int16_t pwm_min = motors->get_pwm_output_min();
|
|
|
|
int16_t pwm_max = motors->get_pwm_output_max();
|
2016-05-26 01:34:27 -03:00
|
|
|
pwm = pwm_min + (pwm_max - pwm_min) * (float)motor_test_throttle_value/100.0f;
|
2014-04-28 04:27:27 -03:00
|
|
|
}
|
2016-05-26 02:05:52 -03:00
|
|
|
#endif
|
2014-04-28 04:27:27 -03:00
|
|
|
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();
|
2014-04-28 04:27:27 -03:00
|
|
|
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
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->output_test(motor_test_seq, pwm);
|
2014-04-28 04:27:27 -03:00
|
|
|
} else {
|
|
|
|
motor_test_stop();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
// mavlink_motor_test_check - perform checks before motor tests can begin
|
|
|
|
// return true if tests can continue, false if not
|
2015-05-29 23:12:49 -03:00
|
|
|
bool Copter::mavlink_motor_test_check(mavlink_channel_t chan, bool check_rc)
|
2014-04-28 04:27:27 -03:00
|
|
|
{
|
|
|
|
// check rc has been calibrated
|
2017-01-19 22:28:36 -04:00
|
|
|
arming.pre_arm_rc_checks(true);
|
2015-03-12 02:19:25 -03:00
|
|
|
if(check_rc && !ap.pre_arm_rc_check) {
|
2015-10-25 14:12:30 -03:00
|
|
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: RC not calibrated");
|
2014-04-28 04:27:27 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// ensure we are landed
|
|
|
|
if (!ap.land_complete) {
|
2015-10-25 14:12:30 -03:00
|
|
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: vehicle not landed");
|
2014-04-28 04:27:27 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check if safety switch has been pushed
|
|
|
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
2015-11-18 15:08:19 -04:00
|
|
|
gcs[chan-MAVLINK_COMM_0].send_text(MAV_SEVERITY_CRITICAL,"Motor Test: Safety switch");
|
2014-04-28 04:27:27 -03:00
|
|
|
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
|
2015-05-29 23:12:49 -03:00
|
|
|
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)
|
2014-04-28 04:27:27 -03:00
|
|
|
{
|
|
|
|
// if test has not started try to start it
|
|
|
|
if (!ap.motor_test) {
|
2015-03-12 02:19:25 -03:00
|
|
|
/* 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)) {
|
2014-04-28 04:27:27 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
} else {
|
|
|
|
// start test
|
|
|
|
ap.motor_test = true;
|
|
|
|
|
|
|
|
// enable and arm motors
|
2017-01-09 03:31:26 -04:00
|
|
|
if (!motors->armed()) {
|
2014-04-28 04:27:27 -03:00
|
|
|
init_rc_out();
|
2015-04-22 15:59:34 -03:00
|
|
|
enable_motor_output();
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->armed(true);
|
2014-04-28 04:27:27 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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
|
2015-11-19 23:04:45 -04:00
|
|
|
motor_test_start_ms = AP_HAL::millis();
|
2015-11-27 13:11:58 -04:00
|
|
|
motor_test_timeout_ms = MIN(timeout_sec * 1000, MOTOR_TEST_TIMEOUT_MS_MAX);
|
2014-04-28 04:27:27 -03:00
|
|
|
|
|
|
|
// 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
|
2015-05-29 23:12:49 -03:00
|
|
|
void Copter::motor_test_stop()
|
2014-04-28 04:27:27 -03:00
|
|
|
{
|
|
|
|
// exit immediately if the test is not running
|
|
|
|
if (!ap.motor_test) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
// flag test is complete
|
|
|
|
ap.motor_test = false;
|
|
|
|
|
|
|
|
// disarm motors
|
2017-01-09 03:31:26 -04:00
|
|
|
motors->armed(false);
|
2014-04-28 04:27:27 -03:00
|
|
|
|
|
|
|
// 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;
|
|
|
|
}
|