// 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 set the circle's circle with circle_nav.set_center() // we assume the caller has performed all required GPS_ok checks void Sub::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(); } }
/// 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); }