//void run_flightplan(void) void flightplan_waypoints_update(void) { // first run any injected wp from the serial port if (wp_inject_pos == WP_INJECT_READY) { current_waypoint = wp_to_relative(wp_inject); navigate_set_goal(GPSlocation, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); setBehavior(current_waypoint.flags); navigate_compute_bearing_to_goal(); wp_inject_pos = 0; return; } // steering is based on cross track error. // waypoint arrival is detected computing distance to the "finish line". // note: locations are measured in meters // velocities are in centimeters per second // locations have a range of +-32000 meters (20 miles) from origin if (desired_behavior._.altitude) { if (abs(IMUheight - navigate_get_goal(NULL)) < ((int16_t)altit.HeightMargin)) { next_waypoint(); } } else { if (tofinish_line < WAYPOINT_PROXIMITY_RADIUS) // crossed the finish line { if (desired_behavior._.loiter) { navigate_set_goal(GPSlocation, wp_to_relative(currentWaypointSet[waypointIndex]).loc); } else { next_waypoint(); } } } }
static void generate_next_motion_command(void) { carmen_ackerman_traj_point_t waypoint; int waypoint_index = 0; int waypoint_status = 0; if (!autonomous_status) return; waypoint = g_robot_position; if (current_state == BEHAVIOR_SELECTOR_FOLLOWING_LANE) waypoint_status = next_waypoint(&waypoint, &waypoint_index); if (current_state == BEHAVIOR_SELECTOR_PARKING) waypoint_status = next_waypoint_astar(&waypoint); if (waypoint_status > 0) /* Goal reached? */ { // clear_current_trajectory(); stop_robot(); return; } if (waypoint_status < 0) /* There is no path to the goal? */ { // clear_current_trajectory(); stop_robot(); return; } if (current_state == BEHAVIOR_SELECTOR_FOLLOWING_LANE) send_trajectory_to_robot(path.path + waypoint_index, path.path_size - waypoint_index); if (current_state == BEHAVIOR_SELECTOR_PARKING) publish_astar_path(path.path + waypoint_index, path.path_size - waypoint_index, g_robot_position); //teste_stop(0.1, 2.0); }