// i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
// i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
// i += put_uint8_t_by_index(version, i, msg->payload); // MAVLink version
if(verbose&¤t_state==PX_WPP_IDLE)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n",msg->sysid);
if(verbose&¤t_state==PX_WPP_SENDLIST)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_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)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n",msg->sysid);
//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(verbose&¤t_state==PX_WPP_SENDLIST)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n",wpr.seq,msg->sysid);
if(verbose&¤t_state==PX_WPP_SENDLIST_SENDWPS&&wpr.seq==protocol_current_wp_id+1)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n",wpr.seq,msg->sysid);
if(verbose&¤t_state==PX_WPP_SENDLIST_SENDWPS&&wpr.seq==protocol_current_wp_id)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n",wpr.seq,msg->sysid);
qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n",current_state);
break;
}elseif(current_state==PX_WPP_SENDLIST){
if(wpr.seq!=0)qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n",wpr.seq);
}elseif(current_state==PX_WPP_SENDLIST_SENDWPS){
if(wpr.seq!=protocol_current_wp_id&&wpr.seq!=protocol_current_wp_id+1)qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_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);
elseif(wpr.seq>=waypoints->size())qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n",wpr.seq);
if(verbose)qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n",msg->sysid,protocol_current_partner_systemid);
if(verbose&¤t_state==PX_WPP_IDLE)qDebug("Got MAVLINK_MSG_ID_MISSION_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n",wpc.count,msg->sysid);
if(verbose&¤t_state==PX_WPP_GETLIST)qDebug("Got MAVLINK_MSG_ID_MISSION_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;
qDebug("clearing receive buffer and readying for receiving waypoints\n");
if(verbose&¤t_state==PX_WPP_GETLIST)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to PX_WPP_GETLIST_GETWPS\n",wp.seq,msg->sysid);
if(verbose&¤t_state==PX_WPP_GETLIST_GETWPS&&wp.seq==protocol_current_wp_id)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n",wp.seq,msg->sysid);
if(verbose&¤t_state==PX_WPP_GETLIST_GETWPS&&wp.seq-1==protocol_current_wp_id)qDebug("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n",wp.seq,msg->sysid);
if(!(wp.seq==protocol_current_wp_id))qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n",wp.seq,protocol_current_wp_id);
elseif(!(wp.seq<protocol_current_count))qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n",wp.seq);
if(verbose)qDebug("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n",wp.seq,msg->sysid,protocol_current_partner_systemid);
boolidle;///< indicates if the system is following the waypoints or is waiting
uint16_tcurrent_active_wp_id;///< id of current waypoint
boolyawReached;///< boolean for yaw attitude reached
boolposReached;///< boolean for position reached
uint64_ttimestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
uint64_ttimestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
std::vector<mavlink_mission_item_t*>waypoints1;///< vector1 that holds the waypoints
std::vector<mavlink_mission_item_t*>waypoints2;///< vector2 that holds the waypoints
std::vector<mavlink_mission_item_t*>*waypoints;///< pointer to the currently active waypoint vector
std::vector<mavlink_mission_item_t*>*waypoints_receive_buffer;///< pointer to the receive buffer waypoint vector