|
|
|
@ -357,15 +357,17 @@ static bool verify_land()
@@ -357,15 +357,17 @@ static bool verify_land()
|
|
|
|
|
velocity_land = 2000; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if ((current_loc.alt - home.alt) < 500){ |
|
|
|
|
if ((current_loc.alt - home.alt) < 300){ |
|
|
|
|
// a LP filter used to tell if we have landed |
|
|
|
|
// will drive to 0 if we are on the ground - maybe, the baro is noisy |
|
|
|
|
velocity_land = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8; |
|
|
|
|
} |
|
|
|
|
old_alt = current_loc.alt; |
|
|
|
|
|
|
|
|
|
if ((current_loc.alt - home.alt) < 300){ |
|
|
|
|
// remenber altitude for climb_rate |
|
|
|
|
old_alt = current_loc.alt; |
|
|
|
|
|
|
|
|
|
if ((current_loc.alt - home.alt) < 200){ |
|
|
|
|
// don't bank to hold position |
|
|
|
|
wp_control = NO_NAV_MODE; |
|
|
|
|
|
|
|
|
|
// Update by JLN for a safe AUTO landing |
|
|
|
@ -375,20 +377,14 @@ static bool verify_land()
@@ -375,20 +377,14 @@ static bool verify_land()
|
|
|
|
|
g.pi_throttle.reset_I(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if(g.sonar_enabled){ |
|
|
|
|
// decide which sensor we're using |
|
|
|
|
if(sonar_alt < 40){ |
|
|
|
|
if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){ |
|
|
|
|
land_complete = true; |
|
|
|
|
//Serial.println("Y"); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
// reset manual_boost hack |
|
|
|
|
manual_boost = 0; |
|
|
|
|
|
|
|
|
|
if((current_loc.alt - home.alt) < 300 && velocity_land <= 100){ |
|
|
|
|
land_complete = true; |
|
|
|
|
// reset old_alt |
|
|
|
|
old_alt = 0; |
|
|
|
|
init_disarm_motors(); |
|
|
|
|
//init_disarm_motors(); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
//Serial.printf("N, %d\n", velocity_land); |
|
|
|
|