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