ardupilot/ArduCopter/motors.cpp

337 lines
10 KiB
C++
Raw Normal View History

#include "Copter.h"
#define ARM_DELAY 20 // called at 10hz so 2 seconds
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint32_t auto_disarm_begin;
// arm_motors_check - checks for pilot input to arm or disarm the copter
// called at 10hz
void Copter::arm_motors_check()
{
2012-08-21 23:19:50 -03:00
static int16_t arming_counter;
// ensure throttle is down
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
if (channel_throttle->get_control_in() > 0) {
2012-08-21 23:19:50 -03:00
arming_counter = 0;
return;
}
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
int16_t tmp = channel_yaw->get_control_in();
2012-08-21 23:19:50 -03:00
// full right
if (tmp > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
if( arming_counter <= AUTO_TRIM_DELAY ) {
arming_counter++;
}
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
// reset arming counter if arming fail
if (!init_arm_motors(false)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
auto_trim_counter = 250;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
2012-08-21 23:19:50 -03:00
}
// full left
2012-08-21 23:19:50 -03:00
}else if (tmp < -4000) {
if (!mode_has_manual_throttle(control_mode) && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
if( arming_counter <= DISARM_DELAY ) {
2012-08-21 23:19:50 -03:00
arming_counter++;
}
// disarm the motors
if (arming_counter == DISARM_DELAY && motors.armed()) {
init_disarm_motors();
}
// Yaw is centered so reset arming counter
2012-08-21 23:19:50 -03:00
}else{
arming_counter = 0;
}
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
{
uint32_t tnow_ms = millis();
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
// disarming is disabled
if (!motors.armed() || disarm_delay_ms == 0 || control_mode == THROW) {
auto_disarm_begin = tnow_ms;
return;
}
#if FRAME_CONFIG == HELI_FRAME
// if the rotor is still spinning, don't initiate auto disarm
if (motors.rotor_speed_above_critical()) {
auto_disarm_begin = tnow_ms;
return;
}
#endif
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
#if FRAME_CONFIG != HELI_FRAME
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
// obvious the copter is armed as the motors will not be spinning
disarm_delay_ms /= 2;
#endif
} else {
bool sprung_throttle_stick = (g.throttle_behavior & THR_BEHAVE_FEEDBACK_FROM_MID_STICK) != 0;
bool thr_low;
if (mode_has_manual_throttle(control_mode) || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = g.rc_3.get_control_mid() + g.throttle_deadzone;
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
thr_low = g.rc_3.get_control_in() <= deadband_top;
}
if (!thr_low || !ap.land_complete) {
// reset timer
auto_disarm_begin = tnow_ms;
}
}
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
auto_disarm_begin = tnow_ms;
}
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// return true if already armed
if (motors.armed()) {
return true;
}
// run pre-arm-checks and display failures
if (!all_arming_checks_passing(arming_from_gcs)) {
2015-02-02 16:58:37 -04:00
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// let dataflash know that we're armed (it may open logs e.g.)
DataFlash_Class::instance()->set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
2013-05-19 10:55:40 -03:00
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
2015-05-04 03:18:46 -03:00
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Arming motors");
2012-08-21 23:19:50 -03:00
#endif
2012-08-21 23:19:50 -03:00
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
2015-07-05 05:38:04 -03:00
Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
2015-01-28 19:18:20 -04:00
hal.util->set_soft_armed(true);
2013-09-11 05:05:25 -03:00
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors.armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
2014-07-28 23:43:47 -03:00
// log flight mode in case it was changed while vehicle was disarmed
2016-01-25 19:50:12 -04:00
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
2014-07-28 23:43:47 -03:00
// reenable failsafe
failsafe_enable();
2014-10-29 03:19:10 -03:00
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
2016-01-21 22:59:47 -04:00
// Log time stamp of arming event
arm_time_ms = millis();
// Start the arming delay
ap.in_arming_delay = true;
2014-10-29 03:19:10 -03:00
// return success
return true;
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
2015-05-04 03:18:46 -03:00
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
2012-08-21 23:19:50 -03:00
#endif
// save compass offsets learned by the EKF if enabled
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
if (ahrs.getMagOffsets(i, magOffsets)) {
compass.set_and_save_offsets(i, magOffsets);
}
}
}
2014-02-02 03:58:36 -04:00
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
2014-02-02 03:58:36 -04:00
autotune_save_tuning_gains();
#endif
2012-08-21 23:19:50 -03:00
// we are not in the air
set_land_complete(true);
2014-09-19 03:54:36 -03:00
set_land_complete_maybe(true);
2014-05-15 04:19:44 -03:00
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
2014-01-07 09:37:39 -04:00
// suspend logging
if (!DataFlash.log_while_disarmed()) {
DataFlash.EnableWrites(false);
}
DataFlash_Class::instance()->set_vehicle_armed(false);
2014-01-07 09:37:39 -04:00
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
2015-01-28 19:18:20 -04:00
hal.util->set_soft_armed(false);
2016-01-21 22:59:47 -04:00
ap.in_arming_delay = false;
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
return;
}
#endif
2016-01-21 22:59:47 -04:00
// Update arming delay state
if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
2016-01-21 22:59:47 -04:00
bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
if (!motors.get_interlock() && interlock) {
motors.set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors.get_interlock() && !interlock) {
motors.set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
2016-01-21 22:59:47 -04:00
// send output signals to motors
motors.output();
}
}
// check for pilot stick input to trigger lost vehicle alarm
void Copter::lost_vehicle_check()
{
static uint8_t soundalarm_counter;
// disable if aux switch is setup to vehicle alarm as the two could interfere
if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
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
if (ap.throttle_zero && !motors.armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true;
gcs_send_text(MAV_SEVERITY_NOTICE,"Locate Copter alarm");
}
} else {
soundalarm_counter++;
}
} else {
soundalarm_counter = 0;
if (AP_Notify::flags.vehicle_lost == true) {
AP_Notify::flags.vehicle_lost = false;
}
}
}