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