MainWindow.cpp 19.1 KB
Newer Older
roangel's avatar
roangel committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//    Main window of the Student's GUI
//
//    Copyright (C) 2017  Angel Romero
//
//    This program is free software: you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program.  If not, see <http://www.gnu.org/licenses/>.

roangel's avatar
roangel committed
18
19
#include "MainWindow.h"
#include "ui_MainWindow.h"
20
#include <string>
roangel's avatar
roangel committed
21

22
23
#include <ros/ros.h>
#include <ros/network.h>
24
#include <ros/package.h>
25

26
#include "d_fall_pps/CMQuery.h"
27
#include "d_fall_pps/Anchors.h"
28
29
30

#include "d_fall_pps/ViconData.h"

31
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
roangel's avatar
roangel committed
32
    QMainWindow(parent),
33
    ui(new Ui::MainWindow),
34
    m_battery_level(0)
roangel's avatar
roangel committed
35
{
36

roangel's avatar
roangel committed
37
    ui->setupUi(this);
38
39

    m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
40
    m_rosNodeThread->init();
41

42
43
    setCrazyRadioStatus(DISCONNECTED);

44
45
    m_ros_namespace = ros::this_node::getNamespace();
    ROS_INFO("namespace: %s", m_ros_namespace.c_str());
46

47
48
    qRegisterMetaType<CrazyflieData>("CrazyflieData");
    QObject::connect(m_rosNodeThread, SIGNAL(newPositionData(const CrazyflieData&)), this, SLOT(updatePositionData(const CrazyflieData&)));
49

50
    ros::NodeHandle nodeHandle(m_ros_namespace);
51

52
53
54
    customSetpointPublisher = nodeHandle.advertise<Setpoint>("CustomControllerService/Setpoint", 1);
    customSetpointSubscriber = nodeHandle.subscribe("CustomControllerService/Setpoint", 1, &MainWindow::customSetpointCallback, this);

55
    // subscribers
roangel's avatar
roangel committed
56
    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
57

58
59
    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);

60
61
    flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);

62
63
    batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);

64
65
    controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);

66

67
    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
68
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
69

70
    ros::NodeHandle my_nodeHandle("~");
71
    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
72
73
74
    customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
    safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);

75

76
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
77
    ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
78
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
79
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
80

81
82

    // First get student ID
83
84
85
86
87
    if(!nh_PPSClient.getParam("studentID", m_student_id))
    {
		ROS_ERROR("Failed to get studentID");
	}

88
89
90
91
92
93
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();



roangel's avatar
roangel committed
94
95
    std::vector<float> default_setpoint(4);
    ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
96
97
98
99

    ROS_INFO_STREAM(m_ros_namespace << "/SafeControllerService");

    if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
100
    {
101
        ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
roangel's avatar
roangel committed
102
103
104
105
106
107
108
109
    }


    ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
    ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
    ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
    ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));

110
    disableGUI();
111
    highlightSafeControllerTab();
112
    ui->label_battery->setStyleSheet("QLabel { color : red; }");
roangel's avatar
roangel committed
113
    m_battery_state = BATTERY_STATE_NORMAL;
roangel's avatar
roangel committed
114

115

116

117
    // NEW STUFF
118

119
120
121
122
123
124
125
    UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
    UWBActivatedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("uwbChanged", 1);
    updateUWBState();

    UWBChangedSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, &MainWindow::uwbChangedCallback, this);

    QObject::connect(ui->checkBox_enable_UWB, SIGNAL(stateChanged(int)), this, SLOT(on_checkBox_enable_UWB_toggled(int)));
126
127
}

128
129
130
131
132
133
134

/*********************************

    UI Callbacks

*********************************/

135
136
137
138
139
void MainWindow::highlightSafeControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
}
140

141
142
143
144
145
146
void MainWindow::highlightCustomControllerTab()
{
    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
}

147
void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
148
{
149
    m_safe_setpoint = newSetpoint;
150
    // here we get the new setpoint, need to update it in GUI
151
152
153
154
    ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
155
156
}

157
158
159
160
161
162
163
164
165
166
void MainWindow::customSetpointCallback(const Setpoint& newSetpoint)
{
    m_custom_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
    ui->current_setpoint_x_2->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y_2->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z_2->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw_2->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}

167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
{
    QString qstr = "Flying State: ";
    switch(msg.data)
    {
        case STATE_MOTORS_OFF:
            qstr.append("Motors OFF");
            break;
        case STATE_TAKE_OFF:
            qstr.append("Take OFF");
            break;
        case STATE_FLYING:
            qstr.append("Flying");
            break;
        case STATE_LAND:
            qstr.append("Land");
            break;
        default:
            break;
    }
    ui->flying_state_label->setText(qstr);
}

190
191
void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
{
192
193
194
195
196
197
    // switch case with unabling buttons motors off, take off, etc... when battery is low
    QString qstr = "";
    switch(msg.data)
    {
        case BATTERY_STATE_LOW:
            qstr.append("Low Battery!");
198
199
200
201
            ui->take_off_button->setEnabled(false);
            ui->land_button->setEnabled(false);
            // ui->groupBox_4->setEnabled(false);

202
            ui->label_battery->setText(qstr);
roangel's avatar
roangel committed
203
            m_battery_state = BATTERY_STATE_LOW;
204
205
            break;
        case BATTERY_STATE_NORMAL:
206
207
208
209
            // ui->groupBox_4->setEnabled(true);
            ui->take_off_button->setEnabled(true);
            ui->land_button->setEnabled(true);

210
            ui->label_battery->clear();
roangel's avatar
roangel committed
211
            m_battery_state = BATTERY_STATE_NORMAL;
212
213
214
215
            break;
        default:
            break;
    }
216
217
}

218
219
220
221
222
223
224
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
            // change icon, the rest of the GUI is available now
225
            ui->connected_disconnected_label->setText("Crazyradio status: Connected!");
226
            enableGUI();
227
228
229
            break;
        case CONNECTING:
            // change icon
230
            ui->connected_disconnected_label->setText("Crazyradio status: Connecting...");
231
232
233
            break;
        case DISCONNECTED:
            // change icon, the rest of the GUI is disabled
234
            ui->connected_disconnected_label->setText("Crazyradio status: Disconnected");
235
            disableGUI();
236
237
            break;
        default:
238
            ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
239
240
241
242
243
            break;
    }
    this->m_radio_status = radio_status;
}

244
245
246
247
void MainWindow::updateBatteryVoltage(float battery_voltage)
{
    m_battery_voltage = battery_voltage;
    // Need to take voltage, display it and transform it to percentage
248
    // int percentage = (int) fromVoltageToPercent(m_battery_voltage);
249

roangel's avatar
roangel committed
250
    QString qstr = "";
roangel's avatar
roangel committed
251
    qstr.append(QString::number(battery_voltage, 'f', 2));
roangel's avatar
roangel committed
252
    qstr.append(" V");
253
    //ROS_INFO_STREAM("battery voltage " << battery_voltage);
roangel's avatar
roangel committed
254
    ui->voltage_field->setText(qstr);
255
256
257
258
}

void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
{
259
    //ROS_INFO("callback CFBattery received in GUI");
260
261
262
    updateBatteryVoltage(msg.data);
}

263
264
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
265
    ROS_INFO("Callback CrazyRadioStatus called");
266
267
268
    this->setCrazyRadioStatus(msg.data);
}

269
270
271
272
void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
273
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
274
    ROS_INFO("command reconnect published");
275
}
276

277
void MainWindow::on_take_off_button_clicked()
278
{
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_TAKE_OFF;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_land_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_LAND;
    this->PPSClientCommandPublisher.publish(msg);
}

void MainWindow::on_motors_OFF_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_CRAZYFLY_MOTORS_OFF;
    this->PPSClientCommandPublisher.publish(msg);
296
}
297
298
299
300
301
302
303

void MainWindow::on_set_setpoint_button_clicked()
{
    Setpoint msg_setpoint;
    msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
    msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
    msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
roangel's avatar
roangel committed
304
    msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
305

306
    this->controllerSetpointPublisher.publish(msg_setpoint);
307
}
308

309
310
311
312
313
314
315
316
317
318
319
void MainWindow::on_set_setpoint_button_2_clicked()
{
    Setpoint msg_setpoint;
    msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
    msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
    msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
    msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;

    this->customSetpointPublisher.publish(msg_setpoint);
}

320
void MainWindow::on_RF_disconnect_button_clicked()
321
322
323
324
325
326
{
    std_msgs::Int32 msg;
    msg.data = CMD_DISCONNECT;
    this->crazyRadioCommandPublisher.publish(msg);
    ROS_INFO("command disconnect published");
}
327

328
329
330
331
332
333
334
335
336
337
338
void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
{
    // send msg that says that parameters have changed in YAML file
    std_msgs::Int32 msg;
    msg.data = 1;
    this->safeYAMLloadedPublisher.publish(msg);
    ROS_INFO("YALMloaded published");
    ui->load_safe_yaml_button->setEnabled(true);
}

void MainWindow::on_load_safe_yaml_button_clicked()
339
{
340
341
342
    ui->load_safe_yaml_button->setEnabled(false);
    ros::NodeHandle nodeHandle("~");
    m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::safeYamlFileTimerCallback, this, true);
roangel's avatar
roangel committed
343
344
345
346
347
348
349
350
351
352

    std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
    ROS_INFO_STREAM(d_fall_pps_path);

    // first, reload the name of the custom controller:
    std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);

    // then, reload the parameters of the custom controller:
353
    cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
roangel's avatar
roangel committed
354
355
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);
356
357
}

358
359
360


void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
361
{
362
363
364
365
366
367
368
369
370
371
372
    // send msg that says that parameters have changed in YAML file
    std_msgs::Int32 msg;
    msg.data = 1;
    this->customYAMLloadedPublisher.publish(msg);
    ROS_INFO("YALMloaded published");
    ui->load_custom_yaml_button->setEnabled(true);
}

void MainWindow::on_load_custom_yaml_button_clicked()
{
    ui->load_custom_yaml_button->setEnabled(false);
roangel's avatar
roangel committed
373
    ros::NodeHandle nodeHandle("~");
374
375
376
377
378
379
380
381
382
383
384
385
386
387
    m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::customYamlFileTimerCallback, this, true);

    std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
    ROS_INFO_STREAM(d_fall_pps_path);

    // first, reload the name of the custom controller:
    std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);

    // then, reload the parameters of the custom controller:
    cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);
388
389
}

390
391
392
393
void MainWindow::on_en_custom_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_CUSTOM_CONTROLLER;
roangel's avatar
roangel committed
394
    this->PPSClientCommandPublisher.publish(msg);
395
396
397
398
399
400
}

void MainWindow::on_en_safe_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_SAFE_CONTROLLER;
roangel's avatar
roangel committed
401
    this->PPSClientCommandPublisher.publish(msg);
402
}
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592

void MainWindow::on_checkBox_enable_UWB_toggled(int state)
{
    std_msgs::Int32 msg;
    msg.data = ui->checkBox_enable_UWB->isChecked();

    UWBActivatedPublisher.publish(msg);
}

/*********************************

    UWB functions

*********************************/

void MainWindow::updateUWBState()
{
    // Load UWB Settings from the UWBManagerService
    Anchors a;

    if(UWBServiceClient.call(a))
    {
        ui->checkBox_enable_UWB->setCheckable(a.response.enableUWB);
    }
    else
    {
        ROS_ERROR("[Student GUI] Could not update UWB Settings!");
        return;
    }

    std_msgs::Int32 msg;
    msg.data = ui->checkBox_enable_UWB->isChecked();

    UWBActivatedPublisher.publish(msg);
}

void MainWindow::uwbChangedCallback(const std_msgs::Int32& msg)
{
    updateUWBState();
}


/*********************************

    Other random functions

*********************************/

MainWindow::~MainWindow()
{
    delete ui;
}

void MainWindow::disableGUI()
{
    ui->motors_OFF_button->setEnabled(false);
    ui->take_off_button->setEnabled(false);
    ui->land_button->setEnabled(false);
}

void MainWindow::enableGUI()
{
    ui->motors_OFF_button->setEnabled(true);
    if(m_battery_state == BATTERY_STATE_NORMAL)
    {
        ui->take_off_button->setEnabled(true);
        ui->land_button->setEnabled(true);
    }
}

void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
    ROS_INFO("context reloaded in student_GUI");
}

void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
{
    switch(msg.data)
    {
        case SAFE_CONTROLLER:
            highlightSafeControllerTab();
            break;
        case CUSTOM_CONTROLLER:
            highlightCustomControllerTab();
            break;
        default:
            break;
    }
}

float MainWindow::fromVoltageToPercent(float voltage)
{
    int num_cutoffs = m_cutoff_voltages.size();
    float hysteresis = 0.05;

    while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
    {
        ++m_battery_level;
    }
    while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
    {
        --m_battery_level;
    }

    float percentage = 100.0 * m_battery_level/num_cutoffs;

    // should not hapen, but just in case...
    if(percentage > 100)
        percentage = 100;
    if(percentage < 0)
        percentage = 0;

    return percentage;
}

void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall))
    {
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
        // we now have the m_context variable with the current context. Put CF Name in label

        QString qstr = "StudentID ";
        qstr.append(QString::number(m_student_id));
        qstr.append(" connected to CF ");
        qstr.append(QString::fromStdString(m_context.crazyflieName));
        ui->groupBox->setTitle(qstr);
	}
    else
    {
		ROS_ERROR("Failed to load context");
	}

	ros::NodeHandle nh("CrazyRadio");
	nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
}

void MainWindow::coordinatesToLocal(CrazyflieData& cf)
{
	AreaBounds area = m_context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
    // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
    float originZ = 0.0;
	// float originZ = (area.zmin + area.zmax) / 2.0;

	cf.x -= originX;
	cf.y -= originY;
	cf.z -= originZ;
}

void MainWindow::updatePositionData(const CrazyflieData& data)
{
    CrazyflieData local = data;
    coordinatesToLocal(local);

    // now we have the local coordinates, put them in the labels
    ui->current_x->setText(QString::number(local.x, 'f', 3));
    ui->current_y->setText(QString::number(local.y, 'f', 3));
    ui->current_z->setText(QString::number(local.z, 'f', 3));
    ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
    ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
    ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));

    ui->current_x_2->setText(QString::number(local.x, 'f', 3));
    ui->current_y_2->setText(QString::number(local.y, 'f', 3));
    ui->current_z_2->setText(QString::number(local.z, 'f', 3));
    ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
    ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
    ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));

    // also update diff
    ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
    ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
    ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
    ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));

    ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
    ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
    ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
    ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}