// returns true if the destination is within fence (used to reject waypoints outside the fence)
bool AC_Fence::check_destination_within_fence(const Location_Class& loc)
{
// Altitude fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX)) {
int32_t alt_above_home_cm;
if (loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_above_home_cm)) {
if ((alt_above_home_cm * 0.01f) > _alt_max) {
return false;
}
}
}
// Circular fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE)) {
if ((get_distance_cm(_ahrs.get_home(), loc) * 0.01f) > _circle_radius) {
return false;
}
}
// polygon fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_POLYGON) && _boundary_num_points > 0) {
// check ekf has a good location
Location temp_loc;
if (_inav.get_location(temp_loc)) {
const struct Location &ekf_origin = _inav.get_origin();
Vector2f position = location_diff(ekf_origin, loc) * 100.0f;
if (_poly_loader.boundary_breached(position, _boundary_num_points, _boundary, true)) {
return false;
}
}
}
return true;
}
// returns true if the destination is within fence (used to reject waypoints outside the fence)
bool AC_Fence::check_destination_within_fence(float dest_alt, float dest_distance_to_home)
{
// Altitude fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) && (dest_alt >= _alt_max)) {
return false;
}
// Circular fence check
if ((get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) && (dest_distance_to_home >= _circle_radius)) {
return false;
}
return true;
}
