mirror of https://github.com/ArduPilot/ardupilot
Copter: change altitude fence to work on home altitudes instead of origin altitudes
This commit is contained in:
parent
05fda4ee4c
commit
be8c118b63
|
@ -401,7 +401,7 @@ static float get_RTL_alt()
|
|||
#if AC_FENCE == ENABLED
|
||||
// ensure not above fence altitude if alt fence is enabled
|
||||
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
||||
rtl_alt = min(rtl_alt, fence.get_safe_alt()*100.0f);
|
||||
rtl_alt = min(rtl_alt, pv_alt_above_origin(fence.get_safe_alt()*100.0f));
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -20,7 +20,7 @@ void fence_check()
|
|||
fence.set_home_distance(home_distance*0.01f);
|
||||
|
||||
// check for a breach
|
||||
new_breaches = fence.check_fence();
|
||||
new_breaches = fence.check_fence(pv_alt_above_home(inertial_nav.get_altitude()));
|
||||
|
||||
// if there is a new breach take action
|
||||
if( new_breaches != AC_FENCE_TYPE_NONE ) {
|
||||
|
|
Loading…
Reference in New Issue