Index: /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp	(revision 78)
+++ /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp	(revision 78)
@@ -0,0 +1,1035 @@
+//  created:    20/04/2016
+//  updated:    20/09/2016
+//  filename:   MavlinkUDP.cpp
+//
+//  author:     Milan Erdelj, <milan.erdelj@hds.utc.fr>
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//              Current_messages and Time_Stamps:
+//              Copyright (c) 2014 MAVlink Development Team. All rights reserved.
+//              Trent Lukaczyk, <aerialhedgehog@gmail.com>
+//              Jaycee Lock,    <jaycee.lock@gmail.com>
+//              Lorenz Meier,   <lm@inf.ethz.ch>
+//
+//  version:    $Id: $
+//
+//  purpose:    MAVLink communication block class that used UDP sockets
+//
+//
+/*********************************************************************/
+
+#include "MavlinkUDP.h"
+#include <sstream>
+#include <iostream>
+
+using namespace std;
+
+MavlinkUDP::MavlinkUDP(const std::string& addr, int port) : m_port(port), m_addr(addr), me(addr), target(GCS_ADDR) {
+    // initialize attributes
+    write_count = 0;
+    home_position_set=false;
+    ack=false;
+    request=false;
+    seq=-1;
+    seqr=-1;
+    seqold=-1;
+    waypoint=false;
+    Waycount=0;
+    Wayseq=-1;
+    message_interval = 1000; // ms
+    state=-1;
+    control_status = 0;      // whether the autopilot is in offboard control mode
+    arming_status = 0;      // whether the autopilot is in arming control mode
+    debug_messages = true;
+
+    resetCommandAck();
+    resetMissionAck();
+
+    // period function flags
+    sendingAttitude = false;
+    sendingPosition = false;
+    sendingSystemStatus = false;
+    sendingAttitudeInterval = -1;
+    sendingPositionInterval = -1;
+    sendingSystemStatusInterval = -1;
+
+    // mission items
+    missionFirst = 0;
+    missionLast = 0;
+    mission_items_loop = false;
+    missionActive = false; // if the mission is launched
+
+    shutdown_flag = false;
+
+    // TODO read the system ID from the file based on the IP address
+    system_id    = 0; // system id
+    autopilot_id = 0; // autopilot component id
+    component_id = 0; // companion computer component id
+
+   // mavlink_command_long_t com = { 0 };
+    compt=0;
+    current_messages.sysid  = system_id;
+    current_messages.compid = autopilot_id;
+    Xtimes=0;
+    Xtimesync=false;
+
+    something_to_send = false; // check if something needs to be sent
+    stop_recv = false;
+    stop_send = false;
+    startThreads(); // start both sending and receiving thread
+}
+
+MavlinkUDP::~MavlinkUDP() {
+    try {
+        stopThreads();
+    } catch (int error) {
+        fprintf(stderr,"MavlinkUDP: Warning! Could not stop threads!\n");
+    }
+}
+
+// starting send and receive threads
+void MavlinkUDP::startThreads() {
+    // start receive thread
+    recv_th = std::thread(&MavlinkUDP::recv_thread, this);
+    printf("MavlinkUDP: Receiver thread created.\n");
+    // start sending thread
+    send_th = std::thread(&MavlinkUDP::send_thread, this);
+    printf("MavlinkUDP: Sender thread created.\n");
+}
+
+void MavlinkUDP::stopThreads() {
+    stop_recv = true;
+    stop_send = true;
+    usleep(100);
+    if(recv_th.joinable()) recv_th.join();
+    if(send_th.joinable()) send_th.join();
+    printf("MavlinkUDP: Threads stopped.\n");
+    // close the sockets
+    close(m_socket_in);
+    close(m_socket_out);
+    printf("MavlinkUDP: Sockets closed.\n");
+}
+
+// quit handler
+void MavlinkUDP::handleQuit(int sig) {
+    try {
+        stopThreads();
+    } catch (int error) {
+        fprintf(stderr,"MavlinkUDP: Warning, could not stop communication block!\n");
+    }
+}
+
+inline void MavlinkUDP::clearBuffer(uint8_t *buffer, int len) {
+    memset(buffer, 0, len);
+}
+
+void MavlinkUDP::clearData() {
+    clearBuffer(buff_in, BUFF_IN_LEN);
+}
+
+void MavlinkUDP::recv_thread() {
+    // UDP socket setup
+    socklen_t clilen=sizeof(client_in);
+    if((m_socket_in=socket(AF_INET,SOCK_DGRAM,0))<0)
+    {
+        throw SocketRuntimeError("[ERRR] RECV socket error!\n");
+    }
+    memset((char *)&myaddr_in, 0, sizeof(myaddr_in));
+    myaddr_in.sin_family=AF_INET;
+    // receiving messages from all addresses
+    myaddr_in.sin_addr.s_addr=htonl(INADDR_ANY);
+    myaddr_in.sin_port=htons(m_port);
+    if(bind(m_socket_in,(struct sockaddr *)&myaddr_in,sizeof(myaddr_in))<0)
+    {
+        throw SocketRuntimeError("[ERRR] UDP in bind error!\n");
+    }
+    //std::cout << "MavlinkUDP: RECV socket bind OK.\n";
+    std::cout << "MavlinkUDP: RECV thread launched.\n";
+    clearBuffer(buff_in, BUFF_IN_LEN);
+    // receive data
+    while(!stop_recv) {
+        recsize = recvfrom(m_socket_in, (void *)buff_in, BUFF_IN_LEN, 0, (struct sockaddr *)&client_in, &clilen);
+        if(recsize > 0) {
+            // parse the received data
+            //print_message(sockUDP.buff_in, sockUDP.recsize);
+            decode_message(buff_in, recsize);
+            clearBuffer(buff_in, BUFF_IN_LEN);
+        }
+        //usleep(10000);
+        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_IN));
+    }
+}
+
+void MavlinkUDP::send_thread() {
+    // socket setup
+    int broadcast = 1;
+    // socket for broadcast
+    if((m_socket_out=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0) {
+        throw SocketRuntimeError("[ERRR] udp out socket error\n");
+    }
+    if((setsockopt(m_socket_out,SOL_SOCKET,SO_BROADCAST,&broadcast,sizeof broadcast)) == -1) {
+        throw SocketRuntimeError("[ERRR] udp out setsockopt error\n");
+    }
+    cout << "MavlinkUDP: SEND thread launched.\n";
+    memset((char *) &myaddr_out, 0, sizeof(myaddr_out));
+    myaddr_out.sin_family=AF_INET;
+    myaddr_out.sin_port=htons(m_port);
+    myaddr_out.sin_addr.s_addr=inet_addr(m_addr.c_str());
+    clearBuffer(buff_out, BUFF_OUT_LEN);
+    // send data to the socket
+    while(!stop_send) {
+        if(something_to_send) {
+            // send quickly and reset the flag
+            sendto(m_socket_out, buff_out, length_to_send, 0, (struct sockaddr *) &myaddr_out, sizeof(myaddr_out));
+            clearBuffer(buff_out,BUFF_OUT_LEN);
+            something_to_send = false;
+        }
+        //clearBuffer(buff_out, BUFF_OUT_LEN);
+        std::this_thread::sleep_for(std::chrono::milliseconds(THREAD_MS_OUT));
+        //usleep(10000);
+    }
+}
+
+// read mission configuration file
+// TODO use the Qt XML library
+//
+//
+
+void MavlinkUDP::updateSetpoint(mavlink_set_position_target_local_ned_t setpoint) {
+    current_setpoint = setpoint;
+}
+
+//
+// MESSAGES
+//
+
+// send Heartbeat message
+void MavlinkUDP::sendHeartbeat() {
+    mavlink_message_t message;
+    mavlink_msg_heartbeat_pack(me.getSysID(), me.getCompID(), &message, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_AUTO_ARMED, 0, MAV_STATE_ACTIVE);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
+    something_to_send = true;
+    printf("[SENT] Heartbeat\n");
+}
+
+// send SystemStatus message
+void MavlinkUDP::sendSystemStatus(uint32_t onboardSensorsPresent, uint32_t onboardSensorsEnabled, uint32_t onboardSensorsHealth, uint16_t load, uint16_t voltage, int16_t current, int8_t batteryRemaining, uint16_t dropRateComm, uint16_t errorsComm, uint16_t errors1, uint16_t errors2, uint16_t errors3, uint16_t errors4) {
+    mavlink_message_t message;
+    mavlink_msg_sys_status_pack(me.getSysID(), me.getCompID(), &message, onboardSensorsPresent, onboardSensorsEnabled, onboardSensorsHealth, load, voltage, current, batteryRemaining, dropRateComm, errorsComm, errors1, errors2, errors3, errors4);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] System status\n");
+}
+
+// send BatteryStatus message
+void MavlinkUDP::sendBatteryStatus(uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, uint16_t *voltages, int16_t current, int32_t currentConsumed, int32_t energyConsumed, int8_t batteryRemaining) {
+    mavlink_message_t message;
+    mavlink_msg_battery_status_pack(me.getSysID(), me.getCompID(), &message, id, battery_function, type, temperature, voltages, current, currentConsumed, energyConsumed, batteryRemaining);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send SystemTime message
+void MavlinkUDP::sendSystemTime() {
+    mavlink_system_time_t sys_time;
+    sys_time.time_unix_usec = get_time_usec();
+    mavlink_message_t message;
+    mavlink_msg_system_time_pack(me.getSysID(), me.getCompID(), &message, sys_time.time_unix_usec, sys_time.time_boot_ms);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] System time\n");
+}
+
+// send LocalPostiionNED message
+void MavlinkUDP::sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz) {
+    mavlink_message_t message;
+    mavlink_msg_local_position_ned_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), x, y, z, vx, vy, vz);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send Attitude message
+void MavlinkUDP::sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) {
+    mavlink_message_t message;
+    mavlink_msg_attitude_pack(me.getSysID(), me.getCompID(), &message, get_time_usec(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionAck message
+void MavlinkUDP::sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type) {
+    mavlink_message_t message;
+    mavlink_msg_mission_ack_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, type);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] Mission ACK\n");
+}
+
+// send commandAck message
+void MavlinkUDP::sendCommandAck(uint16_t command, uint8_t result) {
+    mavlink_message_t message;
+    mavlink_msg_command_ack_pack(me.getSysID(), me.getCompID(), &message, command, result);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] Command ACK: %d\n", command);
+}
+
+// send autopilot version
+void MavlinkUDP::sendAutopilotVersion(uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, uint8_t *flight_custom_version, uint8_t *middleware_custom_version, uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) {
+    mavlink_message_t message;
+    // TODO get these values from autopilot
+    mavlink_msg_autopilot_version_pack(me.getSysID(), me.getCompID(), &message, capabilities, flight_sw_version, middleware_sw_version, os_sw_version, board_version, flight_custom_version, middleware_custom_version, os_custom_version, vendor_id, product_id, uid);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] Autopilot vesrion\n");
+}
+
+// send MissionCount message
+void MavlinkUDP::sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count) {
+    mavlink_message_t message;
+    mavlink_msg_mission_count_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, count);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send CommandLong message
+void MavlinkUDP::sendCommandLong(uint8_t targetSystem, uint8_t targetComponent, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7) {
+    mavlink_message_t message;
+    //    uint8_t confirmation;
+    //    float param1, param2, param3, param4, param5, param6, param7;
+    mavlink_msg_command_long_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, command, confirmation, param1, param2, param3, param4, param5, param6, param7);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] Command long: %d\n", command);
+}
+
+// send MissionWritePartialList message
+void MavlinkUDP::sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex) {
+    mavlink_message_t message;
+    mavlink_msg_mission_write_partial_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, startIndex, endIndex);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionItem message
+void MavlinkUDP::sendMissionItem(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) {
+    mavlink_message_t message;
+    mavlink_msg_mission_item_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionRequestList message
+void MavlinkUDP::sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent) {
+    mavlink_message_t message;
+    mavlink_msg_mission_request_list_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionItemReached message
+void MavlinkUDP::sendMissionItemReached(uint16_t seq) {
+    mavlink_message_t message;
+    mavlink_msg_mission_item_reached_pack(me.getSysID(), me.getCompID(), &message, seq);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionSetCurrent message
+void MavlinkUDP::sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq) {
+    mavlink_message_t message;
+    mavlink_msg_mission_set_current_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent, seq);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send MissionClearAll message
+void MavlinkUDP::sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent) {
+    mavlink_message_t message;
+    mavlink_msg_mission_clear_all_pack(me.getSysID(), me.getCompID(), &message, targetSystem, targetComponent);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+    printf("[SENT] Mission clear all\n");
+}
+
+// send SetPositionTargetLocalNED message
+void MavlinkUDP::sendSetPositionTargetLocalNED(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
+    mavlink_message_t message;
+    mavlink_msg_set_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, x, y, z, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+// send SetPositionTargetGlobalInt message
+void MavlinkUDP::sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate) {
+    mavlink_message_t message;
+    mavlink_msg_set_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, targetSystem, targetComponent, coordinateFrame, typeMask, lat_int, lon_int, alt, vx, vy, vz, afx, afy, afz, yaw, yaw_rate);
+    length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
+    something_to_send = true;
+}
+
+//
+// COMMANDS
+//
+
+// command Waypoint
+void MavlinkUDP::cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_WAYPOINT, 0, holdTime, proximityRadius, passRadius, desiredYaw, latitude, longitude, altitude);
+}
+
+// command SetMessageInterval
+void MavlinkUDP::cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_SET_MESSAGE_INTERVAL, 0, (float)messageID, (float)interval_usec, 0, 0, 0, 0, 0);
+}
+
+// command Land
+void MavlinkUDP::cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_LAND, 0, abortAlt, 0, 0, desiredYaw, latitude, longitude, altitude);
+}
+
+// command LandStart
+void MavlinkUDP::cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_LAND_START, 0, 0, 0, 0, 0, latitude, longitude, 0);
+}
+
+// command TakeOff
+void MavlinkUDP::cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_TAKEOFF, 0, desiredPitch, 0, 0, magnetometerYaw, latitude, longitude, altitude);
+}
+
+// command DoSetMode
+void MavlinkUDP::cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_MODE, 0, mavMode, 0, 0, 0, 0, 0, 0);
+}
+
+// command DoSetHome
+void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_HOME, 0, (float)useCurrent, 0, 0, 0, 0, 0, 0);
+}
+
+// command MissionStart
+void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_MISSION_START, 0, (float)firstItem, (float)lastItem, 0, 0, 0, 0, 0);
+}
+
+// command DoSetParameter
+void MavlinkUDP::cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_SET_PARAMETER, 0, (float)paramNumber, paramValue, 0, 0, 0, 0, 0);
+}
+
+// command RequestAutopilotCapabilities
+void MavlinkUDP::cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES, 0, 1, 0, 0, 0, 0, 0, 0);
+}
+
+// command ReturnToLaunch
+void MavlinkUDP::cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
+}
+
+// command Shutdown
+void MavlinkUDP::cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer) {
+    // for autopilot and onboardComputer, see TMAV_REBOOT_SHUTDOWN_PARAM
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 0, autopilot, onboardComputer, 0, 0, 0, 0, 0);
+}
+
+// check the received message CRC, return true if OK
+bool MavlinkUDP::check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid) {
+    u_int16_t *crc_accum = new u_int16_t(X25_INIT_CRC);
+    u_int16_t recv_crc;
+    for (int i = 1; i < (recsize-2); ++i)
+        crc_accumulate(buff_in[i],crc_accum);
+    crc_accumulate(hds_mavlink_message_crcs[msgid],crc_accum);
+    recv_crc = buff_in[recsize-1]<<8 ^ buff_in[recsize-2];
+    //cout << "CRC(recv): " << hex << setw(4) << recv_crc << endl;
+    //cout << "CRC(calc): " << hex << setw(4) << *crc_accum << endl;
+    // if the CRCs are the same, the subtraction result should be 0:
+    recv_crc -= *crc_accum;
+    delete crc_accum;
+    if(!recv_crc) return true;
+    return false;
+}
+
+// parse the received message
+void MavlinkUDP::print_message(u_int8_t *buff_in, ssize_t recsize) {
+    uint8_t temp;
+    mavlink_message_t msg;
+    mavlink_status_t status;
+    printf("\n[PRINT MESSAGE]\n");
+    printf("Timestamp: %ld\n", get_time_usec());
+    printf("Bytes received: %ld\n", recsize);
+    printf("Datagram: ");
+    for(int i=0; i<recsize; ++i) {
+        temp = buff_in[i];
+        //cout << base(10) << temp << " ";
+        printf("%02x ", temp);
+        if(mavlink_parse_char(MAVLINK_COMM_0, temp, &msg, &status)) {
+            printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
+            //printf("Iteration: %d\n", i);
+        }
+    }
+    printf("\n");
+
+}
+
+void MavlinkUDP::decode_message(u_int8_t *buff_in, ssize_t msg_size) {
+    mavlink_message_t message;
+    mavlink_status_t status;
+    Time_Stamps this_timestamps;
+    // parse message
+    for(int i=0; i<msg_size; ++i) mavlink_parse_char(MAVLINK_COMM_0, buff_in[i], &message, &status);
+    // handle message ID
+    current_messages.sysid  = message.sysid;
+    current_messages.compid = message.compid;
+//    printf("[DECODE]\n");
+//    printf("System ID: %d\n", message.sysid);
+//    printf("Component ID: %d\n", message.compid);
+//    printf("Message ID: %d\n", message.msgid);
+//    printf("Length: %d\n", message.len);
+
+    if(check_mavlink_crc(buff_in, msg_size, message.msgid))
+    {
+        switch (message.msgid)
+        {
+        case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_CLEAR_ALL\n");
+            mavlink_msg_mission_clear_all_decode(&message,&(current_messages.clear_all));
+            current_messages.time_stamps.clear_all = get_time_usec();
+            this_timestamps.clear_all = current_messages.time_stamps.clear_all;
+            // clear the queue
+            clearMissionPlan();
+            //
+            // TODO signal the app that the vector is cleared
+            //
+            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_ITEM_REACHED:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM_REACHED\n");
+            mavlink_msg_mission_item_reached_decode(&message, &(current_messages.mission_reached));
+            current_messages.time_stamps.mission_reached = get_time_usec();
+            this_timestamps.mission_reached = current_messages.time_stamps.mission_reached;
+            seqr=current_messages.mission_reached.seq;
+            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_COUNT:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_COUNT\n");
+            mavlink_msg_mission_count_decode(&message, &(current_messages.mission_count));
+            current_messages.time_stamps.mission_count = get_time_usec();
+            this_timestamps.mission_count = current_messages.time_stamps.mission_count;
+            Waycount=current_messages.mission_count.count;
+            setMissionCount(current_messages.mission_count.count);
+            compt=0;
+            Wayseq=-1;
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_REQUEST_LIST\n");
+            mavlink_msg_mission_request_list_decode(&message, &(current_messages.mission_request_list));
+            current_messages.time_stamps.mission_request_list = get_time_usec();
+            this_timestamps.mission_request_list = current_messages.time_stamps.mission_request_list;
+            // send the mission count
+            sendMissionCount(target.getSysID(), target.getCompID(), getMissionCount());
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST\n");
+            mavlink_msg_mission_write_partial_list_decode(&message, &(current_messages).mission_write_partial_list);
+            current_messages.time_stamps.mission_write_partial_list = get_time_usec();
+            this_timestamps.mission_write_partial_list = current_messages.time_stamps.mission_write_partial_list;
+            //
+            // TODO process the partial list message
+            //
+            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
+            break;
+        }
+        case MAVLINK_MSG_ID_TIMESYNC:
+        {
+            //  printf("MAVLINK_MSG_ID_TIMESYNC\n");
+            mavlink_msg_timesync_decode(&message, &(current_messages.timesync));
+            current_messages.time_stamps.timesync = get_time_usec();
+            this_timestamps.timesync = current_messages.time_stamps.timesync;
+            Xtimec=current_messages.timesync.tc1;
+            Xtimes=current_messages.timesync.ts1;
+            Xtimesync=true;
+            break;
+        }
+        case MAVLINK_MSG_ID_SYSTEM_TIME:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_SYSTEM_TIME\n");
+            mavlink_msg_system_time_decode(&message, &(current_messages.system_time));
+            current_messages.time_stamps.system_time = get_time_usec();
+            this_timestamps.system_time = current_messages.time_stamps.system_time;
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_ITEM:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_ITEM\n");
+            mavlink_msg_mission_item_decode(&message, &(current_messages.mission_item));
+            current_messages.time_stamps.mission_item = get_time_usec();
+            this_timestamps.mission_item = current_messages.time_stamps.mission_item;
+
+            // * Mission items queue:
+            // * 16   MAV_CMD_NAV_WAYPOINT            (hold time, acceptance radius, yaw angle, lat, long, altitude)
+            // * 21   MAV_CMD_NAV_LAND                (abort alt, yaw angle, lat, long, altitude)
+            // * 22   MAV_CMD_NAV_TAKEOFF             (yaw angle, lat, long, altitude)
+            // * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
+            // * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
+
+            //waypoint=true;
+            //setMissionCount(getMissionCount()+1);
+            //Wayseq=current_messages.mission_item.seq;
+            //frame=current_messages.mission_item.frame;
+            command=current_messages.mission_item.command;
+
+            switch(command) {
+            case MAV_CMD_NAV_TAKEOFF:
+            {
+                printf("[RECV] MISSION ITEM: TAKEOFF\n");
+                MissionItem item;
+                item.itemID = command;
+                item.sequence = mavlink_msg_mission_item_get_seq(&message);
+                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
+                item.latitude = mavlink_msg_mission_item_get_x(&message);
+                item.longitude = mavlink_msg_mission_item_get_y(&message);
+                item.altitude = mavlink_msg_mission_item_get_z(&message);
+                missionPlan.push(item);
+                break;
+            }
+            case MAV_CMD_NAV_LAND:
+            {
+                printf("[RECV] MISSION ITEM: LAND\n");
+                MissionItem item;
+                item.itemID = command;
+                item.sequence = mavlink_msg_mission_item_get_seq(&message);
+                item.abortAltitude = mavlink_msg_mission_item_get_param1(&message);
+                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
+                item.latitude = mavlink_msg_mission_item_get_x(&message);
+                item.longitude = mavlink_msg_mission_item_get_y(&message);
+                item.altitude = mavlink_msg_mission_item_get_z(&message);
+                missionPlan.push(item);
+                break;
+            }
+            case MAV_CMD_NAV_WAYPOINT:
+            {
+                MissionItem item;
+                item.itemID = command;
+                item.sequence = mavlink_msg_mission_item_get_seq(&message);
+                //item.type = (uint8_t)mavlink_msg_mission_item_get_param3(&message);
+                item.holdTime = mavlink_msg_mission_item_get_param1(&message);
+                item.yawAngle = mavlink_msg_mission_item_get_param4(&message);
+                item.latitude = mavlink_msg_mission_item_get_x(&message);
+                item.longitude = mavlink_msg_mission_item_get_y(&message);
+                item.altitude = mavlink_msg_mission_item_get_z(&message);
+                missionPlan.push(item);
+                //printf("[RECV] WAYPOINT: seq = %d, x = %.2f, y = %.2f, type = %d\n", mavlink_msg_mission_item_get_seq(&message), recvWaypoint.m_x, recvWaypoint.m_y, (int)recvWaypoint.m_type);
+                printf("[RECV] WAYPOINT: seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", item.sequence, item.latitude, item.longitude, item.altitude);
+                break;
+            }
+            }
+            // respond with ACK immediately
+            sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_ACK:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_MISSION_ACK\n");
+            mavlink_msg_mission_ack_decode(&message, &(current_messages.mission_ack));
+            // check if it is the right mission ack
+            current_messages.time_stamps.mission_ack = get_time_usec();
+            this_timestamps.mission_ack = current_messages.time_stamps.mission_ack;
+            setMissionAck();
+            //typeack=current_messages.mission_ack.type;
+            break;
+        }
+        case MAVLINK_MSG_ID_COMMAND_ACK:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_COMMAND_ACK\n");
+            mavlink_msg_command_ack_decode(&message, &(current_messages.command_ack));
+            // check if it is the right command ack
+            current_messages.time_stamps.command_ack = get_time_usec();
+            this_timestamps.command_ack = current_messages.time_stamps.command_ack;
+            setCommandAck();
+            break;
+        }
+        case MAVLINK_MSG_ID_MISSION_REQUEST:
+        {
+            //    printf("MAVLINK_MSG_ID_MISSION_REQUEST\n");
+            mavlink_msg_mission_request_decode(&message, &(current_messages.mission_request));
+            seq=current_messages.mission_request.seq;
+            current_messages.time_stamps.mission_request = get_time_usec();
+            this_timestamps.mission_request = current_messages.time_stamps.mission_request;
+            request=true;
+            break;
+        }
+        case MAVLINK_MSG_ID_HOME_POSITION:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_HOME_POSITION\n");
+            mavlink_msg_home_position_decode(&message, &(current_messages.home_position));
+            current_messages.time_stamps.home_position = get_time_usec();
+            this_timestamps.home_position = current_messages.time_stamps.home_position;
+            home_position_set=true;
+            break;
+        }
+        case MAVLINK_MSG_ID_HEARTBEAT:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_HEARTBEAT\n");
+            mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));
+            current_messages.time_stamps.heartbeat = get_time_usec();
+            this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
+            state=current_messages.heartbeat.system_status;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_SYS_STATUS:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_SYS_STATUS\n");
+            mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));
+            current_messages.time_stamps.sys_status = get_time_usec();
+            this_timestamps.sys_status = current_messages.time_stamps.sys_status;
+            //USB_port->write_message(message);
+            break;
+        }
+
+        case MAVLINK_MSG_ID_BATTERY_STATUS:
+        {
+            //printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");
+            mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));
+            current_messages.time_stamps.battery_status = get_time_usec();
+            this_timestamps.battery_status = current_messages.time_stamps.battery_status;
+            //USB_port->write_message(message);
+            break;
+        }
+
+        case MAVLINK_MSG_ID_RADIO_STATUS:
+        {
+            //printf("MAVLINK_MSG_ID_RADIO_STATUS\n");
+            mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));
+            current_messages.time_stamps.radio_status = get_time_usec();
+            this_timestamps.radio_status = current_messages.time_stamps.radio_status;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
+        {
+            printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
+            mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
+            current_messages.time_stamps.local_position_ned = get_time_usec();
+            this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
+        {
+            printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
+            mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));
+            current_messages.time_stamps.global_position_int = get_time_usec();
+            this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;
+            //USB_port->write_message(message);
+            break;
+        }
+
+        case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
+        {
+            printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
+            mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));
+            current_messages.time_stamps.position_target_local_ned = get_time_usec();
+            this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
+        {
+            printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
+            mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));
+            current_messages.time_stamps.position_target_global_int = get_time_usec();
+            this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_HIGHRES_IMU:
+        {
+            //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
+            mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
+            current_messages.time_stamps.highres_imu = get_time_usec();
+            this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_ATTITUDE:
+        {
+            //printf("MAVLINK_MSG_ID_ATTITUDE\n");
+            mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
+            current_messages.time_stamps.attitude = get_time_usec();
+            this_timestamps.attitude = current_messages.time_stamps.attitude;
+            break;
+        }
+
+        case MAVLINK_MSG_ID_COMMAND_LONG:
+        {
+            //printf("[RECV] MAVLINK_MSG_ID_COMMAND_LONG\n");
+            mavlink_msg_command_long_decode(&message, &(current_messages.command_long));
+            current_messages.time_stamps.command_long = get_time_usec();
+            this_timestamps.command_long = current_messages.time_stamps.command_long;
+            // process the received command
+            printf("Received command %d\n", current_messages.command_long.command);
+            switch(current_messages.command_long.command) {
+            case MAV_CMD_DO_SET_PARAMETER:
+                switch((int)current_messages.command_long.param1) {
+                case MAV_LOOP_MISSION_ITEMS:
+                    if((int)current_messages.command_long.param2) {
+                        itemsLoop();
+                    } else {
+                        itemsNoLoop();
+                    }
+
+                    break;
+                default:
+                    break;
+                }
+
+                break;
+            case MAV_CMD_SET_MESSAGE_INTERVAL:
+                cout << "[DEBUG] callback interval value: " << (int64_t)current_messages.command_long.param2 << endl;
+                int64_t desired_interval;
+                desired_interval = (int64_t)current_messages.command_long.param2;
+                switch((int)current_messages.command_long.param1) {
+                case MAVLINK_MSG_ID_HEARTBEAT:
+                    if(current_messages.command_long.param2 == -1) {
+                        // stop sending position
+                        sendingHeartbeat = false;
+                    } else {
+                        // start sending position
+                        sendingHeartbeatInterval = desired_interval;
+                        sendingHeartbeat = true;
+                    }
+                    break;
+                case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
+                    if(current_messages.command_long.param2 == -1) {
+                        // stop sending position
+                        sendingPosition = false;
+                    } else {
+                        // start sending position
+                        sendingPositionInterval = desired_interval;
+                        sendingPosition = true;
+                    }
+                    break;
+                case MAVLINK_MSG_ID_ATTITUDE:
+                    if(current_messages.command_long.param2 == -1) {
+                        // stop sending attitude
+                        sendingAttitude = false;
+                    } else {
+                        // start sending attitude
+                        sendingAttitudeInterval = desired_interval;
+                        sendingAttitude = true;
+                    }
+                    break;
+                case MAVLINK_MSG_ID_SYS_STATUS:
+                    if(current_messages.command_long.param2 == -1) {
+                        // stop sending status
+                        sendingSystemStatus = false;
+                    } else {
+                        // start sending status
+                        sendingSystemStatusInterval = desired_interval;
+                        sendingSystemStatus = true;
+                    }
+                    break;
+                default:
+                    break;
+                }
+
+                break;
+            case MAV_CMD_MISSION_START:
+            {
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missCom.firstItem = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                missCom.lastItem = (uint16_t)mavlink_msg_command_long_get_param2(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] MAV_CMD_MISSION_START: firstItem = %d, lastItem = %d\n", missCom.firstItem, missCom.lastItem);
+                missionStarted();
+                break;
+            }
+            case MAV_CMD_DO_LAND_START:
+                // TODO do land start
+                break;
+            case MAV_CMD_DO_SET_HOME:
+            {
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missCom.useCurrent = (uint8_t)mavlink_msg_command_long_get_param1(&message);
+                missCom.latitude = mavlink_msg_command_long_get_param5(&message);
+                missCom.longitude = mavlink_msg_command_long_get_param6(&message);
+                missCom.altitude = mavlink_msg_command_long_get_param7(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] SET HOME: use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", missCom.useCurrent, missCom.latitude, missCom.longitude, missCom.altitude);
+                break;
+            }
+            case MAV_CMD_GET_HOME_POSITION:
+            {
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] GET HOME POSITION\n");
+                break;
+            }
+            case MAV_CMD_DO_SET_MODE:
+            {
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missCom.mode = mavlink_msg_command_long_get_param1(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] SET MODE: %d\n", missCom.mode);
+                break;
+            }
+            case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+                // TODO request autopilot capabilities
+                break;
+            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+            {
+                MissionItem missItem;
+                missItem.itemID = mavlink_msg_command_long_get_command(&message);
+                missionPlan.push(missItem);
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] Return to launch.\n");
+                break;
+            }
+            case MAV_CMD_DO_REPOSITION:
+                // TODO reposition
+                printf("[TODO] Do reposition.\n");
+                break;
+            case MAV_CMD_NAV_TAKEOFF:
+                // TODO takeoff
+                printf("[TODO] Execute takeoff.\n");
+                break;
+            case MAV_CMD_NAV_LAND:
+                // TODO emergency landing
+                printf("[TODO] Execute emergency landing.\n");
+                break;
+            case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+                // shutdown signal received
+                shutdown_flag = true;
+                break;
+            case MAV_CMD_DO_JUMP:
+            {
+                // jump to a desired command in mission list, repeat a specified number of times
+                MissionItem missItem;
+                missItem.itemID = mavlink_msg_command_long_get_command(&message);
+                missItem.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                missItem.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
+                missionPlan.push(missItem);
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missCom.sequence = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                missCom.repeatCount = (uint16_t)mavlink_msg_command_long_get_param2(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] MAV_CMD_DO_JUMP: seq = %d, repeat = %d\n", missItem.sequence, missItem.repeatCount);
+            }
+            case MAV_CMD_DO_PAUSE_CONTINUE:
+            {
+                MissionCommand missCom;
+                missCom.commandId = mavlink_msg_command_long_get_command(&message);
+                missCom.pauseContinue = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                missionCommands.push(missCom);
+                printf("[RECV] MAV_CMD_DO_PAUSE_CONTINUE: pauseContinue = %d\n", missCom.pauseContinue);
+            }
+            default:
+                break;
+            }
+            // respond with ACK immediately
+            sendCommandAck(current_messages.command_long.command, MAV_CMD_ACK_OK);
+            break;
+        }
+        default:
+        {
+            printf("Warning, did not handle message id %i\n",message.msgid);
+            break;
+        }
+        }
+    } else {
+        printf("ERROR: CRC check failed!\n");
+    }
+}
+
+uint64_t MavlinkUDP::get_time_usec() {
+    struct timeval tv;
+    gettimeofday(&tv, NULL);
+    return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
+}
+
+void MavlinkUDP::debug(string debug_msg) {
+    if(debug_messages) cout << "[DEBUG] " << debug_msg << endl;
+}
+
+
+int MavlinkUDP::waitCommandAck(uint64_t timeout) {
+    uint64_t start_time = get_time_usec();
+    uint64_t end_time = start_time + timeout;
+    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
+    while(get_time_usec() < end_time)
+    {
+        if(getCommandAck()){
+            //printf("Command ACK received!\n");
+            resetCommandAck();
+            return 0;
+        }
+        usleep(1000);
+    }
+    printf("[ERROR] Command ACK timeout! %d\n", getCommandAck());
+    return -1;
+}
+
+int MavlinkUDP::waitMissionAck(uint64_t timeout) {
+    uint64_t start_time = get_time_usec();
+    uint64_t end_time = start_time + timeout;
+    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
+    while(get_time_usec() < end_time)
+    {
+        if(getMissionAck()){
+            //printf("Mission ACK received!\n");
+            resetMissionAck();
+            return 0;
+        }
+        usleep(1000);
+    }
+    printf("[ERROR] Mission ACK timeout!\n");
+    return -1;
+}
+
+int MavlinkUDP::waitMissionCount(uint64_t timeout) {
+    uint64_t start_time = get_time_usec();
+    uint64_t end_time = start_time + timeout;
+    int mission_count;
+    //printf("Wait: start_time %lld end_time %lld period %lld\n", start_time, end_time, end_time-start_time);
+    while(get_time_usec() < end_time)
+    {
+        // assignment in if!
+        if((mission_count = getMissionCount())){
+            //printf("Mission ACK received!\n");
+            resetMissionCount();
+            return mission_count;
+        }
+        usleep(1000);
+    }
+    printf("[ERROR] Mission ACK timeout!\n");
+    return -1;
+}
+
+MavlinkComponent::MavlinkComponent(std::string addr) : addrIP(addr) {
+    // craete the system ID out of the IP address
+    int byte1, byte2, byte3, byte4;
+    char dot = '.';
+    std::istringstream s(addrIP);
+    s >> byte1 >> dot >> byte2 >> dot >> byte3 >> dot >> byte4;
+    sysid = byte4;
+    compid = 0;
+    printf("Mavlink component created:\tSystem ID %d\tComponent ID %d\n", sysid, compid);
+}
Index: /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h	(revision 78)
+++ /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h	(revision 78)
@@ -0,0 +1,473 @@
+//  created:    20/04/2016
+//  updated:    20/09/2016
+//  filename:   MavlinkUDP.h
+//
+//  author:     Milan Erdelj, <milan.erdelj@hds.utc.fr>
+//              Copyright Heudiasyc UMR UTC/CNRS 7253
+//
+//              Current_messages and Time_Stamps:
+//              Copyright (c) 2014 MAVlink Development Team. All rights reserved.
+//              Trent Lukaczyk, <aerialhedgehog@gmail.com>
+//              Jaycee Lock,    <jaycee.lock@gmail.com>
+//              Lorenz Meier,   <lm@inf.ethz.ch>
+//
+//  version:    $Id: $
+//
+//  purpose:    MAVLink communication block class that used UDP sockets
+//
+//
+/*********************************************************************/
+
+#ifndef MAVLINKUDP_H
+#define MAVLINKUDP_H
+
+#include <thread>
+#include <iostream>
+#include <queue>
+#include <arpa/inet.h>
+#include <cstring>
+#include <unistd.h>
+#include <sys/time.h>
+#include "include/common/mavlink.h"
+
+// TODO these constants should be read from a file
+#define LOCALHOST "127.0.0.1"
+#define MAVLINK_UDP_PORT 5555
+#define GCS_ADDR "192.168.6.150"
+#define UAV2 "192.168.6.2"
+#define UAV1 "192.168.6.1"
+#define PC_LATITUDE "192.168.6.100"
+
+#define MAX_NEIGH 50
+
+#define THREAD_MS_IN 10
+#define THREAD_MS_OUT 10
+
+#define BUFF_IN_LEN 80
+#define BUFF_OUT_LEN 80
+
+#define ACK_TIMEOUT 100000 // us
+
+#ifndef MAV_CUSTOM_PARAM
+#define MAV_CUSTOM_PARAM
+
+enum TMAVCustomParams {
+    MAV_LOOP_MISSION_ITEMS=42,
+    MAV_CUSTOM_PARAM1=43,
+    MAV_CUSTOM_PARAM2=44
+};
+
+#endif
+
+#ifndef REBOOT_SHUTDOWN_PARAMS
+#define REBOOT_SHUTDOWN_PARAMS
+
+enum TRebootShutdownParam {
+    NO_EFFECT=0,
+    REBOOT=1,
+    SHUTDOWN=2,
+    REBOOT_KEEP_IN_BOOTLOADER=3
+};
+
+#endif
+
+struct Time_Stamps
+{
+    Time_Stamps() {
+        reset_timestamps();
+    }
+    uint64_t mission_ack;
+    uint64_t command_ack;
+    uint64_t command_long;
+    uint64_t timesync;
+    uint64_t mission_count;
+    uint64_t mission_request;
+    uint64_t mission_reached;
+    uint64_t mission_item;
+    uint64_t mission_request_list;
+    uint64_t mission_write_partial_list;
+    uint64_t heartbeat;
+    uint64_t home_position;
+    uint64_t sys_status;
+    uint64_t battery_status;
+    uint64_t radio_status;
+    uint64_t local_position_ned;
+    uint64_t global_position_int;
+    uint64_t position_target_local_ned;
+    uint64_t position_target_global_int;
+    uint64_t highres_imu;
+    uint64_t attitude;
+    uint64_t system_time;
+    uint64_t clear_all;
+
+    void reset_timestamps() {
+        mission_request=0;
+        mission_request_list=0;
+        mission_write_partial_list=0;
+        mission_reached=0;
+        command_long=0;
+        timesync=0;
+        mission_count=0;
+        mission_item=0;
+        mission_ack=0;
+        home_position =0;
+        heartbeat = 0;
+        sys_status = 0;
+        battery_status = 0;
+        radio_status = 0;
+        local_position_ned = 0;
+        global_position_int = 0;
+        position_target_local_ned = 0;
+        position_target_global_int = 0;
+        highres_imu = 0;
+        attitude = 0;
+        system_time = 0;
+        clear_all = 0;
+    }
+};
+
+// Struct containing information on the MAV we are currently connected to
+struct Mavlink_Messages {
+    int sysid;
+    int compid;
+    // system time
+    mavlink_system_time_t system_time;
+    // mission_request
+    mavlink_mission_request_t mission_request;
+    // mission request list
+    mavlink_mission_request_list_t mission_request_list;
+    // mission write partial list
+    mavlink_mission_write_partial_list_t mission_write_partial_list;
+    // mission_reached
+    mavlink_mission_item_reached_t mission_reached;
+    // mission_clear_all
+    mavlink_mission_clear_all_t clear_all;
+    // command long
+    mavlink_command_long_t command_long;
+    // command_ack
+    mavlink_command_ack_t command_ack;
+    // timesync
+    mavlink_timesync_t timesync;
+    // mission_count
+    mavlink_mission_count_t mission_count;
+    // mission_ack
+    mavlink_mission_ack_t mission_ack;
+    // mission_item
+    mavlink_mission_item_t mission_item;
+    // Homeposition
+    mavlink_home_position_t home_position;
+    // Heartbeat
+    mavlink_heartbeat_t heartbeat;
+    // System Status
+    mavlink_sys_status_t sys_status;
+    // Battery Status
+    mavlink_battery_status_t battery_status;
+    // Radio Status
+    mavlink_radio_status_t radio_status;
+    // Local Position
+    mavlink_local_position_ned_t local_position_ned;
+    // Global Position
+    mavlink_global_position_int_t global_position_int;
+    // Local Position Target
+    mavlink_position_target_local_ned_t position_target_local_ned;
+    // Global Position Target
+    mavlink_position_target_global_int_t position_target_global_int;
+    // HiRes IMU
+    mavlink_highres_imu_t highres_imu;
+    // Attitude
+    mavlink_attitude_t attitude;
+    // System Parameters?
+    // To add
+    // Time Stamps
+    Time_Stamps time_stamps;
+    void reset_timestamps() {
+        time_stamps.reset_timestamps();
+    }
+};
+
+// * Mission items queue:
+// * 16   MAV_CMD_NAV_WAYPOINT            (hold time, acceptance radius, yaw angle, lat, long, altitude)
+// * 21   MAV_CMD_NAV_LAND                (abort alt, yaw angle, lat, long, altitude)
+// * 22   MAV_CMD_NAV_TAKEOFF             (yaw angle, lat, long, altitude)
+// * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
+// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
+
+struct MissionItem {
+    uint16_t itemID;
+    uint8_t type;
+    uint64_t holdTime;
+    float radius;
+    float yawAngle;
+    float latitude;
+    float longitude;
+    float altitude;
+    float x, y, z;
+    float abortAltitude;
+    uint16_t sequence;
+    uint16_t repeatCount;
+    MissionItem() {
+        // struct construtor resets all the values
+        itemID = sequence = repeatCount = 0;
+        holdTime = 0;
+        radius = yawAngle = latitude = longitude = altitude = abortAltitude = x = y = z = 0;
+    }
+};
+
+// * Commands queue:
+// * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
+// * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
+// * 179  MAV_CMD_DO_SET_HOME             (use current, lat, long, altitude)
+// * 193  MAV_CMD_DO_PAUSE_CONTINUE       (holdContinue: 0=hold, 1=continue)
+// * 300  MAV_CMD_MISSION_START           (first item, last item)
+// * 410  MAV_CMD_GET_HOME_POS            (empty)
+// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
+
+struct MissionCommand {
+    uint16_t commandId;
+    // DO_JUMP
+    uint16_t sequence;
+    uint16_t repeatCount;
+    // DO_SET_HOME
+    uint8_t useCurrent;
+    float latitude, longitude, altitude;
+    float x, y, z;
+    // DO_PAUSE_CONTINUE
+    uint8_t pauseContinue;
+    // MISSION_START
+    uint16_t firstItem, lastItem;
+    // DO_SET_MODE
+    uint8_t mode;
+    MissionCommand() {
+        // reset all the attributes
+        pauseContinue = useCurrent = 0;
+        commandId = sequence = repeatCount = 0;
+        latitude = longitude = altitude = x = y = z = 0;
+    }
+};
+
+class SocketRuntimeError {
+private:
+    std::string info;
+public:
+    SocketRuntimeError(const std::string& str) : info(str) {}
+    std::string getInfo() const { return info; }
+};
+
+class MavlinkRuntimeError {
+private:
+    std::string info;
+public:
+    MavlinkRuntimeError(const std::string& str) : info(str) {}
+    std::string getInfo() const { return info; }
+};
+
+class MavlinkComponent
+{
+public:
+    MavlinkComponent(std::string addr);
+    //MavComAgent();
+    ~MavlinkComponent() {}
+    uint8_t getSysID() const { return sysid; }
+    uint8_t getCompID() const { return compid; }
+    std::string getAddrIP() { return addrIP; }
+private:
+    uint8_t sysid; // system ID
+    uint8_t compid; // component ID
+    std::string addrIP; // IP address
+};
+
+class MavlinkUDP {
+public:
+    MavlinkUDP(const std::string& addr, int port);
+    ~MavlinkUDP();
+
+    // begin SockUDP
+    // threads start and stop
+    void startThreads();
+    void stopThreads();
+    void handleQuit( int sig );
+    int getSocket() const { return m_socket_in; }
+    int getPort() const { return m_port; }
+    std::string getAddr() const { return m_addr; }
+    bool something_to_send;
+    uint16_t length_to_send;
+    u_int8_t buff_out[BUFF_OUT_LEN];
+    u_int8_t buff_in[BUFF_IN_LEN];
+    ssize_t recsize;
+    socklen_t fromlen;
+    void clearData();
+    // end SockUDP
+
+
+    char control_status;
+    char arming_status;
+    uint64_t write_count; // number of messages sent
+    uint64_t Xtimec;
+    uint64_t Xtimes;
+
+    int system_id;
+    int autopilot_id;
+    int component_id;
+    bool home_position_set;
+    bool ack;
+    bool Xtimesync;
+    int state;
+    int typeack;
+    bool request;
+    bool waypoint;
+    int frame;
+    int command;
+    int seq;
+    int seqr;
+    int seqold;
+    int count;
+    int Waycount;
+    int Wayseq;
+    int compt;
+    uint16_t message_interval;
+    bool debug_messages;
+
+    bool sendingPosition;
+    uint16_t sendingPositionInterval;
+    bool sendingAttitude;
+    uint16_t sendingAttitudeInterval;
+    bool sendingSystemStatus;
+    uint16_t sendingSystemStatusInterval;
+    bool sendingHeartbeat;
+    uint16_t sendingHeartbeatInterval;
+
+    // queue of mission items (plan de vol)
+    std::queue<MissionItem> missionPlan;
+    //void getMissionPlan(std::vector<Waypoint>& x) { x =  missionPlan; }
+    const std::queue<MissionItem>& getMissionPlan() const { return missionPlan; }
+    void clearMissionPlan() { std::queue<MissionItem> empty; std::swap(missionPlan, empty); }
+    uint16_t currentMissionItem;
+    uint16_t missionFirst;
+    uint16_t missionLast;
+
+    // queue of received navigation commands
+    std::queue<MissionCommand> missionCommands;
+    const std::queue<MissionCommand>& getMissionCommands() const { return missionCommands; }
+    void clearMissionCommands() { std::queue<MissionCommand> empty; std::swap(missionCommands, empty); }
+
+    bool mission_items_loop;
+    void itemsLoop() { mission_items_loop = true; }
+    void itemsNoLoop() { mission_items_loop = false; }
+    bool getItemsLoop() const { return mission_items_loop; }
+
+    bool missionActive;
+    bool getMissionStatus() const { return missionActive; }
+    void missionStarted() { missionActive = true; }
+    void missionStopped() { missionActive = false; }
+
+    mavlink_command_long_t com;
+    Mavlink_Messages current_messages;
+    mavlink_set_position_target_local_ned_t initial_position;
+    Time_Stamps last_timestamps;
+
+    MavlinkComponent me, target; // destination system
+
+    bool check_mavlink_crc(u_int8_t *buff_in, ssize_t recsize, u_int8_t msgid);
+    void decode_message(u_int8_t *buff_in, ssize_t size);
+    void print_message(u_int8_t *buff_in, ssize_t recsize);
+
+    // messages
+    void sendHeartbeat();
+    void sendSystemStatus(uint32_t onboardSensorsPresent, uint32_t onboardSensorsEnabled, uint32_t onboardSensorsHealth, uint16_t load, uint16_t voltage, int16_t current, int8_t batteryRemaining, uint16_t dropRateComm, uint16_t errorsComm, uint16_t errors1, uint16_t errors2, uint16_t errors3, uint16_t errors4); // callback
+    void sendBatteryStatus(uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, uint16_t *voltages, int16_t current, int32_t currentConsumed, int32_t energyConsumed, int8_t batteryRemaining); // callback
+    void sendSystemTime();
+    void sendLocalPositionNED(float x, float y, float z, float vx, float vy, float vz); // callback
+    void sendAttitude(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed); // callback
+    void updateSetpoint(mavlink_set_position_target_local_ned_t setpoint);
+    void sendMissionAck(uint8_t targetSystem, uint8_t targetComponent, uint8_t type);
+    void sendCommandAck(uint16_t command, uint8_t result);
+    void sendAutopilotVersion(uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, uint8_t *flight_custom_version, uint8_t *middleware_custom_version, uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid);
+    void sendMissionCount(uint8_t targetSystem, uint8_t targetComponent, uint16_t count);
+    void sendCommandLong(uint8_t targetSystem, uint8_t targetComponent, uint16_t command, uint8_t confirmation, float param1, float param2, float param3, float param4, float param5, float param6, float param7);
+    void sendMissionWritePartialList(uint8_t targetSystem, uint8_t targetComponent, uint16_t startIndex, uint16_t endIndex);
+    void sendMissionItem(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z);
+    void sendMissionRequestList(uint8_t targetSystem, uint8_t targetComponent);
+    void sendMissionItemReached(uint16_t seq);
+    void sendMissionSetCurrent(uint8_t targetSystem, uint8_t targetComponent, uint16_t seq);
+    void sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent);
+    void sendSetPositionTargetLocalNED(uint32_t timeBootMs, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
+    void sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
+
+    // mavlink commands
+    void cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec);
+    void cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
+    void cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location
+    void cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude); // mission command to perform landing
+    void cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude);
+    void cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode);
+    void cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent);
+    void cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem);
+    void cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue);
+    void cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent);
+    void cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent);
+    //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent);
+    //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent);
+    void cmdRebootShutdown(uint8_t targetSystem, uint8_t targetComponent, uint8_t autopilot, uint8_t onboardComputer);
+
+    bool getMissionAck() const { return mission_ack; }
+    bool getCommandAck() const { return command_ack; }
+    int getMissionCount() const { return mission_count; }
+    void setMissionAck() { mission_ack = true; }
+    void setCommandAck() { command_ack = true; }
+    void setMissionCount(int count) { mission_count = count; }
+    void resetMissionAck() { mission_ack = false; }
+    void resetCommandAck() { command_ack = false; }
+    void resetMissionCount() { mission_count = 0; }
+
+    bool ShutdownReceived() { return shutdown_flag; }
+
+    uint64_t get_time_usec();
+
+    int waitCommandAck(uint64_t timeout);
+    int waitMissionAck(uint64_t timeout);
+    int waitMissionCount(uint64_t timeout);
+
+private:
+
+    // begin SockUDP
+    int m_socket_in;
+    int m_socket_out;
+    uint16_t m_port;
+    std::string m_addr;
+    struct addrinfo* m_addrinfo;
+    struct sockaddr_in myaddr_in, myaddr_out, client_in;
+    std::thread send_th;
+    std::thread recv_th;
+    void recv_thread(); // receiver thread
+    void send_thread();// sender thread
+    //int timed_recv(char *msg, size_t max_size, int max_wait_ms);
+    bool stop_recv;
+    bool stop_send;
+    //std::queue<std::string> queue_out;
+    //Queue<uint8_t> queue_out;
+    // end SockUDP
+
+    inline void clearBuffer(uint8_t *buffer, int len);
+
+    mavlink_set_position_target_local_ned_t current_setpoint;
+
+    int bytes_sent;
+    // for local_position_ned
+    float position[6] = {};
+
+    // FIXME doesn't work with mavlink 2.0
+    mavlink_message_t msg;
+    u_int16_t msg_len;
+    u_int8_t hds_mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+
+    bool command_ack;
+    bool mission_ack;
+    int mission_count;
+
+    bool shutdown_flag;
+
+    // TODO change to mission item class
+    void debug(std::string debug_msg);
+};
+
+#endif // MAVLINKUDP_H
