void check_waypoints_reached(absolute_time now, const struct vehicle_global_position_s *global_pos, float turn_distance)
{
mission_command_t current_command;
bool_t time_elapsed = 0;
float vertical_switch_distance, yaw_sp, yaw_err;
float dist = -1.0f, dist_xy = -1.0f, dist_z = -1.0f;
float current_lat = -1.0f, current_lon = -1.0f, current_alt = -1.0f;
float orbit = mission_waypoint_manager_parameters.orbit_def_radius;
float orbit_hold_time = mission_waypoint_manager_parameters.orbit_hold_time;
/* position not valid */
if (!global_pos->valid || check_out_of_bounds(wp_manager.current_setpoint_index, 0, mission.n_commands)) {
/* nothing to check here, return */
return;
}
current_command = mission.command_list[wp_manager.current_setpoint_index];
if (wp_manager.current_setpoint_index < mission.n_commands) {
if (current_command.name == accepted_command_waypoint ||
current_command.name == accepted_command_loiter_time ||
current_command.name == accepted_command_loiter_circle ||
current_command.name == accepted_command_loiter_unlim) {
if (current_command.option4 >= mission_waypoint_manager_parameters.orbit_min_radius)
orbit = current_command.option4;
}
/* keep vertical orbit */
vertical_switch_distance = orbit;
/* Take the larger turn distance - orbit or turn_distance */
if (orbit < turn_distance)
orbit = turn_distance;
current_alt = current_command.option1;
current_lat = (current_command.name == accepted_command_rtl ||
current_command.name == accepted_command_land)? home_position.latitude : current_command.option2;
current_lon = (current_command.name == accepted_command_rtl ||
current_command.name == accepted_command_land)? home_position.longitude : current_command.option3;
dist = wpm_distance_to_point_global_wgs84(current_alt, current_lat, current_lon, global_pos->altitude, (float)global_pos->latitude * 1e-7f, (float)global_pos->longitude * 1e-7f, &dist_xy, &dist_z);
if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
wp_manager.pos_reached = 1;
}
// check if required yaw reached
yaw_sp = get_yaw_to_next_waypoint (wp_manager.current_setpoint_index, wp_manager.next_setpoint_index);
yaw_err = _wrap_pi(yaw_sp - global_pos->yaw);
if (fabsf(yaw_err) < 0.05f) {
wp_manager.yaw_reached = 1;
}
}
//check if the current waypoint was reached
if (wp_manager.pos_reached /* && wp_manager.yaw_reached */) {
if (wp_manager.timestamp_firstinside_orbit == 0) {
// Announce that last waypoint was reached
//wpm_send_waypoint_reached(wp_manager.current_setpoint_index);
wp_manager.timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
if (current_command.name != accepted_command_loiter_unlim && // XXX check this
(now - wp_manager.timestamp_firstinside_orbit >= orbit_hold_time * 1000000 ||
current_command.name == accepted_command_takeoff)) {
time_elapsed = 1;
}
if (time_elapsed) {
current_command.current = 0;
wp_manager.last_setpoint_index = wp_manager.current_setpoint_index;
wp_manager.last_setpoint_valid = wp_manager.current_setpoint_valid;
wp_manager.current_setpoint_index = wp_manager.next_setpoint_index;
wp_manager.current_setpoint_valid = wp_manager.next_setpoint_valid;
if (wp_manager.current_setpoint_valid)
find_next_setpoint_index (wp_manager.current_setpoint_index, &wp_manager.next_setpoint_index, &wp_manager.next_setpoint_valid);
/* if an error occur abort and return to home (then loiter unlim) */
if (!wp_manager.current_setpoint_valid) {
wp_manager.current_setpoint_index = mission.n_commands;
wp_manager.current_setpoint_valid = 1;
}
// Fly to next waypoint
wp_manager.timestamp_firstinside_orbit = 0;
wp_manager.timestamp_lastoutside_orbit = 0;
wp_manager.pos_reached = 0;
wp_manager.yaw_reached = 0;
mission.command_list[wp_manager.current_setpoint_index].current = 1;
//wpm_send_waypoint_current(wp_manager.current_setpoint_index);
missionlib_current_waypoint_changed();
}
}
else {
wp_manager.timestamp_lastoutside_orbit = now;
}
}
void recurssive_define_paths(adjacency_t *adj, int *allowed, int *traversed,
int pos, int size, int offset, int prev)
{
int x, y, top, right, bottom, left, i;
int top_left, top_right, bot_left, bot_right;
int check_top, check_right, check_bot, check_left;
int can_move = 1;
int direction[] = {NORTH, EAST, SOUTH, WEST};
check_top = check_right = check_bot = check_left = 1;
x = get_x(pos, offset);
y = get_y(pos, offset);
if(check_out_of_bounds(x, y, offset, offset))
{
return;
}
if(traversed[pos] == 1)
{
return;
}
top = pos + 1;
bottom = pos - 1;
left = pos - offset;
right = pos + offset;
top_left = pos + 1 - offset;
top_right = pos + 1 + offset;
bot_left = pos - 1 - offset;
bot_right = pos - 1 + offset;
if(y == offset - 1)
{
// upper most cell .`. dont check top
check_top = 0;
check_right = 0;
}
if(y == 0)
{
// lowest most cell .`. dont check bottom
check_bot = 0;
check_left = 0;
}
if(x == 0)
{
// closest cell .`. don't check left
check_top = 0;
check_left = 0;
}
if(x == offset - 1)
{
//furthest most cell .`. don't check right
check_bot = 0;
check_right = 0;
}
if(check_top)
{
check_valid_move(top, top_left, left, allowed, &can_move);
}
if(check_right)
{
check_valid_move(right, top_right, top, allowed, &can_move);
}
if(check_bot)
{
check_valid_move(bottom, bot_right, right, allowed, &can_move);
}
if(check_left)
{
check_valid_move(left, bot_left, bottom, allowed, &can_move);
}
allowed[pos] = can_move;
traversed[pos] = 1;
shuffle_array(direction, 4, prev);
for(i=0; i<4; i++)
{
switch (direction[i])
{
case 0:
recurssive_define_paths(adj, allowed, traversed, top, size, offset, SOUTH);
break;
case 1:
recurssive_define_paths(adj, allowed, traversed, right, size, offset, WEST);
break;
case 2:
recurssive_define_paths(adj, allowed, traversed, bottom, size, offset, NORTH);
break;
case 3:
recurssive_define_paths(adj, allowed, traversed, left, size, offset, EAST);
break;
}
}
}