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

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

Add the mavlink branch

File size: 11.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>
36
37#include "mavlink.h"
38
39using namespace flair::core;
40using namespace flair::gui;
41using namespace std;
42
43namespace flair {
44namespace sensor {
45
46Mavlink::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
109Mavlink::~Mavlink() {
110}
111
112void 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
169void 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
177void 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
208void 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
257void Mavlink::MissionInitialize() {
258       
259}
260void Mavlink::MissionStart() {
261       
262}
263void Mavlink::MissionStop() {
264       
265}
266void Mavlink::MissionKill() {
267       
268}
269// Second tab callbacks 
270void Mavlink::MissionWptAdd() {
271       
272}
273void Mavlink::EntranceWptAdd() {
274       
275}
276void Mavlink::ExitWptAdd() {
277       
278}
279void Mavlink::WptSend() {
280       
281}
282void Mavlink::WptClear() {
283       
284}
285void Mavlink::WptLoopSend() {
286       
287}
288
289void 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
343std::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
Note: See TracBrowser for help on using the repository browser.