// 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 NEU vector3f
Vector3f res_vec;
if (!loc.get_vector_xy_from_origin_NEU(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;
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has performed all required GPS_ok checks
void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, float radius_m)
{
// convert location to vector from ekf origin
Vector3f circle_center_neu;
if (!circle_center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error
circle_center_neu = inertial_nav.get_position();
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
}
circle_nav.set_center(circle_center_neu);
// set circle radius
if (!is_zero(radius_m)) {
circle_nav.set_radius(radius_m * 100.0f);
}
// check our distance from edge of circle
Vector3f circle_edge_neu;
circle_nav.get_closest_point_on_circle(circle_edge_neu);
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
// if more than 3m then fly to edge
if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge(circle_edge_neu);
// convert altitude to same as command
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
// initialise wpnav to move to edge of circle
if (!wp_nav.set_wp_destination(circle_edge)) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event();
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f &curr_pos = inertial_nav.get_position();
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode(AUTO_YAW_HOLD);
}
} else {
auto_circle_start();
}
}
// sets guided mode's target from a Location object
// returns false if destination could not be set (probably caused by missing terrain data)
// or if the fence is enabled and guided waypoint is outside the fence
bool Copter::guided_set_destination(const Location_Class& dest_loc)
{
// ensure we are in position control mode
if (guided_mode != Guided_WP) {
guided_pos_control_start();
}
#if AC_FENCE == ENABLED
// reject destination outside the fence.
// Note: there is a danger that a target specified as a terrain altitude might not be checked if the conversion to alt-above-home fails
int32_t alt_target_cm;
if (dest_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, alt_target_cm)) {
if (!fence.check_destination_within_fence(alt_target_cm*0.01f, get_distance_cm(ahrs.get_home(), dest_loc)*0.01f)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
}
#endif
if (!wp_nav.set_wp_destination(dest_loc)) {
// failure to set destination can only be because of missing terrain data
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
// failure is propagated to GCS with NAK
return false;
}
// log target
Log_Write_GuidedTarget(guided_mode, Vector3f(dest_loc.lat, dest_loc.lng, dest_loc.alt),Vector3f());
return true;
}
bool AC_WPNav::get_wp_destination(Location_Class& destination) {
Vector3f dest = get_wp_destination();
if (!AP::ahrs().get_origin(destination)) {
return false;
}
destination.offset(dest.x*0.01f, dest.y*0.01f);
destination.alt += dest.z;
return true;
}
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE
bool AC_WPNav::set_spline_destination(const Location_Class& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination)
{
// convert destination location to vector
Vector3f dest_neu;
bool dest_terr_alt;
if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) {
return false;
}
// make altitude frames consistent
if (!next_destination.change_alt_frame(destination.get_alt_frame())) {
return false;
}
// convert next destination to vector
Vector3f next_dest_neu;
bool next_dest_terr_alt;
if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) {
return false;
}
// set target as vector from EKF origin
return set_spline_destination(dest_neu, dest_terr_alt, stopped_at_start, seg_end_type, next_dest_neu);
}
// return altitude in cm above home at which vehicle should return home
// rtl_origin_point is the stopping point of the vehicle when rtl is initiated
// rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called
// rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set)
void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed)
{
float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f;
// curr_alt is current altitude above home or above terrain depending upon use_terrain
int32_t curr_alt = current_loc.alt;
// decide if we should use terrain altitudes
rtl_path.terrain_used = terrain_use() && terrain_following_allowed;
if (rtl_path.terrain_used) {
// attempt to retrieve terrain alt for current location, stopping point and origin
int32_t origin_terr_alt, return_target_terr_alt;
if (!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) ||
!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) ||
!current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) {
rtl_path.terrain_used = false;
Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA);
}
}
// maximum of current altitude + climb_min and rtl altitude
float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN));
// don't allow really shallow slopes
if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) {
ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB)));
}
#if AC_FENCE == ENABLED
// ensure not above fence altitude if alt fence is enabled
// Note: we are assuming the fence alt is the same frame as ret
if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) {
ret = MIN(ret, fence.get_safe_alt()*100.0f);
}
#endif
// ensure we do not descend
ret = MAX(ret, curr_alt);
// convert return-target to alt-above-home or alt-above-terrain
if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) {
if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) {
// this should never happen but just in case
rtl_return_target.set_alt_cm(0, Location_Class::ALT_FRAME_ABOVE_HOME);
}
}
// add ret to altitude
rtl_return_target.alt += ret;
}