Changeset 82 in flair-src for branches/mavlink

Timestamp:

Sep 21, 2016, 3:16:16 PM (8 years ago)

Author:

Thomas Fuhrmann

Message:

Add MissionStart and Initialize functions

Location:

branches/mavlink/tools/Controller/Mavlink/src

Files:

• GuiGcs.cpp (modified) (5 diffs)
• GuiGcs.h (modified) (3 diffs)

branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp

@@ -7 +7 @@
 //
 //  authors:    Thomas Fuhrmann
+//              Milan Erdelj
 //              Copyright Heudiasyc UMR UTC/CNRS 7253
 //
@@ -40 +41 @@
 
   mavCom = new MavlinkUDP(outputAddress, outputPort);
-  if (mavCom) {
-    // mavCom->startThreads();    
-  } else {
-    //TODO handle the error
-  }
-  
 }
 
@@ -57 +52 @@
 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 +84 @@
 void GuiGcs::MissionSend() {
   cout << "MavPlanner GuiGcs MissionSend" << endl;
-
 
   // upload a flight plan
@@ -150 +161 @@
   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";
+}

branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h

@@ -7 +7 @@
 //
 //  authors:    Thomas Fuhrmann
+//              Milan Erdelj
 //              Copyright Heudiasyc UMR UTC/CNRS 7253
 //
@@ -24 +25 @@
 
 #include "MavlinkUDP.h"
+
+#define INTER_COMMAND_PAUSE 10000 // us
+#define PERIOD_SECOND 1000000 // us
 
 namespace flair {
@@ -54 +58 @@
 
   MavlinkUDP* mavCom;
+
+  void Initialize();
 };