Changeset 79 in flair-src for branches/mavlink/tools/Controller/Mavlink

Timestamp:

Sep 21, 2016, 2:51:44 PM (8 years ago)

Author:

Thomas Fuhrmann

Message:

MissionSend is working

Location:

branches/mavlink/tools/Controller/Mavlink/src

Files:

• GuiGcs.cpp (modified) (3 diffs)
• GuiGcs.h (modified) (2 diffs)
• GuiInterface.cpp (modified) (1 diff)

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

@@ -22 +22 @@
 #include <FrameworkManager.h>
 
+#include "MavlinkUDP.h"
+
+#include <ListWidget.h>
+
 //todo remove for tests
 #include <iostream>
@@ -33 +37 @@
                            GuiInterface(parent, name) {
   cout << "MavPlanner GuiGcs constructor" << endl;
-  outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
+  //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
+
+  mavCom = new MavlinkUDP(outputAddress, outputPort);
+  if (mavCom) {
+    // mavCom->startThreads();    
+  } else {
+    //TODO handle the error
+  }
+  
 }
 
 GuiGcs::~GuiGcs() {
-
+  if (mavCom) {
+    mavCom->stopThreads();
+    delete mavCom;
+  }
 }
 
@@ -57 +72 @@
 void GuiGcs::MissionSend() {
   cout << "MavPlanner GuiGcs MissionSend" << endl;
+
+
+  // upload a flight plan
+  cout << "---------------\n";
+  cout << "Flight planning \n";
+  cout << "---------------\n";
+
+  // get the list of commands to send
+  std::vector<std::string> missionCommandList = listMissionItems->GetItemList();
+
+  // reset all the previous waypoints
+  mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
+
+  cout << "[INFO] Mission write partial list.\n";
+  // write partial list, takeoff + 4 waypoints + land
+  mavCom->sendMissionWritePartialList(mavCom->target.getSysID(),mavCom->target.getCompID(), 0, missionCommandList.size());
+  mavCom->waitMissionAck(ACK_TIMEOUT);
+
+  // sending mission items
+  cout << "[INFO] Mission items.\n";
+  std::string delimiter = "|";
+  for (auto &missionCommand : missionCommandList) {
+    cout << "[INFO] Mission item : " << missionCommand << "\n";
+    // Parse command
+    std::string command = missionCommand.substr(0, missionCommand.find(delimiter));
+    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
+    // Parse param1
+    float param1 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
+    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
+    // Parse param2
+    float param2 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
+    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
+    // Parse param3
+    float param3 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
+    // cout << "[INFO] Mission item commands : " << command << ", " << param1 << ", " << param2 << ", " << param3 << std::endl;
+    mavCom->sendMissionItem(mavCom->target.getSysID(), mavCom->target.getCompID(), 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MavCommandGet(command), 0, 1, 0, 0, 0, 0, param1, param2, param3);
+    mavCom->waitMissionAck(ACK_TIMEOUT);
+  }
+
+  cout << "[INFO] Check mission item list.\n";
+  // mission request list (to check)
+  mavCom->sendMissionRequestList(mavCom->target.getSysID(), mavCom->target.getCompID());
+  //mavCom.waitMissionCount(ACK_TIMEOUT);
+
+  cout << "----------------\n";
+  cout << "Flight plan sent \n";
+  cout << "----------------\n";
 }
+
+uint16_t GuiGcs::MavCommandGet(const std::string& command) const {
+  uint16_t returnValue = -1;
+  if (command == "WAYPOINT") {
+    returnValue = MAV_CMD_NAV_WAYPOINT;
+  }
+  if (command == "TAKEOFF") {
+    returnValue = MAV_CMD_NAV_TAKEOFF;
+  }
+  if (command == "LAND") {
+    returnValue = MAV_CMD_NAV_LAND;
+  }
+  if (command == "RETURN") {
+    returnValue = MAV_CMD_NAV_RETURN_TO_LAUNCH;
+  }
+  if (command == "JUMP") {
+    returnValue = MAV_CMD_DO_JUMP;
+  }
+  return returnValue;
+}

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

@@ -23 +23 @@
 #include "GuiInterface.h"
 
+#include "MavlinkUDP.h"
+
 namespace flair {
 namespace core {
@@ -44 +46 @@
   virtual void MissionSend();
 
+  uint16_t MavCommandGet(const std::string& command) const;
+
 private:
   flair::core::Socket *outputSocket;
   std::string outputAddress;
   int outputPort;
+
+  MavlinkUDP* mavCom;
 };
 

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

@@ -120 +120 @@
     // Buttons to control GUI
         if (btnAddMissionCmd->Clicked()) {
-                //TODO
       listMissionItems->AddItem(MissionCmdGet());
     }