Copter: landing check changes
This commit is contained in:
parent
dbc18ee900
commit
9a1a999fba
@ -1978,11 +1978,20 @@ void update_throttle_mode(void)
|
||||
// alt hold plus pilot input of climb rate
|
||||
pilot_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
|
||||
|
||||
// special handling if we have landed
|
||||
if (ap.land_complete) {
|
||||
if (pilot_climb_rate > 0) {
|
||||
// indicate we are taking off
|
||||
set_land_complete(false);
|
||||
// clear i term when we're taking off
|
||||
if (pilot_climb_rate > 0 && ap.land_complete) {
|
||||
set_throttle_takeoff();
|
||||
}else{
|
||||
// move throttle to minimum to keep us on the ground
|
||||
set_throttle_out(g.throttle_min, false);
|
||||
}
|
||||
|
||||
}
|
||||
// check land_complete flag again in case it was changed above
|
||||
if (!ap.land_complete) {
|
||||
if( sonar_alt_health >= SONAR_ALT_HEALTH_MAX ) {
|
||||
// if sonar is ok, use surface tracking
|
||||
get_throttle_surface_tracking(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us
|
||||
@ -1990,6 +1999,7 @@ void update_throttle_mode(void)
|
||||
// if no sonar fall back stabilize rate controller
|
||||
get_throttle_rate_stabilized(pilot_climb_rate); // this function calls set_target_alt_for_reporting for us
|
||||
}
|
||||
}
|
||||
}else{
|
||||
// pilot's throttle must be at zero so keep motors off
|
||||
set_throttle_out(0, false);
|
||||
|
@ -863,8 +863,14 @@ void throttle_accel_deactivate()
|
||||
static void
|
||||
set_throttle_takeoff()
|
||||
{
|
||||
if (g.pid_throttle_accel.get_integrator() < 0)
|
||||
// set alt target
|
||||
if (controller_desired_alt < current_loc.alt) {
|
||||
controller_desired_alt = current_loc.alt + 20;
|
||||
}
|
||||
// clear i term from acceleration controller
|
||||
if (g.pid_throttle_accel.get_integrator() < 0) {
|
||||
g.pid_throttle_accel.reset_I();
|
||||
}
|
||||
}
|
||||
|
||||
// get_throttle_accel - accelerometer based throttle controller
|
||||
@ -1178,12 +1184,13 @@ static bool update_land_detector()
|
||||
{
|
||||
// detect whether we have landed by watching for low climb rate and minimum throttle
|
||||
if (abs(climb_rate) < 20 && motors.limit.throttle_lower) {
|
||||
if (!ap.land_complete) {
|
||||
// run throttle controller if accel based throttle controller is enabled and active (active means it has been given a target)
|
||||
if( land_detector < LAND_DETECTOR_TRIGGER ) {
|
||||
if( land_detector < LAND_DETECTOR_TRIGGER) {
|
||||
land_detector++;
|
||||
}else{
|
||||
if(!ap.land_complete) {
|
||||
set_land_complete(true);
|
||||
land_detector = 0;
|
||||
}
|
||||
}
|
||||
}else{
|
||||
|
Loading…
Reference in New Issue
Block a user