[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 |
|
---|
[52] | 40 | #include <iostream>
|
---|
| 41 |
|
---|
[46] | 42 | using namespace flair::core;
|
---|
| 43 | using namespace flair::gui;
|
---|
| 44 | using namespace std;
|
---|
| 45 |
|
---|
| 46 | namespace flair {
|
---|
| 47 | namespace sensor {
|
---|
| 48 |
|
---|
| 49 | Mavlink::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",
|
---|
[50] | 69 | -90, 90, 0.001, 3, 3);
|
---|
[46] | 70 | lonField = new DoubleSpinBox(add_wptGroupBox->LastRowLastCol(), "Longitude",
|
---|
[50] | 71 | -180, 180, 0.001, 3, 3);
|
---|
[46] | 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");
|
---|
[47] | 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");
|
---|
[46] | 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 |
|
---|
| 97 | Mavlink::~Mavlink() {
|
---|
| 98 | }
|
---|
| 99 |
|
---|
| 100 | void 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");
|
---|
[50] | 115 | Thread::SleepMS(500);
|
---|
[46] | 116 | // latValue = latField->Value();
|
---|
| 117 | // lonValue = lonField->Value();
|
---|
| 118 |
|
---|
[47] | 119 | if (btn_initialize->Clicked()) {
|
---|
| 120 | MissionInitialize();
|
---|
[46] | 121 | }
|
---|
[47] | 122 | if (btn_start_mission->Clicked()) {
|
---|
| 123 | MissionStart();
|
---|
[46] | 124 | }
|
---|
[47] | 125 | if (btn_stop_mission->Clicked()) {
|
---|
| 126 | MissionStop();
|
---|
[46] | 127 | }
|
---|
[47] | 128 | if (btn_kill->Clicked()) {
|
---|
| 129 | MissionKill();
|
---|
[46] | 130 | }
|
---|
[47] | 131 | if (btn_add_mission_wpt->Clicked()) {
|
---|
[50] | 132 | list_mission_wpt->AddItem(GetWptLatLon());
|
---|
[46] | 133 | }
|
---|
[47] | 134 | if (btn_add_entrance_wpt->Clicked()) {
|
---|
[50] | 135 | list_entrance_wpt->AddItem(GetWptLatLon());
|
---|
[46] | 136 | }
|
---|
[47] | 137 | if (btn_add_exit_wpt->Clicked()) {
|
---|
[50] | 138 | list_exit_wpt->AddItem(GetWptLatLon());
|
---|
[47] | 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 | }
|
---|
[46] | 149 | if (btn_send_wpt->Clicked()) {
|
---|
[47] | 150 | WptSend();
|
---|
[46] | 151 | }
|
---|
[47] | 152 | if (btn_clear_wpt->Clicked()) {
|
---|
| 153 | WptClear();
|
---|
[46] | 154 | }
|
---|
[47] | 155 | if (btn_loop->Clicked()) {
|
---|
| 156 | WptLoopSend();
|
---|
[46] | 157 | }
|
---|
| 158 |
|
---|
| 159 | // Thread::Info("Debug: Lat=%f, Lon=%f\n", latValue, lonValue);
|
---|
| 160 | }
|
---|
| 161 | }
|
---|
| 162 |
|
---|
| 163 | void 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 |
|
---|
| 171 | void 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 |
|
---|
| 202 | void 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 |
|
---|
[50] | 250 | std::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 |
|
---|
[46] | 258 | // First tab callbacks
|
---|
| 259 | void Mavlink::MissionInitialize() {
|
---|
| 260 |
|
---|
| 261 | }
|
---|
| 262 | void Mavlink::MissionStart() {
|
---|
| 263 |
|
---|
| 264 | }
|
---|
| 265 | void Mavlink::MissionStop() {
|
---|
| 266 |
|
---|
| 267 | }
|
---|
| 268 | void Mavlink::MissionKill() {
|
---|
| 269 |
|
---|
| 270 | }
|
---|
| 271 | // Second tab callbacks
|
---|
| 272 | void Mavlink::MissionWptAdd() {
|
---|
| 273 |
|
---|
| 274 | }
|
---|
| 275 | void Mavlink::EntranceWptAdd() {
|
---|
| 276 |
|
---|
| 277 | }
|
---|
| 278 | void Mavlink::ExitWptAdd() {
|
---|
| 279 |
|
---|
| 280 | }
|
---|
| 281 | void Mavlink::WptSend() {
|
---|
| 282 |
|
---|
| 283 | }
|
---|
| 284 | void Mavlink::WptClear() {
|
---|
| 285 |
|
---|
| 286 | }
|
---|
| 287 | void Mavlink::WptLoopSend() {
|
---|
| 288 |
|
---|
| 289 | }
|
---|
| 290 |
|
---|
| 291 | void 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 |
|
---|
| 345 | std::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
|
---|