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++; }