ardupilot/ArduCopter/motors.cpp

213 lines
7.3 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;
// check if arming/disarm using rudder is allowed
AP_Arming::RudderArming arming_rudder = arming.get_rudder_arming_type();
if (arming_rudder == AP_Arming::RudderArming::IS_DISABLED) {
return;
}
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// not armed with sticks in toy mode
return;
}
#endif
2018-09-09 21:21:31 -03:00
// 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;
}
2018-09-09 21:21:31 -03:00
int16_t yaw_in = channel_yaw->get_control_in();
2012-08-21 23:19:50 -03:00
// full right
2018-09-09 21:21:31 -03:00
if (yaw_in > 4000) {
// increase the arming counter to a maximum of 1 beyond the auto trim counter
2018-09-09 21:21:31 -03:00
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 (!arming.arm(AP_Arming::Method::RUDDER)) {
arming_counter = 0;
}
}
// arm the motors and configure for flight
if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && flightmode->mode_number() == Mode::Number::STABILIZE) {
gcs().send_text(MAV_SEVERITY_INFO, "AutoTrim start");
auto_trim_counter = 250;
auto_trim_started = false;
// ensure auto-disarm doesn't trigger immediately
auto_disarm_begin = millis();
2012-08-21 23:19:50 -03:00
}
// full left and rudder disarming is enabled
} else if ((yaw_in < -4000) && (arming_rudder == AP_Arming::RudderArming::ARMDISARM)) {
if (!flightmode->has_manual_throttle() && !ap.land_complete) {
arming_counter = 0;
return;
}
// increase the counter to a maximum of 1 beyond the disarm delay
2018-09-09 21:21:31 -03:00
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()) {
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::RUDDER);
}
// Yaw is centered so reset arming counter
2018-09-09 21:21:31 -03:00
} else {
2012-08-21 23:19:50 -03:00
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 || flightmode->mode_number() == Mode::Number::THROW) {
auto_disarm_begin = tnow_ms;
return;
}
// if the rotor is still spinning, don't initiate auto disarm
if (motors->get_spool_state() > AP_Motors::SpoolState::GROUND_IDLE) {
auto_disarm_begin = tnow_ms;
return;
}
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
if ((ap.using_interlock && !motors->get_interlock()) || SRV_Channels::get_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 (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = get_throttle_mid() + g.throttle_deadzone;
2017-01-06 21:06:40 -04:00
thr_low = channel_throttle->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) {
2020-02-21 09:09:57 -04:00
arming.disarm(AP_Arming::Method::DISARMDELAY);
auto_disarm_begin = tnow_ms;
}
}
// 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();
if (!g2.afs.terminating_vehicle_via_landing()) {
return;
}
// landing must continue to run the motors output
}
#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 || flightmode->mode_number() == Mode::Number::THROW)) {
2016-01-21 22:59:47 -04:00
ap.in_arming_delay = false;
}
// output any servo channels
SRV_Channels::calc_pwm();
// cork now, so that all channel outputs happen at once
2017-11-02 23:35:14 -03:00
SRV_Channels::cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
// update motors interlock state
bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
if (!motors->get_interlock() && interlock) {
motors->set_interlock(true);
AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_ENABLED);
} else if (motors->get_interlock() && !interlock) {
motors->set_interlock(false);
AP::logger().Write_Event(LogEvent::MOTORS_INTERLOCK_DISABLED);
}
if (ap.motor_test) {
// check if we are performing the motor test
motor_test_output();
} else {
2016-01-21 22:59:47 -04:00
// send output signals to motors
flightmode->output_to_motors();
}
// push all channels
2017-11-02 23:35:14 -03:00
SRV_Channels::push();
}
// 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 (rc().find_channel_for_option(RC_Channel::AUX_FUNC::LOST_VEHICLE_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
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;
}
}
}