// convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
// returns false if conversion failed (likely because terrain data was not available)
bool AC_WPNav::get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &terrain_alt)
{
// convert location to NE vector2f
Vector2f res_vec;
if (!loc.get_vector_xy_from_origin_NE(res_vec)) {
return false;
}
// convert altitude
if (loc.get_alt_frame() == Location_Class::ALT_FRAME_ABOVE_TERRAIN) {
int32_t terr_alt;
if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, terr_alt)) {
return false;
}
vec.z = terr_alt;
terrain_alt = true;
} else {
terrain_alt = false;
int32_t temp_alt;
if (!loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_ORIGIN, temp_alt)) {
return false;
}
vec.z = temp_alt;
terrain_alt = false;
}
// copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful
vec.x = res_vec.x;
vec.y = res_vec.y;
return true;
}
// 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
Vector2f posNE;
if (loc.get_vector_xy_from_origin_NE(posNE)) {
if (_poly_loader.boundary_breached(posNE, _boundary_num_points, _boundary, true)) {
return false;
}
}
}
return true;
}
