void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
{
static uint16_t counter;
if (wpm->current_active_wp_id < wpm->size) {
float orbit;
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
} else {
// XXX set default orbit via param
orbit = 15.0f;
}
int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
float dist = -1.0f;
if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
/* Check if conditions of mission item are satisfied */
// XXX TODO
}
if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
wpm->pos_reached = true;
}
// else
// {
// if(counter % 100 == 0)
// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
// }
}
//check if the current waypoint was reached
if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
if (wpm->current_active_wp_id < wpm->size) {
mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
if (wpm->timestamp_firstinside_orbit == 0) {
// Announce that last waypoint was reached
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm->timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
bool time_elapsed = false;
if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
}
} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
}
if (time_elapsed) {
if (cur_wp->autocontinue) {
cur_wp->current = 0;
/* only accept supported navigation waypoints, skip unknown ones */
do {
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated restart the waypoint list from the beginning
*/
wpm->current_active_wp_id = 0;
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
wpm->current_active_wp_id++;
}
} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
// Fly to next waypoint
wpm->timestamp_firstinside_orbit = 0;
mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
wpm->waypoints[wpm->current_active_wp_id].current = true;
wpm->pos_reached = false;
wpm->yaw_reached = false;
printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
}
}
}
} else {
wpm->timestamp_lastoutside_orbit = now;
}
counter++;
}
void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
{
static uint16_t counter;
if (!global_pos->valid && !local_pos->xy_valid) {
/* nothing to check here, return */
return;
}
if (wpm->current_active_wp_id < wpm->size) {
float orbit;
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
} else {
// XXX set default orbit via param
orbit = 15.0f;
}
int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
float dist = -1.0f;
if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
/* Check if conditions of mission item are satisfied */
// XXX TODO
}
if (dist >= 0.f && dist <= orbit) {
wpm->pos_reached = true;
}
// check if required yaw reached
float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
if (fabsf(yaw_err) < 0.05f) {
wpm->yaw_reached = true;
}
}
//check if the current waypoint was reached
if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
if (wpm->current_active_wp_id < wpm->size) {
mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
if (wpm->timestamp_firstinside_orbit == 0) {
// Announce that last waypoint was reached
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm->timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
bool time_elapsed = false;
if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
time_elapsed = true;
} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
time_elapsed = true;
}
if (time_elapsed) {
if (cur_wp->autocontinue) {
cur_wp->current = 0;
float navigation_lat = -1.0f;
float navigation_lon = -1.0f;
float navigation_alt = -1.0f;
int navigation_frame = -1;
/* initialize to current position in case we don't find a suitable navigation waypoint */
if (global_pos->valid) {
navigation_lat = global_pos->lat/1e7;
navigation_lon = global_pos->lon/1e7;
navigation_alt = global_pos->alt;
navigation_frame = MAV_FRAME_GLOBAL;
} else if (local_pos->xy_valid && local_pos->z_valid) {
navigation_lat = local_pos->x;
navigation_lon = local_pos->y;
navigation_alt = local_pos->z;
navigation_frame = MAV_FRAME_LOCAL_NED;
}
/* only accept supported navigation waypoints, skip unknown ones */
do {
/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
/* this is a navigation waypoint */
navigation_frame = cur_wp->frame;
navigation_lat = cur_wp->x;
navigation_lon = cur_wp->y;
navigation_alt = cur_wp->z;
}
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
cur_wp->autocontinue = false;
/* we risk an endless loop for missions without navigation waypoints, abort. */
break;
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
wpm->current_active_wp_id++;
}
} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
// Fly to next waypoint
wpm->timestamp_firstinside_orbit = 0;
mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id);
mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
wpm->waypoints[wpm->current_active_wp_id].current = true;
wpm->pos_reached = false;
wpm->yaw_reached = false;
printf("Set new waypoint (%u)\n", wpm->current_active_wp_id);
}
}
}
} else {
wpm->timestamp_lastoutside_orbit = now;
}
counter++;
}
