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 | //todo remove for tests
|
---|
30 | #include <iostream>
|
---|
31 |
|
---|
32 | using namespace std;
|
---|
33 | using namespace flair::core;
|
---|
34 | using namespace flair::gui;
|
---|
35 |
|
---|
36 | GuiGcs::GuiGcs(const FrameworkManager *parent, string name,
|
---|
37 | std::string &outputAddress, int outputPort):
|
---|
38 | GuiInterface(parent, name) {
|
---|
39 | cout << "MavPlanner GuiGcs constructor" << endl;
|
---|
40 | //outputSocket = new Socket((Thread *)this, "output socket", outputAddress + ":" + to_string(outputPort));
|
---|
41 |
|
---|
42 | mavCom = new MavlinkUDP(outputAddress, outputPort);
|
---|
43 | }
|
---|
44 |
|
---|
45 | GuiGcs::~GuiGcs() {
|
---|
46 | if (mavCom) {
|
---|
47 | mavCom->stopThreads();
|
---|
48 | delete mavCom;
|
---|
49 | }
|
---|
50 | }
|
---|
51 |
|
---|
52 | void GuiGcs::MissionStart() {
|
---|
53 | cout << "MavPlanner GuiGcs MissionStart" << endl;
|
---|
54 |
|
---|
55 | Initialize();
|
---|
56 |
|
---|
57 | cout << "--------------------\n";
|
---|
58 | cout << " MISSION START\n";
|
---|
59 | cout << "--------------------\n";
|
---|
60 |
|
---|
61 | // set auto armed mode
|
---|
62 | mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_AUTO_ARMED);
|
---|
63 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
64 |
|
---|
65 | usleep(INTER_COMMAND_PAUSE);
|
---|
66 |
|
---|
67 | // mission start message
|
---|
68 | mavCom->cmdMissionStart(mavCom->target.getSysID(), mavCom->target.getCompID(), mavCom->missionFirst, mavCom->missionLast);
|
---|
69 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
70 |
|
---|
71 | mavCom->missionStarted();
|
---|
72 | }
|
---|
73 |
|
---|
74 | void GuiGcs::MissionStop() {
|
---|
75 | cout << "MavPlanner GuiGcs MissionStop" << endl;
|
---|
76 | outputSocket->SendMessage("MavPlanner GuiGcs MissionStop");
|
---|
77 | }
|
---|
78 |
|
---|
79 | void GuiGcs::MissionResume() {
|
---|
80 | cout << "MavPlanner GuiGcs MissionResume" << endl;
|
---|
81 | outputSocket->SendMessage("MavPlanner GuiGcs MissionResume");
|
---|
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 | std::string delimiter = "|";
|
---|
106 | for (auto &missionCommand : missionCommandList) {
|
---|
107 | cout << "[INFO] Mission item : " << missionCommand << "\n";
|
---|
108 | // Parse command
|
---|
109 | std::string command = missionCommand.substr(0, missionCommand.find(delimiter));
|
---|
110 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
111 | // Parse param1
|
---|
112 | float param1 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
113 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
114 | // Parse param2
|
---|
115 | float param2 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
116 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
117 | // Parse param3
|
---|
118 | float param3 = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
119 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
120 | // Parse latitude
|
---|
121 | float latitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
122 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
123 | // Parse longitude
|
---|
124 | float longitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
125 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
126 | // Parse altitude
|
---|
127 | float altitude = std::stof(missionCommand.substr(0, missionCommand.find(delimiter)));
|
---|
128 | missionCommand.erase(0, missionCommand.find(delimiter) + delimiter.length());
|
---|
129 | // cout << "[INFO] Mission item commands : " << command << ", " << param1 << ", " << param2 << ", " << param3 << std::endl;
|
---|
130 | 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);
|
---|
131 | mavCom->waitMissionAck(ACK_TIMEOUT);
|
---|
132 | }
|
---|
133 |
|
---|
134 | cout << "[INFO] Check mission item list.\n";
|
---|
135 | // mission request list (to check)
|
---|
136 | mavCom->sendMissionRequestList(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
137 | //mavCom.waitMissionCount(ACK_TIMEOUT);
|
---|
138 |
|
---|
139 | cout << "----------------\n";
|
---|
140 | cout << "Flight plan sent \n";
|
---|
141 | cout << "----------------\n";
|
---|
142 | }
|
---|
143 |
|
---|
144 | uint16_t GuiGcs::MavCommandGet(const std::string& command) const {
|
---|
145 | uint16_t returnValue = -1;
|
---|
146 | if (command == "WAYPOINT") {
|
---|
147 | returnValue = MAV_CMD_NAV_WAYPOINT;
|
---|
148 | }
|
---|
149 | if (command == "TAKEOFF") {
|
---|
150 | returnValue = MAV_CMD_NAV_TAKEOFF;
|
---|
151 | }
|
---|
152 | if (command == "LAND") {
|
---|
153 | returnValue = MAV_CMD_NAV_LAND;
|
---|
154 | }
|
---|
155 | if (command == "RETURN") {
|
---|
156 | returnValue = MAV_CMD_NAV_RETURN_TO_LAUNCH;
|
---|
157 | }
|
---|
158 | if (command == "JUMP") {
|
---|
159 | returnValue = MAV_CMD_DO_JUMP;
|
---|
160 | }
|
---|
161 | return returnValue;
|
---|
162 | }
|
---|
163 |
|
---|
164 | void GuiGcs::Initialize() {
|
---|
165 | cout << "--------------------\n";
|
---|
166 | cout << "Initialization START\n";
|
---|
167 | cout << "--------------------\n";
|
---|
168 |
|
---|
169 | // UAV start heartbeat, message ID=0, interval 1000 ms
|
---|
170 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_HEARTBEAT, PERIOD_SECOND);
|
---|
171 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
172 |
|
---|
173 | usleep(INTER_COMMAND_PAUSE);
|
---|
174 |
|
---|
175 | // ask for the autopilot capabilities
|
---|
176 | mavCom->cmdRequestAutopilotCapabilities(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
177 | // TODO wait for autopilot version
|
---|
178 | //
|
---|
179 |
|
---|
180 | usleep(INTER_COMMAND_PAUSE);
|
---|
181 |
|
---|
182 | // initiate system status callback on the UAV, message ID=1, interval 1000 ms
|
---|
183 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYS_STATUS, PERIOD_SECOND);
|
---|
184 |
|
---|
185 | usleep(INTER_COMMAND_PAUSE);
|
---|
186 |
|
---|
187 | // initiate system time callback on the UAV
|
---|
188 | mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYSTEM_TIME, PERIOD_SECOND);
|
---|
189 |
|
---|
190 | usleep(INTER_COMMAND_PAUSE);
|
---|
191 |
|
---|
192 | // set preflight mode
|
---|
193 | mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_PREFLIGHT);
|
---|
194 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
195 |
|
---|
196 | usleep(INTER_COMMAND_PAUSE);
|
---|
197 |
|
---|
198 | // set home position
|
---|
199 | mavCom->cmdDoSetHome(mavCom->target.getSysID(), mavCom->target.getCompID(), 0);
|
---|
200 | mavCom->waitCommandAck(ACK_TIMEOUT);
|
---|
201 |
|
---|
202 | usleep(INTER_COMMAND_PAUSE);
|
---|
203 |
|
---|
204 | // clear all mission waypoints
|
---|
205 | mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
|
---|
206 | usleep(INTER_COMMAND_PAUSE);
|
---|
207 | mavCom->waitMissionAck(ACK_TIMEOUT);
|
---|
208 | cout << "--------------------\n";
|
---|
209 | cout << "Initialization END\n";
|
---|
210 | cout << "--------------------\n";
|
---|
211 | }
|
---|