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

Timestamp:

Sep 21, 2016, 5:49:17 PM (8 years ago)

Author:

Thomas Fuhrmann

Message:

Cahnge the parsing of parameters

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

@@ -27 +27 @@
 #include <ListWidget.h>
 
-//todo remove for tests
 #include <iostream>
+#include <array>
 
 using namespace std;
 using namespace flair::core;
 using namespace flair::gui;
+
+const std::string delimiter = "|";
 
 GuiGcs::GuiGcs(const FrameworkManager *parent, string name,
@@ -74 +76 @@
 void GuiGcs::MissionStop() {
   cout << "MavPlanner GuiGcs MissionStop" << endl;
-  outputSocket->SendMessage("MavPlanner GuiGcs MissionStop");
 }
 
 void GuiGcs::MissionResume() {
   cout << "MavPlanner GuiGcs MissionResume" << endl;
-  outputSocket->SendMessage("MavPlanner GuiGcs MissionResume");
 }
 
@@ -103 +103 @@
   // sending mission items
   cout << "[INFO] Mission items.\n";
-  std::string delimiter = "|";
   for (auto &missionCommand : missionCommandList) {
     cout << "[INFO] Mission item : " << missionCommand << "\n";
@@ -109 +108 @@
     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)));
-    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
-    // Parse latitude
-    float latitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
-    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
-    // Parse longitude
-    float longitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
-    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
-    // Parse altitude
-    float altitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
-    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
-    // 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, param1, param2, param3, 0, latitude, longitude, altitude);
+    std::array<float, 6> missionParameters;
+    ParametersParse(missionCommand, missionParameters);
+
+    mavCom->sendMissionItem(mavCom->target.getSysID(), mavCom->target.getCompID(), 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MavCommandGet(command), 0, 1,
+            missionParameters[0], missionParameters[1], missionParameters[2], 0, missionParameters[3], missionParameters[4], missionParameters[5]);
     mavCom->waitMissionAck(ACK_TIMEOUT);
   }
@@ -142 +126 @@
 }
 
+void GuiGcs::ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat) {
+  std::string parametersBuffer = parametersString;
+  for (int count = 0; count < parametersFloat.size(); count++) {
+    parametersFloat[count] = std::stof(parametersBuffer.substr(0, parametersBuffer.find(delimiter)));
+    parametersBuffer.erase(0, parametersBuffer.find(delimiter) + delimiter.length());
+  }
+}
+
 uint16_t GuiGcs::MavCommandGet(const std::string& command) const {
   uint16_t returnValue = -1;

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

@@ -21 +21 @@
 
 #include <string>
+#include <array>
 
 #include "GuiInterface.h"
@@ -53 +54 @@
 
 private:
-  flair::core::Socket *outputSocket;
   std::string outputAddress;
   int outputPort;
@@ -60 +60 @@
 
   void Initialize();
+  void ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat);
 };