bool minLineBetweenLineSegments( ofPoint pt1, ofPoint pt2, ofPoint pt3, ofPoint pt4, ofPoint * pA, ofPoint * pB)
{
// check if they intersect
if( intersectionTwoLines(pt1, pt2, pt3, pt4, pA) )
{
pB->x = pA->x;
pB->y = pA->y;
return true;
}else{
vector<float>distSq(4);
ofPoint s[4];
ofPoint e[4];
e[0] = pt3;
e[1] = pt4;
e[2] = pt1;
e[3] = pt2;
s[0] = distanceToSegment(pt1, pt2, pt3);
s[1] = distanceToSegment(pt1, pt2, pt4);
s[2] = distanceToSegment(pt3, pt4, pt1);
s[3] = distanceToSegment(pt3, pt4, pt2);
distSq[0] = (s[0].x-pt3.x)*(s[0].x-pt3.x)+(s[0].y-pt3.y)*(s[0].y-pt3.y);
distSq[1] = (s[1].x-pt4.x)*(s[1].x-pt4.x)+(s[1].y-pt4.y)*(s[1].y-pt4.y);
distSq[2] = (s[2].x-pt1.x)*(s[2].x-pt1.x)+(s[2].y-pt1.y)*(s[2].y-pt1.y);
distSq[3] = (s[3].x-pt2.x)*(s[3].x-pt2.x)+(s[3].y-pt2.y)*(s[3].y-pt2.y);
int close = 0;
float closeD = 0;
for( int i = 0; i < 4; i++)
{
if( i == 0 || distSq[i] < closeD)
{
close = i;
closeD = distSq[i];
}
}
pA->x = s[close].x;
pA->y = s[close].y;
pB->x = e[close].x;
pB->y = e[close].y;
return false;
}
}
// Signed distance between a point and a polygon
double FFPoint::signedDistanceToPolygon(
size_t& nvert, double* vertx, double* verty, bool expanding){
/* signed distance between a point and a polygon. */
double d = distanceToSegment(vertx[0], verty[0], vertx[nvert-1], verty[nvert-1]);
double di;
for ( size_t i = 0; i < nvert-1; i++ ){
di = distanceToSegment(vertx[i], verty[i], vertx[i+1], verty[i+1]);
if ( di < d ) d = di;
}
if ( pointInPolygon(nvert, vertx, verty) xor expanding ) return -d;
return d;
}
//--------------------------------------------------------------
void ofxMtlMapping2DPolygon::addPoint(int x, int y)
{
//Check if we are not clicking on an already existing vertex
list<ofxMtlMapping2DVertex*>::iterator it;
for (it=vertices.begin(); it!=vertices.end(); it++) {
ofxMtlMapping2DVertex* vertex = *it;
//CE IF JE DOIS POUVOIR LE REMPLACER PAR UNE VAR STATIC.
if (vertex->isMouseOver()){
cout << "EXISTING VERTEX \n";
return;
}
}
//Check if we are not an existing line
if (vertices.size() >= 2) {
int nextVertex = 0;
list<ofxMtlMapping2DVertex*>::iterator it;
list<ofxMtlMapping2DVertex*>::iterator itNext;
for (it=vertices.begin(); it!=vertices.end(); it++) {
it++;
if (it == vertices.end()) {
itNext = vertices.begin();
} else {
itNext = it;
}
it--;
ofxMtlMapping2DVertex* vertex = *it;
ofxMtlMapping2DVertex* nextVertex = *itNext;
double dist = distanceToSegment(vertex->center, nextVertex->center, ofVec2f(x, y));
if ( dist < 10.0) {
//Create a new vertex
ofxMtlMapping2DVertex* newVertex = new ofxMtlMapping2DVertex();
newVertex->init(x-newVertex->width/2, y-newVertex->height/2);
newVertex->enabled = true;
it++;
vertices.insert(it, newVertex);
it--;
return;
}
}
return;
}
//Create a new vertex
ofxMtlMapping2DVertex* newVertex = new ofxMtlMapping2DVertex();
newVertex->init(x-newVertex->width/2, y-newVertex->height/2);
newVertex->enabled = true;
vertices.push_back(newVertex);
}
static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user)
{
// Handle param messages
paramClient->handleMAVLinkPacket(msg);
//check for timed-out operations
struct timeval tv;
gettimeofday(&tv, NULL);
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state);
current_state = PX_WPP_IDLE;
protocol_current_count = 0;
protocol_current_partner_systemid = 0;
protocol_current_partner_compid = 0;
protocol_current_wp_id = -1;
if(waypoints->size() == 0)
{
current_active_wp_id = -1;
}
}
if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size())
{
send_setpoint(current_active_wp_id);
}
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE");
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
yawReached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
yawReached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
yawReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos);
if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
posReached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
if (wp->param2 == 0)
{
dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
}
else
{
dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
}
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_CMD: // special action from ground station
{
mavlink_cmd_t action;
mavlink_msg_cmd_decode(msg, &action);
if(action.target == systemid)
{
if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
switch (action.action)
{
// case MAV_ACTION_LAUNCH:
// if (verbose) std::cerr << "Launch received" << std::endl;
// current_active_wp_id = 0;
// if (waypoints->size()>0)
// {
// setActive(waypoints[current_active_wp_id]);
// }
// else
// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// break;
// case MAV_ACTION_CONTINUE:
// if (verbose) std::c
// err << "Continue received" << std::endl;
// idle = false;
// setActive(waypoints[current_active_wp_id]);
// break;
// case MAV_ACTION_HALT:
// if (verbose) std::cerr << "Halt received" << std::endl;
// idle = true;
// break;
// default:
// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// break;
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
mavlink_waypoint_ack_t wpa;
mavlink_msg_waypoint_ack_decode(msg, &wpa);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid))
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (protocol_current_wp_id == waypoints->size()-1)
{
if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
current_state = PX_WPP_IDLE;
protocol_current_wp_id = 0;
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
mavlink_waypoint_set_current_t wpc;
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE)
{
if (wpc.seq < waypoints->size())
{
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (i == current_active_wp_id)
{
waypoints->at(i)->current = true;
}
else
{
waypoints->at(i)->current = false;
}
}
if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
mavlink_waypoint_request_list_t wprl;
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
if(wprl.target_system == systemid && wprl.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST)
{
if (waypoints->size() > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
current_state = PX_WPP_SENDLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
}
else
{
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
protocol_current_count = waypoints->size();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
mavlink_waypoint_request_t wpr;
mavlink_msg_waypoint_request_decode(msg, &wpr);
if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid)
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
{
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
current_state = PX_WPP_SENDLIST_SENDWPS;
protocol_current_wp_id = wpr.seq;
send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
}
else
{
if (verbose)
{
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
else if (current_state == PX_WPP_SENDLIST)
{
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
}
else if (current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
mavlink_waypoint_count_t wpc;
mavlink_msg_waypoint_count_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0))
{
if (wpc.count > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
protocol_current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints_receive_buffer->back();
waypoints_receive_buffer->pop_back();
}
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
else if (wpc.count == 0)
{
printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
break;
}
else
{
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
}
}
else
{
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT:
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid))
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
{
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1;
mavlink_waypoint_t* newwp = new mavlink_waypoint_t;
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
waypoints_receive_buffer->push_back(newwp);
if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
{
if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
if (current_active_wp_id > waypoints_receive_buffer->size()-1)
{
current_active_wp_id = waypoints_receive_buffer->size() - 1;
}
// switch the waypoints list
std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints;
waypoints = waypoints_receive_buffer;
waypoints_receive_buffer = waypoints_temp;
//get the new current waypoint
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (waypoints->at(i)->current == 1)
{
current_active_wp_id = i;
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
break;
}
}
if (i == waypoints->size())
{
current_active_wp_id = -1;
yawReached = false;
posReached = false;
timestamp_firstinside_orbit = 0;
}
current_state = PX_WPP_IDLE;
}
else
{
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
}
else
{
if (current_state == PX_WPP_IDLE)
{
//we're done receiving waypoints, answer with ack.
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
}
if (verbose)
{
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
else if (current_state == PX_WPP_GETLIST)
{
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else if (current_state == PX_WPP_GETLIST_GETWPS)
{
if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
}
else if(wp.target_system == systemid && wp.target_component == compid)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
mavlink_waypoint_clear_all_t wpca;
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE)
{
protocol_timestamp_lastaction = now;
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
while(waypoints->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
}
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
}
break;
}
default:
{
if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl;
break;
}
}
//check if the current waypoint was reached
if ((posReached && /*yawReached &&*/ !idle))
{
if (current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id);
if (timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
send_waypoint_reached(cur_wp->seq);
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))
if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
current_active_wp_id = 1;
}
else
{
if ((uint16_t)(current_active_wp_id + 1) < waypoints->size())
current_active_wp_id++;
}
// Fly to next waypoint
timestamp_firstinside_orbit = 0;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
waypoints->at(current_active_wp_id)->current = true;
posReached = false;
yawReached = false;
if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id);
}
}
}
}
else
{
timestamp_lastoutside_orbit = now;
}
}
