mirror of https://github.com/ArduPilot/ardupilot
Copter: Change all zero throttle checks that should be conservative to use ap.throttle_zero
This commit is contained in:
parent
506c7661a4
commit
8d63a65793
|
@ -265,7 +265,7 @@ static void exit_mission()
|
|||
}else{
|
||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
||||
if (g.rc_3.control_in == 0 || failsafe.radio) {
|
||||
if (ap.throttle_zero || failsafe.radio) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
#else
|
||||
|
|
|
@ -61,7 +61,7 @@ static void land_gps_run()
|
|||
|
||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
||||
if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
|
||||
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
#else
|
||||
|
@ -131,7 +131,7 @@ static void land_nogps_run()
|
|||
attitude_control.set_throttle_out(0, false);
|
||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
||||
if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
|
||||
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
#else
|
||||
|
|
|
@ -334,7 +334,7 @@ static void rtl_land_run()
|
|||
|
||||
#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
|
||||
// disarm when the landing detector says we've landed and throttle is at minimum
|
||||
if (ap.land_complete && (g.rc_3.control_in == 0 || failsafe.radio)) {
|
||||
if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
#else
|
||||
|
|
|
@ -25,7 +25,7 @@ void crash_check()
|
|||
#endif
|
||||
|
||||
// return immediately if motors are not armed or pilot's throttle is above zero
|
||||
if (!motors.armed() || (g.rc_3.control_in != 0 && !failsafe.radio)) {
|
||||
if (!motors.armed() || (!ap.throttle_zero && !failsafe.radio)) {
|
||||
inverted_count = 0;
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -16,7 +16,7 @@ static void failsafe_radio_on_event()
|
|||
case STABILIZE:
|
||||
case ACRO:
|
||||
// if throttle is zero OR vehicle is landed disarm motors
|
||||
if (g.rc_3.control_in == 0 || ap.land_complete) {
|
||||
if (ap.throttle_zero || ap.land_complete) {
|
||||
init_disarm_motors();
|
||||
|
||||
// if failsafe_throttle is FS_THR_ENABLED_ALWAYS_LAND then land immediately
|
||||
|
@ -115,7 +115,7 @@ static void failsafe_battery_event(void)
|
|||
case STABILIZE:
|
||||
case ACRO:
|
||||
// if throttle is zero OR vehicle is landed disarm motors
|
||||
if (g.rc_3.control_in == 0 || ap.land_complete) {
|
||||
if (ap.throttle_zero || ap.land_complete) {
|
||||
init_disarm_motors();
|
||||
}else{
|
||||
// set mode to RTL or LAND
|
||||
|
@ -274,7 +274,7 @@ static void failsafe_gcs_check()
|
|||
case ACRO:
|
||||
case SPORT:
|
||||
// if throttle is zero disarm motors
|
||||
if (g.rc_3.control_in == 0) {
|
||||
if (ap.throttle_zero) {
|
||||
init_disarm_motors();
|
||||
}else if(home_distance > wp_nav.get_wp_radius()) {
|
||||
if (!set_mode(RTL)) {
|
||||
|
|
|
@ -30,7 +30,7 @@ void fence_check()
|
|||
|
||||
// disarm immediately if we think we are on the ground
|
||||
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
|
||||
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
|
||||
if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
|
||||
init_disarm_motors();
|
||||
}else{
|
||||
// if we are within 100m of the fence, RTL
|
||||
|
|
|
@ -356,7 +356,7 @@ static void update_auto_armed()
|
|||
return;
|
||||
}
|
||||
// if in stabilize or acro flight mode and throttle is zero, auto-armed should become false
|
||||
if(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio) {
|
||||
if(manual_flight_mode(control_mode) && ap.throttle_zero && !failsafe.radio) {
|
||||
set_auto_armed(false);
|
||||
}
|
||||
}else{
|
||||
|
@ -364,12 +364,12 @@ static void update_auto_armed()
|
|||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
|
||||
if(motors.armed() && g.rc_3.control_in != 0 && motors.motor_runup_complete()) {
|
||||
if(motors.armed() && !ap.throttle_zero && motors.motor_runup_complete()) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#else
|
||||
// if motors are armed and throttle is above zero auto_armed should be true
|
||||
if(motors.armed() && g.rc_3.control_in != 0) {
|
||||
if(motors.armed() && !ap.throttle_zero) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
|
|
Loading…
Reference in New Issue