source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/Mavlink_test.cpp@ 51

Last change on this file since 51 was 50, checked in by Thomas Fuhrmann, 8 years ago

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