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

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

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