mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-23 00:04:02 -04:00
Added home offset for altitude
This commit is contained in:
parent
f74e265bb9
commit
6b9d35cc34
@ -357,14 +357,14 @@ static bool verify_land()
|
|||||||
velocity_land = 2000;
|
velocity_land = 2000;
|
||||||
|
|
||||||
|
|
||||||
if (current_loc.alt < 500){
|
if ((current_loc.alt - home.alt) < 500){
|
||||||
// a LP filter used to tell if we have landed
|
// 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
|
// 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;
|
velocity_land = ((velocity_land * 7) + (old_alt - current_loc.alt)) / 8;
|
||||||
}
|
}
|
||||||
old_alt = current_loc.alt;
|
old_alt = current_loc.alt;
|
||||||
|
|
||||||
if (current_loc.alt < 300){
|
if ((current_loc.alt - home.alt) < 300){
|
||||||
|
|
||||||
wp_control = NO_NAV_MODE;
|
wp_control = NO_NAV_MODE;
|
||||||
|
|
||||||
@ -384,7 +384,7 @@ static bool verify_land()
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if(current_loc.alt < 300 && velocity_land <= 100){
|
if((current_loc.alt - home.alt) < 300 && velocity_land <= 100){
|
||||||
land_complete = true;
|
land_complete = true;
|
||||||
// reset old_alt
|
// reset old_alt
|
||||||
old_alt == 0;
|
old_alt == 0;
|
||||||
|
Loading…
Reference in New Issue
Block a user