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 |
---|