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

Last change on this file since 52 was 52, checked in by Thomas Fuhrmann, 5 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.