1 | // %flair:license{
|
---|
2 | // This file is part of the Flair framework distributed under the
|
---|
3 | // CECILL-C License, Version 1.0.
|
---|
4 | // %flair:license}
|
---|
5 | // created: 2016/09/02
|
---|
6 | // filename: GuiGcs.h
|
---|
7 | //
|
---|
8 | // authors: Thomas Fuhrmann
|
---|
9 | // Milan Erdelj
|
---|
10 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
11 | //
|
---|
12 | // version: $Id: $
|
---|
13 | //
|
---|
14 | // purpose: Implementation of GuiInterface, using sockets
|
---|
15 | //
|
---|
16 | //
|
---|
17 | /*********************************************************************/
|
---|
18 |
|
---|
19 | #include "GuiGcs.h"
|
---|
20 | #include "GuiInterface.h"
|
---|
21 |
|
---|
22 | #include <Socket.h>
|
---|
23 | #include <FrameworkManager.h>
|
---|
24 |
|
---|
25 | #include "MavlinkUDP.h"
|
---|
26 |
|
---|
27 | #include <ListWidget.h>
|
---|
28 |
|
---|
29 | #include <iostream>
|
---|
30 | #include <array>
|
---|
31 |
|
---|
32 | using namespace std;
|
---|
33 | using namespace flair::core;
|
---|
34 | using namespace flair::gui;
|
---|
35 |
|
---|
36 | const std::string delimiter = "|";
|
---|
37 |
|
---|
38 | GuiGcs::GuiGcs(const FrameworkManager *parent, string name,
|
---|
39 | std::string &outputAddress, int outputPort):
|
---|
40 | GuiInterface(parent, name) {
|
---|
41 | cout << "MavPlanner GuiGcs constructor" << endl;
|
---|
42 | //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
|
---|
43 |
|
---|
44 | mavCom = new MavlinkUDP(outputAddress, outputPort);
|
---|
45 | }
|
---|
46 |
|
---|
47 | GuiGcs::~GuiGcs() {
|
---|
48 | if (mavCom) {
|
---|
49 | mavCom->stopThreads();
|
---|
50 | delete mavCom;
|
---|
51 | }
|
---|
52 | }
|
---|
53 |
|
---|
54 | void GuiGcs::MissionStart() {
|
---|
55 | cout << "MavPlanner GuiGcs MissionStart" << endl;
|
---|
56 |
|
---|
57 | Initialize();
|
---|
58 |
|
---|
59 | cout << "--------------------\n";
|
---|
60 | cout << " MISSION START\n";
|
---|
61 | cout << "--------------------\n";
|
---|
62 |
|
---|
63 | // set auto armed mode
|
---|
64 | mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_AUTO_ARMED);
|
---|
65 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
66 |
|
---|
67 | usleep(INTER_COMMAND_PAUSE);
|
---|
68 |
|
---|
69 | // mission start message
|
---|
70 | mavCom->cmdMissionStart(mavCom->target.getSysID(), mavCom->target.getCompID(), mavCom->missionFirst, mavCom->missionLast);
|
---|
71 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
72 |
|
---|
73 | mavCom->missionStarted();
|
---|
74 | }
|
---|
75 |
|
---|
76 | void GuiGcs::MissionStop() {
|
---|
77 | cout << "MavPlanner GuiGcs MissionStop" << endl;
|
---|
78 | }
|
---|
79 |
|
---|
80 | void GuiGcs::MissionResume() {
|
---|
81 | cout << "MavPlanner GuiGcs MissionResume" << endl;
|
---|
82 | }
|
---|
83 |
|
---|
84 | void GuiGcs::MissionSend() {
|
---|
85 | cout << "MavPlanner GuiGcs MissionSend" << endl;
|
---|
86 |
|
---|
87 | // upload a flight plan
|
---|
88 | cout << "---------------\n";
|
---|
89 | cout << "Flight planning \n";
|
---|
90 | cout << "---------------\n";
|
---|
91 |
|
---|
92 | // get the list of commands to send
|
---|
93 | std::vector<std::string> missionCommandList = listMissionItems->GetItemList();
|
---|
94 |
|
---|
95 | // reset all the previous waypoints
|
---|
96 | mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
97 |
|
---|
98 | cout << "[INFO] Mission write partial list.\n";
|
---|
99 | // write partial list, takeoff + 4 waypoints + land
|
---|
100 | mavCom->sendMissionWritePartialList(mavCom->target.getSysID(),mavCom->target.getCompID(), 0, missionCommandList.size());
|
---|
101 | mavCom->waitMissionAck(ACK_TIMEOUT);
|
---|
102 |
|
---|
103 | // sending mission items
|
---|
104 | cout << "[INFO] Mission items.\n";
|
---|
105 | for (auto &missionCommand : missionCommandList) {
|
---|
106 | cout << "[INFO] Mission item : " << missionCommand << "\n";
|
---|
107 | // Parse command
|
---|
108 | std::string command = missionCommand.substr(0, missionCommand.find(delimiter));
|
---|
109 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
110 | std::array<float, 6> missionParameters;
|
---|
111 | ParametersParse(missionCommand, missionParameters);
|
---|
112 |
|
---|
113 | mavCom->sendMissionItem(mavCom->target.getSysID(), mavCom->target.getCompID(), 0, MAV_FRAME_GLOBAL_RELATIVE_ALT, MavCommandGet(command), 0, 1,
|
---|
114 | missionParameters[0], missionParameters[1], missionParameters[2], 0, missionParameters[3], missionParameters[4], missionParameters[5]);
|
---|
115 | mavCom->waitMissionAck(ACK_TIMEOUT);
|
---|
116 | }
|
---|
117 |
|
---|
118 | cout << "[INFO] Check mission item list.\n";
|
---|
119 | // mission request list (to check)
|
---|
120 | mavCom->sendMissionRequestList(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
121 | //mavCom.waitMissionCount(ACK_TIMEOUT);
|
---|
122 |
|
---|
123 | cout << "----------------\n";
|
---|
124 | cout << "Flight plan sent \n";
|
---|
125 | cout << "----------------\n";
|
---|
126 | }
|
---|
127 |
|
---|
128 | void GuiGcs::ParametersParse(const std::string& parametersString, std::array<float, 6>& parametersFloat) {
|
---|
129 | std::string parametersBuffer = parametersString;
|
---|
130 | for (int count = 0; count < parametersFloat.size(); count++) {
|
---|
131 | parametersFloat[count] = std::stof(parametersBuffer.substr(0, parametersBuffer.find(delimiter)));
|
---|
132 | parametersBuffer.erase(0, parametersBuffer.find(delimiter) + delimiter.length());
|
---|
133 | }
|
---|
134 | }
|
---|
135 |
|
---|
136 | uint16_t GuiGcs::MavCommandGet(const std::string& command) const {
|
---|
137 | uint16_t returnValue = -1;
|
---|
138 | if (command == "WAYPOINT") {
|
---|
139 | returnValue = MAV_CMD_NAV_WAYPOINT;
|
---|
140 | }
|
---|
141 | if (command == "TAKEOFF") {
|
---|
142 | returnValue = MAV_CMD_NAV_TAKEOFF;
|
---|
143 | }
|
---|
144 | if (command == "LAND") {
|
---|
145 | returnValue = MAV_CMD_NAV_LAND;
|
---|
146 | }
|
---|
147 | if (command == "RETURN") {
|
---|
148 | returnValue = MAV_CMD_NAV_RETURN_TO_LAUNCH;
|
---|
149 | }
|
---|
150 | if (command == "JUMP") {
|
---|
151 | returnValue = MAV_CMD_DO_JUMP;
|
---|
152 | }
|
---|
153 | return returnValue;
|
---|
154 | }
|
---|
155 |
|
---|
156 | void GuiGcs::Initialize() {
|
---|
157 | cout << "--------------------\n";
|
---|
158 | cout << "Initialization START\n";
|
---|
159 | cout << "--------------------\n";
|
---|
160 |
|
---|
161 | // UAV start heartbeat, message ID=0, interval 1000 ms
|
---|
162 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_HEARTBEAT, PERIOD_SECOND);
|
---|
163 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
164 |
|
---|
165 | usleep(INTER_COMMAND_PAUSE);
|
---|
166 |
|
---|
167 | // ask for the autopilot capabilities
|
---|
168 | mavCom->cmdRequestAutopilotCapabilities(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
169 | // TODO wait for autopilot version
|
---|
170 | //
|
---|
171 |
|
---|
172 | usleep(INTER_COMMAND_PAUSE);
|
---|
173 |
|
---|
174 | // initiate system status callback on the UAV, message ID=1, interval 1000 ms
|
---|
175 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYS_STATUS, PERIOD_SECOND);
|
---|
176 |
|
---|
177 | usleep(INTER_COMMAND_PAUSE);
|
---|
178 |
|
---|
179 | // initiate system time callback on the UAV
|
---|
180 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYSTEM_TIME, PERIOD_SECOND);
|
---|
181 |
|
---|
182 | usleep(INTER_COMMAND_PAUSE);
|
---|
183 |
|
---|
184 | // set preflight mode
|
---|
185 | mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_PREFLIGHT);
|
---|
186 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
187 |
|
---|
188 | usleep(INTER_COMMAND_PAUSE);
|
---|
189 |
|
---|
190 | // set home position
|
---|
191 | mavCom->cmdDoSetHome(mavCom->target.getSysID(), mavCom->target.getCompID(), 0);
|
---|
192 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
193 |
|
---|
194 | usleep(INTER_COMMAND_PAUSE);
|
---|
195 |
|
---|
196 | // clear all mission waypoints
|
---|
197 | mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
198 | usleep(INTER_COMMAND_PAUSE);
|
---|
199 | mavCom->waitMissionAck(ACK_TIMEOUT);
|
---|
200 | cout << "--------------------\n";
|
---|
201 | cout << "Initialization END\n";
|
---|
202 | cout << "--------------------\n";
|
---|
203 | }
|
---|