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

Last change on this file since 47 was 47, checked in by Thomas Fuhrmann, 5 years ago

GUI is working with ListWidget? (display only)

File size: 12.0 KB
RevLine 
[46]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>
[47]36#include <ListWidget.h>
[46]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.0001, 4, 3);
68  lonField = new DoubleSpinBox(add_wptGroupBox->LastRowLastCol(), "Longitude",
69                                -180, 180, 0.0001, 4, 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");
[47]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 
[46]83
[47]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
[46]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
[47]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");
[46]106
[47]107  list_mission_wpt->AddItem("toto");
108  list_entrance_wpt->AddItem("toto");
109  list_entrance_wpt->AddItem("tata");
110  list_exit_wpt->AddItem("toto");
[46]111
[47]112
[46]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(1000);
138    // latValue = latField->Value();
139    // lonValue = lonField->Value();
140       
141        // Check the values of the buttons
142        // First tab
[47]143        if (btn_initialize->Clicked()) {
144                MissionInitialize();
[46]145    }
[47]146        if (btn_start_mission->Clicked()) {
147                MissionStart();
[46]148    }
[47]149        if (btn_stop_mission->Clicked()) {
150                MissionStop();
[46]151    }
[47]152        if (btn_kill->Clicked()) {
153                MissionKill();
[46]154    }
[47]155        // Second tab
156        if (btn_add_mission_wpt->Clicked()) {
157      std::ostringstream stringStream;
158      stringStream.precision(3);
159      stringStream << fixed;
160      stringStream << latField->Value() << " - " << lonField->Value();
161                list_mission_wpt->AddItem(stringStream.str());
[46]162    }
[47]163        if (btn_add_entrance_wpt->Clicked()) {
164                EntranceWptAdd();
[46]165    }
[47]166        if (btn_add_exit_wpt->Clicked()) {
167                ExitWptAdd();
168    }
169    if (btn_remove_mission_wpt->Clicked()) {
170      list_mission_wpt->RemoveItem();
171    }
172    if (btn_remove_entrance_wpt->Clicked()) {
173      list_entrance_wpt->RemoveItem();
174    }
175    if (btn_remove_exit_wpt->Clicked()) {
176      list_exit_wpt->RemoveItem();
177    }
[46]178    if (btn_send_wpt->Clicked()) {
[47]179                WptSend();
[46]180    }
[47]181        if (btn_clear_wpt->Clicked()) {
182                WptClear();
[46]183    }
[47]184        if (btn_loop->Clicked()) {
185                WptLoopSend();
[46]186    }
187
188    // Thread::Info("Debug: Lat=%f, Lon=%f\n", latValue, lonValue);
189  }
190}
191
192void Mavlink::SendAllWptMessage() {
193  Thread::Info("Debug: SendAllWptMessage\n");
194  // SendWptMessage(latField_wpt0, lonField_wpt0);
195  // SendWptMessage(latField_wpt1, lonField_wpt1);
196  // SendWptMessage(latField_wpt2, lonField_wpt2);
197  // SendWptMessage(latField_wpt3, lonField_wpt3);
198}
199
200void Mavlink::SendWptMessage(gui::DoubleSpinBox* latField, gui::DoubleSpinBox* lonField) {
201
202// (uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
203// uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue,
204// float param1, float param2, float param3, float param4, float x, float y, float z)
205
206
207  Thread::Info("Debug: SendWptMessage\n");
208  mavlink_message_t msg;
209
210  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);
211
212
213  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
214  uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);
215
216  // //print the buffer
217  // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", buf_size);
218  // for(uint8_t index = 0; index < buf_size; index++) {
219  //   Thread::Info("%d\n", buf[index]);
220  // }
221
222  std::string message_to_send = U8toString(buf, buf_size);
223
224  //send the message
225  mavlinkSocket->SendMessage(message_to_send); 
226
227  DecodeWptMessage(message_to_send);
228
229}
230
231void Mavlink::DecodeWptMessage(std::string wpt_to_decode) {
232
233  Thread::Info("Debug: start of decode loop\n");
234
235  mavlink_message_t rcv_msg;
236  mavlink_status_t rcv_status;
237
238  float received_x;
239  float received_y;
240
241  const char * message_to_decode = wpt_to_decode.c_str();
242  uint8_t message_size = wpt_to_decode.size();
243
244  // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", message_size);
245
246  for (uint8_t index = 0; index < message_size; index++) {
247
248    //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);
249
250    uint8_t single_data = (uint8_t)message_to_decode[index];
251    // Thread::Info("%d\n", single_data);
252
253    // Thread::Info("Hexa data after cast : %02x\n", single_data);
254
255    if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
256        // Thread::Info("Debug: in the if, before the switch\n");
257        switch(rcv_msg.msgid)
258        {
259            case MAVLINK_MSG_ID_HEARTBEAT :
260                 Thread::Info("Debug: heartbeat received !!!\n");
261                 break;
262            case MAVLINK_MSG_ID_MISSION_ITEM :
263                 // Thread::Info("Debug: mission item received !!!\n");
264                 
265                 received_x = mavlink_msg_mission_item_get_x(&rcv_msg);
266                 received_y = mavlink_msg_mission_item_get_y(&rcv_msg);
267
268                 Thread::Info("Debug: received x : %f, y : %f\n", received_x, received_y);
269                 break;
270            default :
271                  Thread::Info("Debug: default, no item decoded...\n");
272                  break;
273        }
274    }
275  }
276  Thread::Info("Debug: end of decode loop\n");
277}
278
279// First tab callbacks
280void Mavlink::MissionInitialize() {
281       
282}
283void Mavlink::MissionStart() {
284       
285}
286void Mavlink::MissionStop() {
287       
288}
289void Mavlink::MissionKill() {
290       
291}
292// Second tab callbacks 
293void Mavlink::MissionWptAdd() {
294       
295}
296void Mavlink::EntranceWptAdd() {
297       
298}
299void Mavlink::ExitWptAdd() {
300       
301}
302void Mavlink::WptSend() {
303       
304}
305void Mavlink::WptClear() {
306       
307}
308void Mavlink::WptLoopSend() {
309       
310}
311
312void Mavlink::SendHeartbeat() {
313  Thread::Info("Debug: heartbeat pressed !\n");
314  mavlink_message_t msg;
315
316  msg.payload64[0] = (uint64_t)36;
317
318  mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_UNINIT);
319
320  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
321  uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);
322
323  // std::string data_buffer = U8toString(buf, buf_size);
324  // std::string data_buffer(my_chars);
325
326  // Thread::Info("Debug:  buf_size=%d, data_buffer_size=%d\n", buf_size, data_buffer.size());
327  // Thread::Info("Debug: send data_buffer=%s\n", data_buffer.c_str());
328
329  //to send the message thanks to udp
330  // mavlinkSocket->SendMessage(data_buffer);
331
332  //decode the content of the heartbeat message
333  Thread::Info("Debug: start of decode loop\n");
334
335  mavlink_message_t rcv_msg;
336  mavlink_status_t rcv_status;
337
338  // for (uint8_t index = 0; index < data_buffer.size(); index++) {
339  for (uint8_t index = 0; index < buf_size; index++) {
340
341    //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);
342
343    // uint8_t single_data = static_cast<uint8_t>(data_buffer.c_str()[index]);
344    uint8_t single_data = buf[index];
345
346    Thread::Info("Hexa data after cast : %02x\n", single_data);
347
348    if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
349        Thread::Info("Debug: in the if, before the switch\n");
350        switch(msg.msgid)
351        {
352            case MAVLINK_MSG_ID_HEARTBEAT:
353            {
354                 Thread::Info("Debug: heartbeat received !!!\n");
355            }
356            break;
357            default:
358              //Do nothing
359              break;
360        }
361    }
362  }
363  Thread::Info("Debug: end of decode loop\n");
364}
365
366std::string Mavlink::U8toString(const uint8_t* chars, const uint16_t chars_size) {
367  // string converted_chars;
368  char message[chars_size];
369
370  // Thread::Info("Debug: U8toString size : %d\n", chars_size);
371
372  for (uint16_t index = 0; index < chars_size; index++) {
373    message[index] = (char)chars[index];
374    // Thread::Info("Debug: U8toString index : %d, get_data : %02X, saved_data : %02x\n", index, (char)chars[index], message[index]);
375    // converted_chars.append((const char*)(&chars[index]));
376
377  }
378
379  std::string converted_chars(message, chars_size);
380
381  // Thread::Info("Debug: U8toString string size : %d\n", converted_chars.size());
382
383  return converted_chars; 
384}
385
386} // end namespace sensor
387} // end namespace flair
Note: See TracBrowser for help on using the repository browser.