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/07/05
|
---|
6 | // filename: Mavlink_test.cpp
|
---|
7 | //
|
---|
8 | // author: Thomas Fuhrmann
|
---|
9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
10 | //
|
---|
11 | // version: $Id: $
|
---|
12 | //
|
---|
13 | // purpose: Base class for host side remote controls that talks to target
|
---|
14 | // side through ethernet connection
|
---|
15 | //
|
---|
16 | //
|
---|
17 | /*********************************************************************/
|
---|
18 | #include "Mavlink_test.h"
|
---|
19 |
|
---|
20 | #include <Controller.h>
|
---|
21 | #include <cvmatrix.h>
|
---|
22 | #include <Tab.h>
|
---|
23 | #include <TabWidget.h>
|
---|
24 | #include <CheckBox.h>
|
---|
25 | #include <Label.h>
|
---|
26 | #include <DataPlot1D.h>
|
---|
27 | #include <SpinBox.h>
|
---|
28 | #include <FrameworkManager.h>
|
---|
29 | #include <Socket.h>
|
---|
30 | #include <sstream>
|
---|
31 |
|
---|
32 | #include <DoubleSpinBox.h>
|
---|
33 | #include <PushButton.h>
|
---|
34 | #include <ComboBox.h>
|
---|
35 | #include <GroupBox.h>
|
---|
36 |
|
---|
37 | #include "mavlink.h"
|
---|
38 |
|
---|
39 | using namespace flair::core;
|
---|
40 | using namespace flair::gui;
|
---|
41 | using namespace std;
|
---|
42 |
|
---|
43 | namespace flair {
|
---|
44 | namespace sensor {
|
---|
45 |
|
---|
46 | Mavlink::Mavlink(const FrameworkManager *parent, string name,
|
---|
47 | string receiverAddress, int receiverPort)
|
---|
48 | : Thread(parent, name, 6),
|
---|
49 | tab(new Tab(parent->GetTabWidget(), name)) {
|
---|
50 |
|
---|
51 | tabWidget = new TabWidget(tab->NewRow(), name);
|
---|
52 |
|
---|
53 | // Main tab
|
---|
54 | Tab* settingsTab = new Tab(tabWidget, "Settings");
|
---|
55 |
|
---|
56 | // Controls group
|
---|
57 | controlsGroupBox = new GroupBox(settingsTab->NewRow(), "Controls");
|
---|
58 | btn_initialize = new PushButton(controlsGroupBox->LastRowLastCol(), "Initialize");
|
---|
59 | btn_start_mission = new PushButton(controlsGroupBox->LastRowLastCol(), "Start mission");
|
---|
60 | btn_stop_mission = new PushButton(controlsGroupBox->LastRowLastCol(), "Stop mission");
|
---|
61 | btn_kill = new PushButton(controlsGroupBox->LastRowLastCol(), "Kill");
|
---|
62 |
|
---|
63 | // Add wpt group
|
---|
64 | add_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Add waypoint");
|
---|
65 | latField = new DoubleSpinBox(add_wptGroupBox->NewRow(), "Latitude",
|
---|
66 | -90, 90, 0.0001, 4, 3);
|
---|
67 | lonField = new DoubleSpinBox(add_wptGroupBox->LastRowLastCol(), "Longitude",
|
---|
68 | -180, 180, 0.0001, 4, 3);
|
---|
69 | btn_add_mission_wpt = new PushButton(add_wptGroupBox->NewRow(), "Add mission wpt");
|
---|
70 | btn_add_entrance_wpt = new PushButton(add_wptGroupBox->LastRowLastCol(), "Add entrance wpt");
|
---|
71 | btn_add_exit_wpt = new PushButton(add_wptGroupBox->LastRowLastCol(), "Add exit wpt");
|
---|
72 |
|
---|
73 | // Show wpt group
|
---|
74 | show_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Show waypoints");
|
---|
75 | label_mission_wpt = new Label(show_wptGroupBox->NewRow(), "Label mission");
|
---|
76 | label_mission_wpt->SetText("Mission wpt");
|
---|
77 | label_entrance_wpt = new Label(show_wptGroupBox->LastRowLastCol(), "Label entrance");
|
---|
78 | label_entrance_wpt->SetText("Entrance wpt");
|
---|
79 | label_exit_wpt = new Label(show_wptGroupBox->LastRowLastCol(), "Label exit");
|
---|
80 | label_exit_wpt->SetText("Exit wpt");
|
---|
81 | label_mission_wpt_list = new Label(show_wptGroupBox->NewRow(), "Label mission list");
|
---|
82 | label_entrance_wpt_list = new Label(show_wptGroupBox->LastRowLastCol(), "Label entrance list");
|
---|
83 | label_exit_wpt_list = new Label(show_wptGroupBox->LastRowLastCol(), "Label exit list");
|
---|
84 |
|
---|
85 | // Action wpt group
|
---|
86 | action_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Action");
|
---|
87 | btn_send_wpt = new PushButton(action_wptGroupBox->NewRow(), "Send wpt");
|
---|
88 | btn_clear_wpt = new PushButton(action_wptGroupBox->LastRowLastCol(), "Clear wpt");
|
---|
89 | btn_loop = new PushButton(action_wptGroupBox->LastRowLastCol(), "Loop");
|
---|
90 |
|
---|
91 |
|
---|
92 |
|
---|
93 | // Put some initial values in the labels
|
---|
94 | label_mission_wpt_list->SetText("3/3\n5/3.5\n");
|
---|
95 | label_entrance_wpt_list->SetText("1.5/2.5\n6/6.5\n");
|
---|
96 | label_exit_wpt_list->SetText("7/7.5\n2/5\n");
|
---|
97 |
|
---|
98 | // cmb_box_mission_wpt = new ComboBox(settingsTab->NewRow(), "Mission wpt list");
|
---|
99 | // cmb_box_mission_wpt->AddItem("7 - 7");
|
---|
100 | // cmb_box_mission_wpt->AddItem("2.5 - 3");
|
---|
101 | // cmb_box_mission_wpt->AddItem("3 - 6.5");
|
---|
102 |
|
---|
103 | mavlinkSocket = new Socket((Thread *)this, "mavlink socket", "127.0.0.1:5000");
|
---|
104 |
|
---|
105 | recvSocket = new Socket((Thread *)this, "recv socket", "127.0.0.1:5001");
|
---|
106 |
|
---|
107 | }
|
---|
108 |
|
---|
109 | Mavlink::~Mavlink() {
|
---|
110 | }
|
---|
111 |
|
---|
112 | void Mavlink::Run() {
|
---|
113 | Thread::Info("Debug: enter mavlink acquisition loop\n");
|
---|
114 |
|
---|
115 | // double latValue;
|
---|
116 | // double lonValue;
|
---|
117 |
|
---|
118 | Message *msgControllerAction = new Message(1024);
|
---|
119 |
|
---|
120 | if (getFrameworkManager()->ErrorOccured()) {
|
---|
121 | SafeStop();
|
---|
122 | Thread::Err("An error occurred, we don't launch the Run loop.\n");
|
---|
123 | }
|
---|
124 |
|
---|
125 | while (!ToBeStopped()) {
|
---|
126 | // Thread::Info("Debug: Mavlink acquisition loop\n");
|
---|
127 | Thread::SleepMS(1000);
|
---|
128 | // latValue = latField->Value();
|
---|
129 | // lonValue = lonField->Value();
|
---|
130 |
|
---|
131 | // Check the values of the buttons
|
---|
132 | // First tab
|
---|
133 | if (btn_initialize->Clicked()) {
|
---|
134 | MissionInitialize();
|
---|
135 | }
|
---|
136 | if (btn_start_mission->Clicked()) {
|
---|
137 | MissionStart();
|
---|
138 | }
|
---|
139 | if (btn_stop_mission->Clicked()) {
|
---|
140 | MissionStop();
|
---|
141 | }
|
---|
142 | if (btn_kill->Clicked()) {
|
---|
143 | MissionKill();
|
---|
144 | }
|
---|
145 | // Second tab
|
---|
146 | if (btn_add_mission_wpt->Clicked()) {
|
---|
147 | MissionWptAdd();
|
---|
148 | }
|
---|
149 | if (btn_add_entrance_wpt->Clicked()) {
|
---|
150 | EntranceWptAdd();
|
---|
151 | }
|
---|
152 | if (btn_add_exit_wpt->Clicked()) {
|
---|
153 | ExitWptAdd();
|
---|
154 | }
|
---|
155 | if (btn_send_wpt->Clicked()) {
|
---|
156 | WptSend();
|
---|
157 | }
|
---|
158 | if (btn_clear_wpt->Clicked()) {
|
---|
159 | WptClear();
|
---|
160 | }
|
---|
161 | if (btn_loop->Clicked()) {
|
---|
162 | WptLoopSend();
|
---|
163 | }
|
---|
164 |
|
---|
165 | // Thread::Info("Debug: Lat=%f, Lon=%f\n", latValue, lonValue);
|
---|
166 | }
|
---|
167 | }
|
---|
168 |
|
---|
169 | void Mavlink::SendAllWptMessage() {
|
---|
170 | Thread::Info("Debug: SendAllWptMessage\n");
|
---|
171 | // SendWptMessage(latField_wpt0, lonField_wpt0);
|
---|
172 | // SendWptMessage(latField_wpt1, lonField_wpt1);
|
---|
173 | // SendWptMessage(latField_wpt2, lonField_wpt2);
|
---|
174 | // SendWptMessage(latField_wpt3, lonField_wpt3);
|
---|
175 | }
|
---|
176 |
|
---|
177 | void Mavlink::SendWptMessage(gui::DoubleSpinBox* latField, gui::DoubleSpinBox* lonField) {
|
---|
178 |
|
---|
179 | // (uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
|
---|
180 | // uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue,
|
---|
181 | // float param1, float param2, float param3, float param4, float x, float y, float z)
|
---|
182 |
|
---|
183 |
|
---|
184 | Thread::Info("Debug: SendWptMessage\n");
|
---|
185 | mavlink_message_t msg;
|
---|
186 |
|
---|
187 | mavlink_msg_mission_item_pack(100, 200, &msg, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, latField->Value(), lonField->Value(), 5);
|
---|
188 |
|
---|
189 |
|
---|
190 | uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
---|
191 | uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);
|
---|
192 |
|
---|
193 | // //print the buffer
|
---|
194 | // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", buf_size);
|
---|
195 | // for(uint8_t index = 0; index < buf_size; index++) {
|
---|
196 | // Thread::Info("%d\n", buf[index]);
|
---|
197 | // }
|
---|
198 |
|
---|
199 | std::string message_to_send = U8toString(buf, buf_size);
|
---|
200 |
|
---|
201 | //send the message
|
---|
202 | mavlinkSocket->SendMessage(message_to_send);
|
---|
203 |
|
---|
204 | DecodeWptMessage(message_to_send);
|
---|
205 |
|
---|
206 | }
|
---|
207 |
|
---|
208 | void Mavlink::DecodeWptMessage(std::string wpt_to_decode) {
|
---|
209 |
|
---|
210 | Thread::Info("Debug: start of decode loop\n");
|
---|
211 |
|
---|
212 | mavlink_message_t rcv_msg;
|
---|
213 | mavlink_status_t rcv_status;
|
---|
214 |
|
---|
215 | float received_x;
|
---|
216 | float received_y;
|
---|
217 |
|
---|
218 | const char * message_to_decode = wpt_to_decode.c_str();
|
---|
219 | uint8_t message_size = wpt_to_decode.size();
|
---|
220 |
|
---|
221 | // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", message_size);
|
---|
222 |
|
---|
223 | for (uint8_t index = 0; index < message_size; index++) {
|
---|
224 |
|
---|
225 | //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);
|
---|
226 |
|
---|
227 | uint8_t single_data = (uint8_t)message_to_decode[index];
|
---|
228 | // Thread::Info("%d\n", single_data);
|
---|
229 |
|
---|
230 | // Thread::Info("Hexa data after cast : %02x\n", single_data);
|
---|
231 |
|
---|
232 | if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
|
---|
233 | // Thread::Info("Debug: in the if, before the switch\n");
|
---|
234 | switch(rcv_msg.msgid)
|
---|
235 | {
|
---|
236 | case MAVLINK_MSG_ID_HEARTBEAT :
|
---|
237 | Thread::Info("Debug: heartbeat received !!!\n");
|
---|
238 | break;
|
---|
239 | case MAVLINK_MSG_ID_MISSION_ITEM :
|
---|
240 | // Thread::Info("Debug: mission item received !!!\n");
|
---|
241 |
|
---|
242 | received_x = mavlink_msg_mission_item_get_x(&rcv_msg);
|
---|
243 | received_y = mavlink_msg_mission_item_get_y(&rcv_msg);
|
---|
244 |
|
---|
245 | Thread::Info("Debug: received x : %f, y : %f\n", received_x, received_y);
|
---|
246 | break;
|
---|
247 | default :
|
---|
248 | Thread::Info("Debug: default, no item decoded...\n");
|
---|
249 | break;
|
---|
250 | }
|
---|
251 | }
|
---|
252 | }
|
---|
253 | Thread::Info("Debug: end of decode loop\n");
|
---|
254 | }
|
---|
255 |
|
---|
256 | // First tab callbacks
|
---|
257 | void Mavlink::MissionInitialize() {
|
---|
258 |
|
---|
259 | }
|
---|
260 | void Mavlink::MissionStart() {
|
---|
261 |
|
---|
262 | }
|
---|
263 | void Mavlink::MissionStop() {
|
---|
264 |
|
---|
265 | }
|
---|
266 | void Mavlink::MissionKill() {
|
---|
267 |
|
---|
268 | }
|
---|
269 | // Second tab callbacks
|
---|
270 | void Mavlink::MissionWptAdd() {
|
---|
271 |
|
---|
272 | }
|
---|
273 | void Mavlink::EntranceWptAdd() {
|
---|
274 |
|
---|
275 | }
|
---|
276 | void Mavlink::ExitWptAdd() {
|
---|
277 |
|
---|
278 | }
|
---|
279 | void Mavlink::WptSend() {
|
---|
280 |
|
---|
281 | }
|
---|
282 | void Mavlink::WptClear() {
|
---|
283 |
|
---|
284 | }
|
---|
285 | void Mavlink::WptLoopSend() {
|
---|
286 |
|
---|
287 | }
|
---|
288 |
|
---|
289 | void Mavlink::SendHeartbeat() {
|
---|
290 | Thread::Info("Debug: heartbeat pressed !\n");
|
---|
291 | mavlink_message_t msg;
|
---|
292 |
|
---|
293 | msg.payload64[0] = (uint64_t)36;
|
---|
294 |
|
---|
295 | mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_UNINIT);
|
---|
296 |
|
---|
297 | uint8_t buf[MAVLINK_MAX_PACKET_LEN];
|
---|
298 | uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);
|
---|
299 |
|
---|
300 | // std::string data_buffer = U8toString(buf, buf_size);
|
---|
301 | // std::string data_buffer(my_chars);
|
---|
302 |
|
---|
303 | // Thread::Info("Debug: buf_size=%d, data_buffer_size=%d\n", buf_size, data_buffer.size());
|
---|
304 | // Thread::Info("Debug: send data_buffer=%s\n", data_buffer.c_str());
|
---|
305 |
|
---|
306 | //to send the message thanks to udp
|
---|
307 | // mavlinkSocket->SendMessage(data_buffer);
|
---|
308 |
|
---|
309 | //decode the content of the heartbeat message
|
---|
310 | Thread::Info("Debug: start of decode loop\n");
|
---|
311 |
|
---|
312 | mavlink_message_t rcv_msg;
|
---|
313 | mavlink_status_t rcv_status;
|
---|
314 |
|
---|
315 | // for (uint8_t index = 0; index < data_buffer.size(); index++) {
|
---|
316 | for (uint8_t index = 0; index < buf_size; index++) {
|
---|
317 |
|
---|
318 | //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);
|
---|
319 |
|
---|
320 | // uint8_t single_data = static_cast<uint8_t>(data_buffer.c_str()[index]);
|
---|
321 | uint8_t single_data = buf[index];
|
---|
322 |
|
---|
323 | Thread::Info("Hexa data after cast : %02x\n", single_data);
|
---|
324 |
|
---|
325 | if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
|
---|
326 | Thread::Info("Debug: in the if, before the switch\n");
|
---|
327 | switch(msg.msgid)
|
---|
328 | {
|
---|
329 | case MAVLINK_MSG_ID_HEARTBEAT:
|
---|
330 | {
|
---|
331 | Thread::Info("Debug: heartbeat received !!!\n");
|
---|
332 | }
|
---|
333 | break;
|
---|
334 | default:
|
---|
335 | //Do nothing
|
---|
336 | break;
|
---|
337 | }
|
---|
338 | }
|
---|
339 | }
|
---|
340 | Thread::Info("Debug: end of decode loop\n");
|
---|
341 | }
|
---|
342 |
|
---|
343 | std::string Mavlink::U8toString(const uint8_t* chars, const uint16_t chars_size) {
|
---|
344 | // string converted_chars;
|
---|
345 | char message[chars_size];
|
---|
346 |
|
---|
347 | // Thread::Info("Debug: U8toString size : %d\n", chars_size);
|
---|
348 |
|
---|
349 | for (uint16_t index = 0; index < chars_size; index++) {
|
---|
350 | message[index] = (char)chars[index];
|
---|
351 | // Thread::Info("Debug: U8toString index : %d, get_data : %02X, saved_data : %02x\n", index, (char)chars[index], message[index]);
|
---|
352 | // converted_chars.append((const char*)(&chars[index]));
|
---|
353 |
|
---|
354 | }
|
---|
355 |
|
---|
356 | std::string converted_chars(message, chars_size);
|
---|
357 |
|
---|
358 | // Thread::Info("Debug: U8toString string size : %d\n", converted_chars.size());
|
---|
359 |
|
---|
360 | return converted_chars;
|
---|
361 | }
|
---|
362 |
|
---|
363 | } // end namespace sensor
|
---|
364 | } // end namespace flair
|
---|