// 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 (flightmode->has_manual_throttle() || !sprung_throttle_stick) {
thr_low = ap.throttle_zero;
} else {
float deadband_top = get_throttle_mid() + g.throttle_deadzone;
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) {
init_disarm_motors();
auto_disarm_begin = tnow_ms;
}
}
// get_pilot_desired_climb_rate - transform pilot's throttle input to climb rate in cm/s
// without any deadzone at the bottom
float Copter::get_pilot_desired_climb_rate(float throttle_control)
{
// throttle failsafe check
if( failsafe.radio ) {
return 0.0f;
}
#if TOY_MODE_ENABLED == ENABLED
if (g2.toy_mode.enabled()) {
// allow throttle to be reduced after throttle arming and for
// slower descent close to the ground
g2.toy_mode.throttle_adjust(throttle_control);
}
#endif
float desired_rate = 0.0f;
float mid_stick = get_throttle_mid();
float deadband_top = mid_stick + g.throttle_deadzone;
float deadband_bottom = mid_stick - g.throttle_deadzone;
// ensure a reasonable throttle value
throttle_control = constrain_float(throttle_control,0.0f,1000.0f);
// ensure a reasonable deadzone
g.throttle_deadzone = constrain_int16(g.throttle_deadzone, 0, 400);
// check throttle is above, below or in the deadband
if (throttle_control < deadband_bottom) {
// below the deadband
desired_rate = get_pilot_speed_dn() * (throttle_control-deadband_bottom) / deadband_bottom;
}else if (throttle_control > deadband_top) {
// above the deadband
desired_rate = g.pilot_speed_up * (throttle_control-deadband_top) / (1000.0f-deadband_top);
}else{
// must be in the deadband
desired_rate = 0.0f;
}
return desired_rate;
}
