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