Index: branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp
===================================================================
--- branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp	(revision 81)
+++ branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp	(revision 82)
@@ -7,4 +7,5 @@
 //
 //  authors:    Thomas Fuhrmann
+//              Milan Erdelj
 //              Copyright Heudiasyc UMR UTC/CNRS 7253
 //
@@ -40,10 +41,4 @@
 
   mavCom = new MavlinkUDP(outputAddress, outputPort);
-  if (mavCom) {
-    // mavCom->startThreads();    
-  } else {
-    //TODO handle the error
-  }
-  
 }
 
@@ -57,5 +52,22 @@
 void GuiGcs::MissionStart() {
   cout << "MavPlanner GuiGcs MissionStart" << endl;
-  outputSocket->SendMessage("MavPlanner GuiGcs MissionStart");
+
+  Initialize();
+
+    cout << "--------------------\n";
+    cout << "   MISSION START\n";
+    cout << "--------------------\n";
+
+    // set auto armed mode
+    mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_AUTO_ARMED);
+    mavCom->waitCommandAck(ACK_TIMEOUT);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // mission start message
+    mavCom->cmdMissionStart(mavCom->target.getSysID(), mavCom->target.getCompID(), mavCom->missionFirst, mavCom->missionLast);
+    mavCom->waitCommandAck(ACK_TIMEOUT);
+
+    mavCom->missionStarted();
 }
 
@@ -72,5 +84,4 @@
 void GuiGcs::MissionSend() {
   cout << "MavPlanner GuiGcs MissionSend" << endl;
-
 
   // upload a flight plan
@@ -150,2 +161,51 @@
   return returnValue;
 }
+
+void GuiGcs::Initialize() {
+    cout << "--------------------\n";
+    cout << "Initialization START\n";
+    cout << "--------------------\n";
+
+    // UAV start heartbeat, message ID=0, interval 1000 ms
+    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_HEARTBEAT, PERIOD_SECOND);
+    mavCom->waitCommandAck(ACK_TIMEOUT);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // ask for the autopilot capabilities
+    mavCom->cmdRequestAutopilotCapabilities(mavCom->target.getSysID(), mavCom->target.getCompID());
+    // TODO wait for autopilot version
+    //
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // initiate system status callback on the UAV, message ID=1, interval 1000 ms
+    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYS_STATUS, PERIOD_SECOND);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // initiate system time callback on the UAV
+    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYSTEM_TIME, PERIOD_SECOND);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // set preflight mode
+    mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_PREFLIGHT);
+    mavCom->waitCommandAck(ACK_TIMEOUT);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // set home position
+    mavCom->cmdDoSetHome(mavCom->target.getSysID(), mavCom->target.getCompID(), 0);
+    mavCom->waitCommandAck(ACK_TIMEOUT);
+
+    usleep(INTER_COMMAND_PAUSE);
+
+    // clear all mission waypoints
+    mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
+    usleep(INTER_COMMAND_PAUSE);
+    mavCom->waitMissionAck(ACK_TIMEOUT);
+    cout << "--------------------\n";
+    cout << "Initialization END\n";
+    cout << "--------------------\n";
+}
Index: branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h
===================================================================
--- branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h	(revision 81)
+++ branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h	(revision 82)
@@ -7,4 +7,5 @@
 //
 //  authors:    Thomas Fuhrmann
+//              Milan Erdelj
 //              Copyright Heudiasyc UMR UTC/CNRS 7253
 //
@@ -24,4 +25,7 @@
 
 #include "MavlinkUDP.h"
+
+#define INTER_COMMAND_PAUSE 10000 // us
+#define PERIOD_SECOND 1000000 // us
 
 namespace flair {
@@ -54,4 +58,6 @@
 
   MavlinkUDP* mavCom;
+
+  void Initialize();
 };
 
