Index: /branches/mavlink/tools/Controller/Mavlink/src/CallbackTimer.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/CallbackTimer.h	(revision 84)
+++ /branches/mavlink/tools/Controller/Mavlink/src/CallbackTimer.h	(revision 84)
@@ -0,0 +1,60 @@
+//  created:    17/12/2015
+//  updated:    25/07/2016
+//  filename:   CallbackTimer.h
+//
+//  author:     Milan Erdelj
+//
+//  version:    $Id: $
+//
+//  purpose:    Callback timer class (executing functions periodically)
+//
+//
+/*********************************************************************/
+
+#ifndef CALLBACKTIMER_H
+#define CALLBACKTIMER_H
+
+#include <functional>
+#include <chrono>
+#include <future>
+#include <cstdio>
+#include <iostream>
+
+class CallbackTimer {
+public:
+    CallbackTimer() :_execute(false) {}
+
+    ~CallbackTimer() {
+        if(_execute.load(std::memory_order_acquire)) {
+            stop();
+        }
+    }
+
+    void stop() {
+        _execute.store(false, std::memory_order_release);
+        if(_thd.joinable()) _thd.join();
+    }
+
+    void start(uint64_t interval, std::function<void(void)> callback) {
+        if(_execute.load(std::memory_order_acquire)) {
+            stop();
+        }
+        _execute.store(true, std::memory_order_release);
+        _thd = std::thread([this, interval, callback]() {
+            while (_execute.load(std::memory_order_acquire)) {
+                callback();
+                std::this_thread::sleep_for(std::chrono::microseconds(interval));
+            }
+        });
+    }
+
+    bool is_running() const noexcept {
+        return ( _execute.load(std::memory_order_acquire) && _thd.joinable() );
+    }
+
+private:
+    std::atomic<bool> _execute;
+    std::thread _thd;
+};
+
+#endif // CALLBACKTIMER_H
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.cpp
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.cpp	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.cpp	(revision 84)
@@ -45,5 +45,5 @@
 }
 
-void GuiFlair::MissionResume() {
+void GuiFlair::MissionPause() {
 	cout << "MavPlanner GuiFlair MissionResume" << endl;
 }
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.h	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiFlair.h	(revision 84)
@@ -40,5 +40,5 @@
   virtual void MissionStart();
   virtual void MissionStop();
-  virtual void MissionResume();
+  virtual void MissionPause();
   virtual void MissionSend();
 };
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp	(revision 84)
@@ -42,4 +42,6 @@
   //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
 
+  missionPauseEnabled = false;
+
   mavCom = new MavlinkUDP(outputAddress, outputPort);
 }
@@ -78,6 +80,17 @@
 }
 
-void GuiGcs::MissionResume() {
-  cout << "MavPlanner GuiGcs MissionResume" << endl;
+void GuiGcs::MissionPause() {
+  cout << "MavPlanner GuiGcs MissionPause" << endl;
+  // send pause/continue depending on the state of the button
+  if(missionPauseEnabled) {
+      missionPauseEnabled = false;
+      cout << "Sent pause command.\n";
+      mavCom->cmdDoPauseContinue(mavCom->target.getSysID(), mavCom->target.getCompID(), 0);
+  } else {
+      missionPauseEnabled = true;
+      cout << "Sent continue command.\n";
+      mavCom->cmdDoPauseContinue(mavCom->target.getSysID(), mavCom->target.getCompID(), 1);
+  }
+  mavCom->waitCommandAck(ACK_TIMEOUT);
 }
 
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h	(revision 84)
@@ -48,5 +48,5 @@
   virtual void MissionStart();
   virtual void MissionStop();
-  virtual void MissionResume();
+  virtual void MissionPause();
   virtual void MissionSend();
 
@@ -59,4 +59,6 @@
   MavlinkUDP* mavCom;
 
+  bool missionPauseEnabled;
+
   void Initialize();
   void ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat);
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp	(revision 84)
@@ -52,5 +52,5 @@
   btnStartMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Start mission");
   btnStopMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Stop mission");
-  btnResumeMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Resume mission");
+  btnPauseMission = new PushButton(controlsGroupBox->LastRowLastCol(), "Pause mission");
 
   // Add cmd group
@@ -132,6 +132,6 @@
   		MissionStop();
     }
-  	if (btnResumeMission->Clicked()) {
-  		MissionResume();
+  	if (btnPauseMission->Clicked()) {
+  		MissionPause();
     }
     if (btnSendMission->Clicked()) {
Index: /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.h	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.h	(revision 84)
@@ -52,5 +52,5 @@
   virtual void MissionStart() = 0;
   virtual void MissionStop() = 0;
-  virtual void MissionResume() = 0;
+  virtual void MissionPause() = 0;
   virtual void MissionSend() = 0;
 
@@ -66,5 +66,5 @@
   flair::gui::PushButton* btnStartMission;
   flair::gui::PushButton* btnStopMission;
-  flair::gui::PushButton* btnResumeMission;
+  flair::gui::PushButton* btnPauseMission;
   
   // Add cmd groupbox
Index: /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.cpp	(revision 84)
@@ -43,4 +43,6 @@
     debug_messages = true;
 
+    heartbeat_period = HEARTBEAT_DEFAULT_PERIOD;
+
     resetCommandAck();
     resetMissionAck();
@@ -50,12 +52,12 @@
     sendingPosition = false;
     sendingSystemStatus = false;
-    sendingAttitudeInterval = -1;
-    sendingPositionInterval = -1;
-    sendingSystemStatusInterval = -1;
+    sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD;
+    sendingPositionInterval = POSITION_DEFAULT_PERIOD;
+    sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD;
 
     // mission items
     missionFirst = 0;
     missionLast = 0;
-    mission_items_loop = false;
+    //mission_items_loop = false;
     missionActive = false; // if the mission is launched
 
@@ -66,4 +68,8 @@
     autopilot_id = 0; // autopilot component id
     component_id = 0; // companion computer component id
+
+    // setpoint
+    recvSetpointX = 0;
+    recvSetpointY = 0;
 
    // mavlink_command_long_t com = { 0 };
@@ -210,5 +216,5 @@
     length_to_send = mavlink_msg_to_send_buffer(buff_out, &message);
     something_to_send = true;
-    printf("[SENT] Heartbeat\n");
+    //printf("[SENT] Heartbeat\n");
 }
 
@@ -219,5 +225,5 @@
     length_to_send = mavlink_msg_to_send_buffer(buff_out,&message);
     something_to_send = true;
-    printf("[SENT] System status\n");
+    //printf("[SENT] System status\n");
 }
 
@@ -358,4 +364,13 @@
     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);
+    printf("[SENT] SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", x, y);
+    something_to_send = true;
+}
+
+// send PositionTargetLocalNED message
+void MavlinkUDP::sendPositionTargetLocalNED(uint32_t time_boot_ms, 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_position_target_local_ned_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, 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;
 }
@@ -365,4 +380,12 @@
     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;
+}
+
+// send PositionTargetGlobalInt message
+void MavlinkUDP::sendPositionTargetGlobalInt(uint32_t time_boot_ms, 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_position_target_global_int_pack(me.getSysID(), me.getCompID(), &message, time_boot_ms, 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;
@@ -374,5 +397,5 @@
 
 // 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) {
+void MavlinkUDP::cmdNavWaypoint(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);
 }
@@ -384,5 +407,5 @@
 
 // command Land
-void MavlinkUDP::cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude) {
+void MavlinkUDP::cmdNavLand(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);
 }
@@ -394,5 +417,5 @@
 
 // command TakeOff
-void MavlinkUDP::cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude) {
+void MavlinkUDP::cmdNavTakeoff(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);
 }
@@ -403,4 +426,9 @@
 }
 
+// command DoPauseContinue
+void MavlinkUDP::cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_DO_PAUSE_CONTINUE, 0, pauseContinue, 0, 0, 0, 0, 0, 0);
+}
+
 // command DoSetHome
 void MavlinkUDP::cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent) {
@@ -408,4 +436,9 @@
 }
 
+// command GetHomePosition
+void MavlinkUDP::cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent) {
+    sendCommandLong(targetSystem, targetComponent, MAV_CMD_GET_HOME_POSITION, 0, 0, 0, 0, 0, 0, 0, 0);
+}
+
 // command MissionStart
 void MavlinkUDP::cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem) {
@@ -424,5 +457,5 @@
 
 // command ReturnToLaunch
-void MavlinkUDP::cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
+void MavlinkUDP::cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent) {
     sendCommandLong(targetSystem, targetComponent, MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0);
 }
@@ -432,4 +465,33 @@
     // 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);
+}
+
+
+// callback Heartbeat
+void MavlinkUDP::startHeartbeat(uint64_t period) {
+    timerHeartbeat.start(period, std::bind(&MavlinkUDP::callbackHeartbeat,this));
+}
+void MavlinkUDP::stopHeartbeat() {
+    timerHeartbeat.stop();
+}
+void MavlinkUDP::callbackHeartbeat() {
+    sendHeartbeat();
+}
+
+// callback SystemTime
+void MavlinkUDP::startSystemTime(uint64_t period) {
+    timerSystemTime.start(period, std::bind(&MavlinkUDP::callbackSystemTime,this));
+}
+void MavlinkUDP::stopSystemTime() {
+    timerSystemTime.stop();
+}
+void MavlinkUDP::callbackSystemTime() {
+    sendSystemTime();
+}
+
+// stop the callback function timers
+void MavlinkUDP::stopAllCallbackTimers() {
+    stopHeartbeat();
+    stopSystemTime();
 }
 
@@ -500,7 +562,8 @@
             // clear the queue
             clearMissionPlan();
-            //
-            // TODO signal the app that the vector is cleared
-            //
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            missionCommands.push(mavItem);
+            //printf("[DEBUG] sysid = %d compid = %d\n", message.sysid, message.compid);
             sendMissionAck(message.sysid, message.compid, MAV_MISSION_ACCEPTED);
             break;
@@ -592,43 +655,67 @@
             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);
+                MavlinkItem mavItem;
+                mavItem.id = command;
+                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
+                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
+                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
+                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
+                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
+                // adding commands to the mission plan
+                missionPlan.push(mavItem);
+                //missionCommands.push(mavItem);
+                printf("[RECV] MISSION_ITEM: TAKEOFF\n");
                 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);
+                MavlinkItem mavItem;
+                mavItem.id = command;
+                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
+                mavItem.abort_altitude = mavlink_msg_mission_item_get_param1(&message);
+                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
+                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
+                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
+                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
+                // adding commands to the mission plan
+                missionPlan.push(mavItem);
+                printf("[RECV] MISSION_ITEM: LAND\n");
+                break;
+            }
+            case MAV_CMD_DO_JUMP:
+            {
+                MavlinkItem mavItem;
+                mavItem.id = command;
+                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
+                mavItem.jump_sequence = mavlink_msg_mission_item_get_param1(&message);
+                mavItem.jump_repeat_count = mavlink_msg_mission_item_get_param2(&message);
+                // adding commands to the mission plan
+                missionPlan.push(mavItem);
+                printf("[RECV] MISSION_ITEM: MAV_CMD_DO_JUMP seq %d repeat %d\n", mavItem.jump_sequence, mavItem.jump_repeat_count);
+                break;
+            }
+            case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+            {
+                MavlinkItem mavItem;
+                mavItem.id = command;
+                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
+                // adding commands to the mission plan
+                missionPlan.push(mavItem);
+                printf("[RECV] MISSION_ITEM: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
                 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);
+                MavlinkItem mavItem;
+                mavItem.id = command;
+                mavItem.sequence = mavlink_msg_mission_item_get_seq(&message);
+                mavItem.hold_time = mavlink_msg_mission_item_get_param1(&message);
+                mavItem.yaw_angle = mavlink_msg_mission_item_get_param4(&message);
+                mavItem.latitude = mavlink_msg_mission_item_get_x(&message);
+                mavItem.longitude = mavlink_msg_mission_item_get_y(&message);
+                mavItem.altitude = mavlink_msg_mission_item_get_z(&message);
+                // adding commands to the mission plan
+                missionPlan.push(mavItem);
+                printf("[RECV] MISSION_ITEM: WAYPOINT seq = %d, lat = %.2f, long = %.2f, alt = %.2f\n", mavItem.sequence, mavItem.latitude, mavItem.longitude, mavItem.altitude);
                 break;
             }
@@ -719,8 +806,21 @@
         case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
         {
-            printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");
+            printf("[RECV] 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;
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_local_position_ned_get_time_boot_ms(&message);
+            mavItem.x = mavlink_msg_local_position_ned_get_x(&message);
+            mavItem.y = mavlink_msg_local_position_ned_get_y(&message);
+            mavItem.z = mavlink_msg_local_position_ned_get_z(&message);
+            mavItem.vx = mavlink_msg_local_position_ned_get_vx(&message);
+            mavItem.vy = mavlink_msg_local_position_ned_get_vy(&message);
+            mavItem.vz = mavlink_msg_local_position_ned_get_vz(&message);
+            missionCommands.push(mavItem);
+            recvSetpointX = mavItem.x;
+            recvSetpointY = mavItem.y;
             break;
         }
@@ -728,9 +828,50 @@
         case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
         {
-            printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");
+            printf("[RECV] 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);
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_global_position_int_get_time_boot_ms(&message);
+            mavItem.latitude = (float)mavlink_msg_global_position_int_get_lat(&message);
+            mavItem.longitude = (float)mavlink_msg_global_position_int_get_lon(&message);
+            mavItem.altitude = (float)mavlink_msg_global_position_int_get_alt(&message);
+            mavItem.relative_alt = (float)mavlink_msg_global_position_int_get_relative_alt(&message);
+            mavItem.vx = (float)mavlink_msg_global_position_int_get_vx(&message);
+            mavItem.vy = (float)mavlink_msg_global_position_int_get_vy(&message);
+            mavItem.vz = (float)mavlink_msg_global_position_int_get_vz(&message);
+            mavItem.yaw_angle = (float)mavlink_msg_global_position_int_get_hdg(&message); // heading
+            missionCommands.push(mavItem);
+            break;
+        }
+
+        case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+        {
+            mavlink_msg_set_position_target_local_ned_decode(&message, &(current_messages.set_position_target_local_ned));
+            current_messages.time_stamps.set_position_target_local_ned = get_time_usec();
+            this_timestamps.set_position_target_local_ned = current_messages.time_stamps.set_position_target_local_ned;
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_set_position_target_local_ned_get_time_boot_ms(&message);
+            mavItem.target_system = mavlink_msg_set_position_target_local_ned_get_target_system(&message);
+            mavItem.target_component = mavlink_msg_set_position_target_local_ned_get_target_component(&message);
+            mavItem.coordinate_frame = mavlink_msg_set_position_target_local_ned_get_coordinate_frame(&message);
+            mavItem.type_mask = mavlink_msg_set_position_target_local_ned_get_type_mask(&message);
+            mavItem.x = mavlink_msg_set_position_target_local_ned_get_x(&message);
+            mavItem.y = mavlink_msg_set_position_target_local_ned_get_y(&message);
+            mavItem.z = mavlink_msg_set_position_target_local_ned_get_z(&message);
+            mavItem.vx = mavlink_msg_set_position_target_local_ned_get_vx(&message);
+            mavItem.vy = mavlink_msg_set_position_target_local_ned_get_vy(&message);
+            mavItem.vz = mavlink_msg_set_position_target_local_ned_get_vz(&message);
+            mavItem.afx = mavlink_msg_set_position_target_local_ned_get_afx(&message);
+            mavItem.afy = mavlink_msg_set_position_target_local_ned_get_afy(&message);
+            mavItem.afz = mavlink_msg_set_position_target_local_ned_get_afz(&message);
+            mavItem.yaw = mavlink_msg_set_position_target_local_ned_get_yaw(&message);
+            mavItem.yaw_rate = mavlink_msg_set_position_target_local_ned_get_yaw_rate(&message);
+            missionCommands.push(mavItem);
+            printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED x = %.2f y = %.2f\n", mavItem.x, mavItem.y);
             break;
         }
@@ -738,8 +879,55 @@
         case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
         {
-            printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");
+            printf("[RECV] 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;
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_position_target_local_ned_get_time_boot_ms(&message);
+            mavItem.coordinate_frame = mavlink_msg_position_target_local_ned_get_coordinate_frame(&message);
+            mavItem.type_mask = mavlink_msg_position_target_local_ned_get_type_mask(&message);
+            mavItem.x = mavlink_msg_position_target_local_ned_get_x(&message);
+            mavItem.y = mavlink_msg_position_target_local_ned_get_y(&message);
+            mavItem.z = mavlink_msg_position_target_local_ned_get_z(&message);
+            mavItem.vx = mavlink_msg_position_target_local_ned_get_vx(&message);
+            mavItem.vy = mavlink_msg_position_target_local_ned_get_vy(&message);
+            mavItem.vz = mavlink_msg_position_target_local_ned_get_vz(&message);
+            mavItem.afx = mavlink_msg_position_target_local_ned_get_afx(&message);
+            mavItem.afy = mavlink_msg_position_target_local_ned_get_afy(&message);
+            mavItem.afz = mavlink_msg_position_target_local_ned_get_afz(&message);
+            mavItem.yaw = mavlink_msg_position_target_local_ned_get_yaw(&message);
+            mavItem.yaw_rate = mavlink_msg_position_target_local_ned_get_yaw_rate(&message);
+            missionCommands.push(mavItem);
+            break;
+        }
+
+        case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
+        {
+            printf("[RECV] MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT\n");
+            mavlink_msg_set_position_target_global_int_decode(&message, &(current_messages.set_position_target_global_int));
+            current_messages.time_stamps.set_position_target_global_int = get_time_usec();
+            this_timestamps.set_position_target_global_int = current_messages.time_stamps.set_position_target_global_int;
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_set_position_target_global_int_get_time_boot_ms(&message);
+            mavItem.target_system = mavlink_msg_set_position_target_global_int_get_target_system(&message);
+            mavItem.target_component = mavlink_msg_set_position_target_global_int_get_target_component(&message);
+            mavItem.coordinate_frame = mavlink_msg_set_position_target_global_int_get_coordinate_frame(&message);
+            mavItem.type_mask = mavlink_msg_set_position_target_global_int_get_type_mask(&message);
+            mavItem.lat_int = mavlink_msg_set_position_target_global_int_get_lat_int(&message);
+            mavItem.lon_int = mavlink_msg_set_position_target_global_int_get_lon_int(&message);
+            mavItem.altitude = mavlink_msg_set_position_target_global_int_get_alt(&message);
+            mavItem.vx = mavlink_msg_set_position_target_global_int_get_vx(&message);
+            mavItem.vy = mavlink_msg_set_position_target_global_int_get_vy(&message);
+            mavItem.vz = mavlink_msg_set_position_target_global_int_get_vz(&message);
+            mavItem.afx = mavlink_msg_set_position_target_global_int_get_afx(&message);
+            mavItem.afy = mavlink_msg_set_position_target_global_int_get_afy(&message);
+            mavItem.afz = mavlink_msg_set_position_target_global_int_get_afz(&message);
+            mavItem.yaw = mavlink_msg_set_position_target_global_int_get_yaw(&message);
+            mavItem.yaw_rate = mavlink_msg_set_position_target_global_int_get_yaw_rate(&message);
+            missionCommands.push(mavItem);
             break;
         }
@@ -747,8 +935,26 @@
         case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
         {
-            printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");
+            printf("[RECV] 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;
+            // insert it into the command queue
+            MavlinkItem mavItem;
+            mavItem.id = message.msgid;
+            mavItem.time_boot_ms = mavlink_msg_position_target_global_int_get_time_boot_ms(&message);
+            mavItem.coordinate_frame = mavlink_msg_position_target_global_int_get_coordinate_frame(&message);
+            mavItem.type_mask = mavlink_msg_position_target_global_int_get_type_mask(&message);
+            mavItem.lat_int = mavlink_msg_position_target_global_int_get_lat_int(&message);
+            mavItem.lon_int = mavlink_msg_position_target_global_int_get_lon_int(&message);
+            mavItem.altitude = mavlink_msg_position_target_global_int_get_alt(&message);
+            mavItem.vx = mavlink_msg_position_target_global_int_get_vx(&message);
+            mavItem.vy = mavlink_msg_position_target_global_int_get_vy(&message);
+            mavItem.vz = mavlink_msg_position_target_global_int_get_vz(&message);
+            mavItem.afx = mavlink_msg_position_target_global_int_get_afx(&message);
+            mavItem.afy = mavlink_msg_position_target_global_int_get_afy(&message);
+            mavItem.afz = mavlink_msg_position_target_global_int_get_afz(&message);
+            mavItem.yaw = mavlink_msg_position_target_global_int_get_yaw(&message);
+            mavItem.yaw_rate = mavlink_msg_position_target_global_int_get_yaw_rate(&message);
+            missionCommands.push(mavItem);
             break;
         }
@@ -756,5 +962,5 @@
         case MAVLINK_MSG_ID_HIGHRES_IMU:
         {
-            //printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");
+            //printf("[RECV] 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();
@@ -781,164 +987,184 @@
             printf("Received command %d\n", current_messages.command_long.command);
             switch(current_messages.command_long.command) {
-            case MAV_CMD_DO_SET_PARAMETER:
+            case MAV_CMD_SET_MESSAGE_INTERVAL:
+            {
+                int64_t desired_interval = (int64_t)current_messages.command_long.param2;
+                printf("[RECV] COMMAND_LONG: MAV_CMD_SET_MESSAGE_INTERVAL %d %ld\n", (int)current_messages.command_long.param1, (int64_t)current_messages.command_long.param2);
+                // adding to the command queue
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.callback_message = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                (desired_interval == -1)?(mavItem.callback_flag = false):(mavItem.callback_flag = true);
                 switch((int)current_messages.command_long.param1) {
-                case MAV_LOOP_MISSION_ITEMS:
-                    if((int)current_messages.command_long.param2) {
-                        itemsLoop();
+                case MAVLINK_MSG_ID_HEARTBEAT:
+                {
+                    if(current_messages.command_long.param2 == -1) {
+                        stopHeartbeat();
+                    } else if(current_messages.command_long.param2 == 0) {
+                        startHeartbeat(HEARTBEAT_DEFAULT_PERIOD);
                     } else {
-                        itemsNoLoop();
+                        startHeartbeat(desired_interval);
                     }
-
                     break;
+                }
+                case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
+                {
+                    if(current_messages.command_long.param2 == -1) {
+                        sendingPosition = false;
+                    } else if(current_messages.command_long.param2 == 0) {
+                        sendingPositionInterval = POSITION_DEFAULT_PERIOD;
+                        sendingPosition = true;
+                    } else {
+                        sendingPositionInterval = desired_interval;
+                        sendingPosition = true;
+                    }
+                    mavItem.callback_period = sendingPositionInterval;
+                    break;
+                }
+                case MAVLINK_MSG_ID_ATTITUDE:
+                {
+                    if(current_messages.command_long.param2 == -1) {
+                        sendingAttitude = false;
+                    } else if(current_messages.command_long.param2 == 0) {
+                        sendingAttitudeInterval = ATTITUDE_DEFAULT_PERIOD;
+                        sendingAttitude = true;
+                    } else {
+                        sendingAttitudeInterval = desired_interval;
+                        sendingAttitude = true;
+                    }
+                    mavItem.callback_period = sendingAttitudeInterval;
+                    break;
+                }
+                case MAVLINK_MSG_ID_SYS_STATUS:
+                {
+                    if(current_messages.command_long.param2 == -1) {
+                        sendingSystemStatus = false;
+                    } else if(current_messages.command_long.param2 == 0) {
+                        sendingSystemStatusInterval = STATUS_DEFAULT_PERIOD;
+                        sendingSystemStatus = true;
+                    } else {
+                        sendingSystemStatusInterval = desired_interval;
+                        sendingSystemStatus = true;
+                    }
+                    mavItem.callback_period = sendingSystemStatusInterval;
+                    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;
+                // finally add the command to the queue
+                missionCommands.push(mavItem);
+                break;
+            }
+                // * Commands queue:
+                // * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
+                // * 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)
+
             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);
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.first_item = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                mavItem.last_item = (uint16_t)mavlink_msg_command_long_get_param2(&message);
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_MISSION_START firstItem = %d, lastItem = %d\n", mavItem.first_item, mavItem.last_item);
                 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);
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.use_current = (uint8_t)mavlink_msg_command_long_get_param1(&message);
+                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
+                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
+                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_HOME use_current = %d, lat = %.2f, long = %.2f, altitude = %.2f\n", mavItem.use_current, mavItem.latitude, mavItem.longitude, mavItem.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");
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                missionCommands.push(mavItem);
+                // TODO respond with a home position
+                printf("[RECV] COMMAND_LONG: MAV_CMD_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;
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.mode = mavlink_msg_command_long_get_param1(&message);
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_SET_MODE %d\n", mavItem.mode);
+                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;
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
+                break;
+            }
             case MAV_CMD_NAV_TAKEOFF:
-                // TODO takeoff
-                printf("[TODO] Execute takeoff.\n");
-                break;
+            {
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.min_pitch = mavlink_msg_command_long_get_param1(&message);
+                mavItem.yaw_angle = mavlink_msg_command_long_get_param4(&message);
+                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
+                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
+                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
+                // adding commands to the command queue
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_TAKEOFF\n");
+                break;
+            }
             case MAV_CMD_NAV_LAND:
-                // TODO emergency landing
-                printf("[TODO] Execute emergency landing.\n");
-                break;
+            {
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.abort_altitude = mavlink_msg_command_long_get_param1(&message);
+                mavItem.desired_yaw = mavlink_msg_command_long_get_param4(&message);
+                mavItem.latitude = mavlink_msg_command_long_get_param5(&message);
+                mavItem.longitude = mavlink_msg_command_long_get_param6(&message);
+                mavItem.altitude = mavlink_msg_command_long_get_param7(&message);
+                // adding commands to the command queue
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_NAV_LAND\n");
+                break;
+            }
+            case MAV_CMD_DO_PAUSE_CONTINUE:
+            {
+                MavlinkItem mavItem;
+                mavItem.id = mavlink_msg_command_long_get_command(&message);
+                mavItem.pause_continue = (uint16_t)mavlink_msg_command_long_get_param1(&message);
+                missionCommands.push(mavItem);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_DO_PAUSE_CONTINUE pauseContinue = %d\n", mavItem.pause_continue);
+            }
             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);
+                printf("[RECV] COMMAND_LONG: MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN\n");
+                break;
+            }
+            case MAV_CMD_DO_REPOSITION:
+            {
+                // TODO reposition
+                printf("[TODO] DO_REPOSITION\n");
+                break;
+            }
+            case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
+            {
+                // TODO request autopilot capabilities
+                break;
             }
             default:
Index: /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h
===================================================================
--- /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h	(revision 83)
+++ /branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h	(revision 84)
@@ -30,4 +30,5 @@
 #include <sys/time.h>
 #include "include/common/mavlink.h"
+#include "CallbackTimer.h"
 
 // TODO these constants should be read from a file
@@ -48,15 +49,19 @@
 
 #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
+#define HEARTBEAT_DEFAULT_PERIOD 1000000
+#define POSITION_DEFAULT_PERIOD 1000000
+#define ATTITUDE_DEFAULT_PERIOD 1000000
+#define STATUS_DEFAULT_PERIOD 1000000
+
+//#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
@@ -94,4 +99,6 @@
     uint64_t local_position_ned;
     uint64_t global_position_int;
+    uint64_t set_position_target_local_ned;
+    uint64_t set_position_target_global_int;
     uint64_t position_target_local_ned;
     uint64_t position_target_global_int;
@@ -118,4 +125,6 @@
         local_position_ned = 0;
         global_position_int = 0;
+        set_position_target_local_ned = 0;
+        set_position_target_global_int = 0;
         position_target_local_ned = 0;
         position_target_global_int = 0;
@@ -131,53 +140,29 @@
     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_set_position_target_local_ned_t set_position_target_local_ned;
+    mavlink_set_position_target_global_int_t set_position_target_global_int;
     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() {
@@ -193,28 +178,6 @@
 // * 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)
@@ -223,24 +186,56 @@
 // * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
 
-struct MissionCommand {
-    uint16_t commandId;
-    // DO_JUMP
+struct MavlinkItem {
+    // TODO add timestamp
+    //
+    uint16_t id;
+    uint8_t target_system;
+    uint8_t target_component;
     uint16_t sequence;
-    uint16_t repeatCount;
-    // DO_SET_HOME
-    uint8_t useCurrent;
-    float latitude, longitude, altitude;
+    uint16_t jump_sequence;
+    uint16_t jump_repeat_count;
+    uint8_t use_current;
+    uint64_t hold_time;
+    float radius;
+    float latitude, longitude, altitude, relative_alt;
     float x, y, z;
-    // DO_PAUSE_CONTINUE
-    uint8_t pauseContinue;
-    // MISSION_START
-    uint16_t firstItem, lastItem;
-    // DO_SET_MODE
+    float vx, vy, vz;
+    float afx, afy, afz;
+    float yaw, yaw_rate;
+    float abort_altitude;
+    float desired_yaw;
+    float yaw_angle;
+    float min_pitch;
+    uint8_t pause_continue;
+    uint16_t first_item, last_item;
     uint8_t mode;
-    MissionCommand() {
-        // reset all the attributes
-        pauseContinue = useCurrent = 0;
-        commandId = sequence = repeatCount = 0;
-        latitude = longitude = altitude = x = y = z = 0;
+    uint32_t time_boot_ms;
+    uint8_t coordinate_frame;
+    uint16_t type_mask;
+    int32_t lat_int, lon_int;
+    bool callback_flag;
+    uint64_t callback_period;
+    uint16_t callback_message;
+    MavlinkItem() {
+        target_system = target_component = 0;
+        first_item = last_item = 0;
+        mode = 0;
+        pause_continue = use_current = 0;
+        hold_time = 0;
+        id = sequence = 0;
+        jump_sequence = jump_repeat_count = 0;
+        radius = yaw_angle = latitude = longitude = altitude = relative_alt = x = y = z = 0;
+        desired_yaw = abort_altitude = 0;
+        min_pitch = 0;
+        vx = vy = vz = 0;
+        afx = afy = afz = 0;
+        yaw = yaw_rate = 0;
+        time_boot_ms = 0;
+        coordinate_frame = 0;
+        type_mask = 0;
+        lat_int = lon_int = 0;
+        callback_flag = false;
+        callback_message = 0;
+        callback_period = HEARTBEAT_DEFAULT_PERIOD;
     }
 };
@@ -282,5 +277,4 @@
     ~MavlinkUDP();
 
-    // begin SockUDP
     // threads start and stop
     void startThreads();
@@ -297,6 +291,4 @@
     socklen_t fromlen;
     void clearData();
-    // end SockUDP
-
 
     char control_status;
@@ -328,31 +320,33 @@
     bool debug_messages;
 
+    // received setpoint values
+    float recvSetpointX;
+    float recvSetpointY;
+
     bool sendingPosition;
-    uint16_t sendingPositionInterval;
+    uint64_t sendingPositionInterval;
     bool sendingAttitude;
-    uint16_t sendingAttitudeInterval;
+    uint64_t sendingAttitudeInterval;
     bool sendingSystemStatus;
-    uint16_t sendingSystemStatusInterval;
+    uint64_t sendingSystemStatusInterval;
     bool sendingHeartbeat;
-    uint16_t sendingHeartbeatInterval;
+    uint64_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); }
+    std::queue<MavlinkItem> missionPlan;
+    const std::queue<MavlinkItem>& getMissionPlan() const { return missionPlan; }
+    void clearMissionPlan() { std::queue<MavlinkItem> 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; }
+    // queue of received commands
+    std::queue<MavlinkItem> missionCommands;
+    const std::queue<MavlinkItem>& getMissionCommands() const { return missionCommands; }
+    void clearMissionCommands() { std::queue<MavlinkItem> 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;
@@ -392,18 +386,22 @@
     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 sendPositionTargetLocalNED(uint32_t timeBootMs, 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);
+    void sendPositionTargetGlobalInt(uint32_t time_boot_ms, 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 cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
+    void cmdNavLand(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 cmdNavTakeoff(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 cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent);
     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 cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent);
+    void cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue);
     //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent);
     //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent);
@@ -430,5 +428,4 @@
 private:
 
-    // begin SockUDP
     int m_socket_in;
     int m_socket_out;
@@ -444,7 +441,4 @@
     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);
@@ -467,5 +461,19 @@
     bool shutdown_flag;
 
-    // TODO change to mission item class
+    int64_t heartbeat_period;
+    // callback timers
+    CallbackTimer timerHeartbeat;
+    void startHeartbeat(uint64_t period);
+    void stopHeartbeat();
+    void callbackHeartbeat();
+
+    CallbackTimer timerSystemTime;
+    void startSystemTime(uint64_t period);
+    void stopSystemTime();
+    void callbackSystemTime();
+
+    void stopAllCallbackTimers();
+
+    // debug
     void debug(std::string debug_msg);
 };
