Changeset 79 in flair-src


Ignore:
Timestamp:
Sep 21, 2016, 2:51:44 PM (4 years ago)
Author:
Thomas Fuhrmann
Message:

MissionSend? is working

Location:
branches/mavlink/tools/Controller/Mavlink/src
Files:
3 edited

Legend:

Unmodified
Added
Removed
  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp

    r77 r79  
    2222#include <FrameworkManager.h>
    2323
     24#include "MavlinkUDP.h"
     25
     26#include <ListWidget.h>
     27
    2428//todo remove for tests
    2529#include <iostream>
     
    3337                           GuiInterface(parent, name) {
    3438  cout << "MavPlanner GuiGcs constructor" << endl;
    35   outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
     39  //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
     40
     41  mavCom = new MavlinkUDP(outputAddress, outputPort);
     42  if (mavCom) {
     43    // mavCom->startThreads();   
     44  } else {
     45    //TODO handle the error
     46  }
     47 
    3648}
    3749
    3850GuiGcs::~GuiGcs() {
    39 
     51  if (mavCom) {
     52    mavCom->stopThreads();
     53    delete mavCom;
     54  }
    4055}
    4156
     
    5772void GuiGcs::MissionSend() {
    5873  cout << "MavPlanner GuiGcs MissionSend" << endl;
     74
     75
     76  // upload a flight plan
     77  cout << "---------------\n";
     78  cout << "Flight planning \n";
     79  cout << "---------------\n";
     80
     81  // get the list of commands to send
     82  std::vector<std::string> missionCommandList = listMissionItems->GetItemList();
     83
     84  // reset all the previous waypoints
     85  mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
     86
     87  cout << "[INFO] Mission write partial list.\n";
     88  // write partial list, takeoff + 4 waypoints + land
     89  mavCom->sendMissionWritePartialList(mavCom->target.getSysID(),mavCom->target.getCompID(), 0, missionCommandList.size());
     90  mavCom->waitMissionAck(ACK_TIMEOUT);
     91
     92  // sending mission items
     93  cout << "[INFO] Mission items.\n";
     94  std::string delimiter = "|";
     95  for (auto &missionCommand : missionCommandList) {
     96    cout << "[INFO] Mission item : " << missionCommand << "\n";
     97    // Parse command
     98    std::string command = missionCommand.substr(0, missionCommand.find(delimiter));
     99    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
     100    // Parse param1
     101    float param1 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
     102    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
     103    // Parse param2
     104    float param2 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
     105    missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
     106    // Parse param3
     107    float param3 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
     108    // cout << "[INFO] Mission item commands : " << command << ", " << param1 << ", " << param2 << ", " << param3 << std::endl;
     109    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);
     110    mavCom->waitMissionAck(ACK_TIMEOUT);
     111  }
     112
     113  cout << "[INFO] Check mission item list.\n";
     114  // mission request list (to check)
     115  mavCom->sendMissionRequestList(mavCom->target.getSysID(), mavCom->target.getCompID());
     116  //mavCom.waitMissionCount(ACK_TIMEOUT);
     117
     118  cout << "----------------\n";
     119  cout << "Flight plan sent \n";
     120  cout << "----------------\n";
    59121}
     122
     123uint16_t GuiGcs::MavCommandGet(const std::string& command) const {
     124  uint16_t returnValue = -1;
     125  if (command == "WAYPOINT") {
     126    returnValue = MAV_CMD_NAV_WAYPOINT;
     127  }
     128  if (command == "TAKEOFF") {
     129    returnValue = MAV_CMD_NAV_TAKEOFF;
     130  }
     131  if (command == "LAND") {
     132    returnValue = MAV_CMD_NAV_LAND;
     133  }
     134  if (command == "RETURN") {
     135    returnValue = MAV_CMD_NAV_RETURN_TO_LAUNCH;
     136  }
     137  if (command == "JUMP") {
     138    returnValue = MAV_CMD_DO_JUMP;
     139  }
     140  return returnValue;
     141}
  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h

    r77 r79  
    2323#include "GuiInterface.h"
    2424
     25#include "MavlinkUDP.h"
     26
    2527namespace flair {
    2628namespace core {
     
    4446  virtual void MissionSend();
    4547
     48  uint16_t MavCommandGet(const std::string& command) const;
     49
    4650private:
    4751  flair::core::Socket *outputSocket;
    4852  std::string outputAddress;
    4953  int outputPort;
     54
     55  MavlinkUDP* mavCom;
    5056};
    5157
  • branches/mavlink/tools/Controller/Mavlink/src/GuiInterface.cpp

    r77 r79  
    120120    // Buttons to control GUI
    121121        if (btnAddMissionCmd->Clicked()) {
    122                 //TODO
    123122      listMissionItems->AddItem(MissionCmdGet());
    124123    }
Note: See TracChangeset for help on using the changeset viewer.