// 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; }
// 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; }
// 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; }
// 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; }