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;
	}
}
Ejemplo n.º 2
0
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;
        }
    }

}