Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
/*=====================================================================
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
(c) 2009, 2010 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
This file is part of the PIXHAWK project
PIXHAWK 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.
PIXHAWK 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 PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
======================================================================*/
/**
* @file
* @brief Represents one unmanned aerial vehicle
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
#include <QList>
#include <QMessageBox>
#include <QTimer>
#include <iostream>
#include <QDebug>
#include <cmath>
#include "UAS.h"
#include "LinkInterface.h"
#include "UASManager.h"
#include "MG.h"
#include "mavlink.h"
UAS::UAS(int id=0) :
startTime(MG::TIME::getGroundTimeNow()),
commStatus(COMM_DISCONNECTED),
name(""),
links(new QList<LinkInterface*>()),
thrustSum(0),
startVoltage(0),
manualRollAngle(0),
manualPitchAngle(0),
manualYawAngle(0),
manualThrust(0),
controlRollManual(true),
controlPitchManual(true),
controlYawManual(true),
currentVoltage(0.0f),
controlThrustManual(true),
lpVoltage(0.0f),
mode(MAV_MODE_UNINIT),
onboardTimeOffset(0)
{
uasId = id;
setBattery(LIPOLY, 3);
}
UAS::~UAS()
{
delete links;
}
int UAS::getUASID()
{
return uasId;
}
void UAS::setSelected()
{
UASManager::instance()->setActiveUAS(this);
}
void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
if (!links->contains(link))
{
addLink(link);
}
if (message.sysid == uasId)
{
QString uasState;
QString stateDescription;
QString patternPath;
bool uasIsAuto;
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
emit heartbeat(this);
// Set new type if it has changed
if (this->type != message_heartbeat_get_type(&message))
{
this->type = message_heartbeat_get_type(&message);
emit systemTypeSet(this, type);
}
break;
case MAVLINK_MSG_ID_BOOT:
//std::cerr << "Boot detected, software v." << std::dec << message_boot_get_version(message.payload) << " time: " << MG::TIME::getGroundTimeNow() << std::endl;
//emit valueChanged(uasId, "Software version", message_boot_get_version(message.payload), MG::TIME::getGroundTimeNow());
getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
onboardTimeOffset = 0; // Reset offset measurement
break;
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
case MAVLINK_MSG_ID_SYS_STATUS:
{
sys_status_t state;
message_sys_status_decode(&message, &state);
getStatusForCode((int)state.status, uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
QString mode;
switch (state.mode)
{
case MAV_MODE_LOCKED:
mode = "MAV_MODE_LOCKED";
break;
case MAV_MODE_MANUAL:
mode = "MAV_MODE_MANUAL";
break;
case MAV_MODE_AUTO:
mode = "MAV_MODE_AUTO";
break;
case MAV_MODE_TEST1:
mode = "MAV_MODE_TEST1";
break;
case MAV_MODE_TEST2:
mode = "MAV_MODE_TEST2";
break;
case MAV_MODE_TEST3:
mode = "MAV_MODE_TEST3";
break;
default:
mode = "MAV_MODE_UNINIT";
break;
}
emit modeChanged(this->getUASID(), mode, "");
currentVoltage = state.vbat;
filterVoltage(currentVoltage);
if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining();
//qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
emit batteryChanged(this, filterVoltage(), getChargeLevel(), timeRemaining);
emit voltageChanged(message.sysid, state.vbat/1000.0f);
}
break;
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
/*
case MAVLINK_MSG_ID_SYSTEM:
// std::cerr << std::endl;
// std::cerr << "System Message received" << std::endl;
currentVoltage = message_system_get_voltage(message.payload)/1000.0f;
filterVoltage(currentVoltage);
if (startVoltage == 0) startVoltage = currentVoltage;
timeRemaining = calculateTimeRemaining();
//qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining;
emit batteryChanged(this, filterVoltage(), getChargeLevel(), timeRemaining);
emit voltageChanged(message.acid, message_system_get_voltage(message.payload)/1000.0f);
emit loadChanged(this, message_system_get_cpu_usage(message.payload)/100.0f);
emit valueChanged(uasId, "Battery", message_system_get_voltage(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "CPU Load", message_system_get_cpu_usage(message.payload), MG::TIME::getGroundTimeNow());
break;
case MAVLINK_MSG_ID_ACTUATORS:
emit actuatorChanged(this, 0, message_actuator_get_act1(message.payload));
emit valueChanged(this->getUASID(), "Top Rotor", message_actuator_get_act1(message.payload), MG::TIME::getGroundTimeNow());
emit actuatorChanged(this, 1, message_actuator_get_act2(message.payload));
emit valueChanged(this->getUASID(), "Bottom Rotor", message_actuator_get_act2(message.payload), MG::TIME::getGroundTimeNow());
emit actuatorChanged(this, 2, message_actuator_get_act3(message.payload));
emit valueChanged(this->getUASID(), "Left Servo", message_actuator_get_act3(message.payload), MG::TIME::getGroundTimeNow());
emit actuatorChanged(this, 3, message_actuator_get_act4(message.payload));
emit valueChanged(this->getUASID(), "Right Servo", message_actuator_get_act4(message.payload), MG::TIME::getGroundTimeNow());
// Calculate thrust sum
thrustSum = (message_actuator_get_act1(message.payload) + message_actuator_get_act2(message.payload)) / 2;
emit thrustChanged(this, thrustSum);
break;
case MAVLINK_MSG_ID_STATE:
emit valueChanged(uasId, "State", message_state_get_state(message.payload), MG::TIME::getGroundTimeNow());
getStatusForCode(message_state_get_state(message.payload), uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
break;
case MAVLINK_MSG_ID_POSITION:
// Position message is displayed in meters and degrees
{
quint64 time = MG::TIME::getGroundTimeNow();//message_position_get_msec(message.payload);
emit valueChanged(this, "x", message_position_get_posX(message.payload), time);
emit valueChanged(this, "y", message_position_get_posY(message.payload), time);
emit valueChanged(this, "z", message_position_get_posZ(message.payload), time);
emit valueChanged(this, "roll", message_position_get_roll(message.payload), time);
emit valueChanged(this, "pitch", message_position_get_pitch(message.payload), time);
emit valueChanged(this, "yaw", message_position_get_yaw(message.payload), time);
emit valueChanged(this, "x speed", message_position_get_speedX(message.payload), time);
emit valueChanged(this, "y speed", message_position_get_speedY(message.payload), time);
emit valueChanged(this, "z speed", message_position_get_speedZ(message.payload), time);
emit valueChanged(this, "roll speed", message_position_get_speedRoll(message.payload), time);
emit valueChanged(this, "pitch speed", message_position_get_speedPitch(message.payload), time);
emit valueChanged(this, "yaw speed", message_position_get_speedYaw(message.payload), time);
emit localPositionChanged(this, message_position_get_posX(message.payload), message_position_get_posY(message.payload), message_position_get_posZ(message.payload), time);
emit valueChanged(uasId, "x", message_position_get_posX(message.payload), time);
emit valueChanged(uasId, "y", message_position_get_posY(message.payload), time);
emit valueChanged(uasId, "z", message_position_get_posZ(message.payload), time);
emit valueChanged(uasId, "roll", message_position_get_roll(message.payload), time);
emit valueChanged(uasId, "pitch", message_position_get_pitch(message.payload), time);
emit valueChanged(uasId, "yaw", message_position_get_yaw(message.payload), time);
emit valueChanged(uasId, "x speed", message_position_get_speedX(message.payload), time);
emit valueChanged(uasId, "y speed", message_position_get_speedY(message.payload), time);
emit valueChanged(uasId, "z speed", message_position_get_speedZ(message.payload), time);
emit valueChanged(uasId, "roll speed", message_position_get_speedRoll(message.payload), time);
emit valueChanged(uasId, "pitch speed", message_position_get_speedPitch(message.payload), time);
emit valueChanged(uasId, "yaw speed", message_position_get_speedYaw(message.payload), time);
}
break;
case MAVLINK_MSG_ID_MARKER:
emit valueChanged(uasId, "marker confidence", message_marker_get_confidence(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker x", message_marker_get_posX(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker y", message_marker_get_posY(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker z", message_marker_get_posZ(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker roll", message_marker_get_roll(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker pitch", message_marker_get_pitch(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "marker yaw", message_marker_get_yaw(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow());
break;
case MAVLINK_MSG_ID_IMAGE:
//std::cerr << std::endl;
//std::cerr << "IMAGE DATA RECEIVED" << std::endl;
emit imageStarted(message_image_get_rid(message.payload), message_image_get_width(message.payload), message_image_get_height(message.payload), message_image_get_depth(message.payload), message_image_get_channels(message.payload));
break;
case MAVLINK_MSG_ID_BYTES:
//std::cerr << std::endl;
//std::cerr << "BYTES RESSOURCE RECEIVED" << std::endl;
emit imageDataReceived(message_bytestream_get_rid(message.payload), message_bytestream_get_data(message.payload), message_bytestream_get_length(message.payload), message_bytestream_get_index(message.payload));
break;
raw_imu_t raw;
message_raw_imu_decode(&message, &raw);
quint64 time = raw.msec;
if (time == 0)
{
time = MG::TIME::getGroundTimeNow();
}
else
{
if (onboardTimeOffset == 0)
{
onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
}
time += onboardTimeOffset;
}
emit valueChanged(uasId, "Accel. X", raw.xacc, time);
emit valueChanged(uasId, "Accel. Y", raw.yacc, time);
emit valueChanged(uasId, "Accel. Z", raw.zacc, time);
emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time);
emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time);
emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time);
emit valueChanged(uasId, "Mag. X", raw.xmag, time);
emit valueChanged(uasId, "Mag. Y", raw.ymag, time);
emit valueChanged(uasId, "Mag. Z", raw.zmag, time);
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
}
break;
case MAVLINK_MSG_ID_ATTITUDE:
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << message_attitude_get_roll(message.payload) << " pitch: " << message_attitude_get_pitch(message.payload) << " yaw: " << message_attitude_get_yaw(message.payload) << std::endl;
{
quint64 time = message_attitude_get_msec(&message);
if (time == 0)
{
time = MG::TIME::getGroundTimeNow();
}
else
{
if (onboardTimeOffset == 0)
{
onboardTimeOffset = MG::TIME::getGroundTimeNow() - time;
}
time += onboardTimeOffset;
}
emit valueChanged(uasId, "roll IMU", message_attitude_get_roll(&message), time);
emit valueChanged(uasId, "pitch IMU", message_attitude_get_pitch(&message), time);
emit valueChanged(uasId, "yaw IMU", message_attitude_get_yaw(&message), time);
emit valueChanged(this, "roll IMU", message_attitude_get_roll(&message), time);
emit valueChanged(this, "pitch IMU", message_attitude_get_pitch(&message), time);
emit valueChanged(this, "yaw IMU", message_attitude_get_yaw(&message), time);
emit attitudeChanged(this, message_attitude_get_roll(&message), message_attitude_get_pitch(&message), message_attitude_get_yaw(&message), time);
}
break;
/*
case MAVLINK_MSG_ID_DEBUG:
emit valueChanged(uasId, QString("debug ") + QString::number(message_debug_get_index(message.payload)), message_debug_get_value(message.payload), MG::TIME::getGroundTimeNow());
break;
case MAVLINK_MSG_ID_MODE:
{
switch(static_cast<int>(message_mode_get_mode(message.payload)))
{
case (int)MAV_MODE_MANUAL:
{
uasIsAuto = false;
}
break;
case (int)MAV_MODE_AUTO:
{
uasIsAuto = true;
}
break;
default:
{
uasIsAuto = false;
}
break;
}
emit autoModeChanged(mode);
emit valueChanged(uasId, "auto mode", static_cast<int>(message_mode_get_mode(message.payload)), MG::TIME::getGroundTimeNow());
//qDebug() << "UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO, UAS MODE CHANGED TO AUTO:" << uasIsAuto;
}
break;
case MAVLINK_MSG_ID_EMITWAYPT:
emit waypointUpdated(this->getUASID(), message_emitwaypoint_get_id(message.payload), message_emitwaypoint_get_x(message.payload), message_emitwaypoint_get_y(message.payload), message_emitwaypoint_get_z(message.payload), message_emitwaypoint_get_yaw(message.payload), (message_emitwaypoint_get_autocontinue(message.payload) == 1 ? true : false), (message_emitwaypoint_get_active(message.payload) == 1 ? true : false));
break;
case MAVLINK_MSG_ID_GOTOWAYPT:
emit valueChanged(uasId, "WP X", message_gotowaypoint_get_x(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP Y", message_gotowaypoint_get_y(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP Z", message_gotowaypoint_get_z(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP speed X", message_gotowaypoint_get_speedX(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP speed Y", message_gotowaypoint_get_speedY(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP speed Z", message_gotowaypoint_get_speedZ(message.payload), MG::TIME::getGroundTimeNow());
emit valueChanged(uasId, "WP yaw", message_gotowaypoint_get_yaw(message.payload)/M_PI*180.0f, MG::TIME::getGroundTimeNow());
break;
case MAVLINK_MSG_ID_WP_REACHED:
qDebug() << "WAYPOINT REACHED";
emit waypointReached(this, message_wp_reached_get_id(message.payload));
break;
case MAVLINK_MSG_ID_DETECTION:
patternPath = QString(message_detection_get_patternpath(message.payload));
//qDebug() << "OBJECT DETECTED";
emit detectionReceived(uasId, patternPath, 0, 0, 0, 0, 0, 0, 0, 0, message_detection_get_confidence(message.payload), (message_detection_get_detected(message.payload) == 1 ? true : false ));
break;
default:
std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
//qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast<int>(message.acid) << " with message id:" << static_cast<int>(message.msgid) << std::endl;
break;
*/
}
}
// virtual void attitudeChanged(int uasId, double roll, double pitch, double yaw, quint64 usec);
// virtual void localPositionChanged(int uasId, double x, double y, double z, quint64 usec);
// virtual void globalPositionChanged(int uasId, double lon, double lat, double alt, quint64 usec);
// virtual void actuatorsChanged(int uasId, double actuator1, double actuator2, double actuator3, double actuator4, double actuator5);
// virtual void voltageChanged(int uasId, double voltage);
}
void UAS::setAutoMode(bool autoMode)
{
mavlink_message_t msg;
if (autoMode)
{
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)MAV_MODE_AUTO);
mode = MAV_MODE_AUTO;
sendMessage(msg);
}
else
{
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)MAV_MODE_MANUAL);
mode = MAV_MODE_MANUAL;
sendMessage(msg);
}
}
if (mode >= MAV_MODE_LOCKED && mode <= MAV_MODE_TEST3)
{
this->mode = mode;
mavlink_message_t msg;
message_set_mode_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, getUASID(), (unsigned char)mode);
sendMessage(msg);
}
393
394
395
396
397
398
399
400
401
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
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
}
void UAS::sendMessage(mavlink_message_t message)
{
qDebug() << "In send message";
// Emit message on all links that are currently connected
QList<LinkInterface*>::iterator i;
qDebug() << "LINKS: " << links->length();
for (i = links->begin(); i != links->end(); ++i)
{
// if (i != NULL)
// {
qDebug() << "UAS::sendMessage()";
sendMessage(*i, message);
}
}
void UAS::sendMessage(LinkInterface* link, mavlink_message_t message)
{
qDebug() << "UAS::sendMessage(link, message)";
// Create buffer
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// Write message into buffer, prepending start sign
int len = message_to_send_buffer(buffer, &message);
// If link is connected
if (link->isConnected())
{
// Send the portion of the buffer now occupied by the message
link->writeBytes((const char*)buffer, len);
}
}
float UAS::filterVoltage()
{
return lpVoltage;
}
/**
* @param value battery voltage
*/
float UAS::filterVoltage(float value)
{
/*
currentVoltage = value;
static QList<float> voltages<float>(20);
const int dropPercent = 20;
voltages.takeFirst();
voltages.append(value);
// Drop top and bottom xx percent of values
QList<float> v(voltages);
qSort(v);
v = QList::mid(v.size()/dropPercent, v.size() - v.size()/dropPercent);
lpVoltage = 0.0f;
foreach (float value, v)
{
lpVoltage += v;
}*/
return currentVoltage;
}
void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription)
{
switch (statusCode)
{
case MAV_STATE_UNINIT:
uasState = tr("UNINIT");
stateDescription = tr("Not initialized");
break;
case MAV_STATE_BOOT:
uasState = tr("BOOT");
stateDescription = tr("Booting system, please wait..");
break;
case MAV_STATE_CALIBRATING:
uasState = tr("CALIBRATING");
stateDescription = tr("Calibrating sensors..");
break;
case MAV_STATE_ACTIVE:
uasState = tr("ACTIVE");
stateDescription = tr("Normal operation mode");
break;
case MAV_STATE_CRITICAL:
uasState = tr("CRITICAL");
stateDescription = tr("Failure occured!");
break;
case MAV_STATE_EMERGENCY:
uasState = tr("EMERGENCY");
stateDescription = tr("EMERGENCY: Please land");
break;
case MAV_STATE_POWEROFF:
uasState = tr("SHUTDOWN");
stateDescription = tr("Powering off system");
break;
default:
uasState = tr("UNKNOWN");
stateDescription = tr("FAILURE: Unknown system state");
break;
}
}
/* MANAGEMENT */
/*
*
* @return The uptime in milliseconds
*
**/
quint64 UAS::getUptime()
{
if(startTime == 0) {
return 0;
} else {
return MG::TIME::getGroundTimeNow() - startTime;
}
}
int UAS::getCommunicationStatus()
{
return commStatus;
}
void UAS::requestWaypoints()
{
mavlink_message_t message;
//messagePackGetWaypoints(MG::SYSTEM::ID, &message); FIXME
sendMessage(message);
qDebug() << "UAS Request WPs";
}
/**
* @brief Launches the system
*
**/
void UAS::launch()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_LAUNCH);
sendMessage(message);
qDebug() << "UAS LAUNCHED!";
//emit commandSent(LAUNCH);
}
/**
* Depending on the UAS, this might make the rotors of a helicopter spinning
*
**/
void UAS::enable_motors()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_START);
sendMessage(message);
}
/**
* @warning Depending on the UAS, this might completely stop all motors.
*
**/
void UAS::disable_motors()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(),(int)MAV_ACTION_MOTORS_STOP);
sendMessage(message);
}
void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust)
{
// Scale values
double rollPitchScaling = 0.2f;
double yawScaling = 0.5f;
double thrustScaling = 1.0f;
manualRollAngle = roll * rollPitchScaling;
manualPitchAngle = pitch * rollPitchScaling;
manualYawAngle = yaw * yawScaling;
manualThrust = thrust * thrustScaling;
if(mode == MAV_MODE_MANUAL)
{
mavlink_message_t message;
message_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual);
sendMessage(message);
qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust;
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow());
}
}
void UAS::receiveButton(int buttonIndex)
{
switch (buttonIndex)
{
case 0:
setAutoMode(false);
break;
case 1:
setAutoMode(true);
break;
default:
break;
}
qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!";
}
void UAS::setWaypoint(Waypoint* wp)
{
mavlink_message_t message;
// FIXME
//messagePackSetWaypoint(MG::SYSTEM::ID, &message, wp->id, wp->x, wp->y, wp->z, wp->yaw, (wp->autocontinue ? 1 : 0));
sendMessage(message);
qDebug() << "UAS SENT Waypoint " << wp->id;
}
void UAS::setWaypointActive(int id)
{
mavlink_message_t message;
// FIXME
//messagePackChooseWaypoint(MG::SYSTEM::ID, &message, id);
sendMessage(message);
// TODO This should be not directly emitted, but rather being fed back from the UAS
emit waypointSelected(getUASID(), id);
}
void UAS::halt()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_HALT);
sendMessage(message);
}
void UAS::go()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_CONTINUE);
sendMessage(message);
}
/** Order the robot to return home / to land on the runway **/
void UAS::home()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_RETURN);
sendMessage(message);
}
/**
* The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation
* and might differ between systems.
*/
void UAS::emergencySTOP()
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_LAND);
}
/**
* All systems are immediately shut down (e.g. the main power line is cut).
* @warning This might lead to a crash
*
* The command will not be executed until emergencyKILLConfirm is issues immediately afterwards
*/
bool UAS::emergencyKILL()
{
bool result = false;
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS");
msgBox.setInformativeText("Do you want to cut power on all systems?");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
msgBox.setDefaultButton(QMessageBox::Cancel);
int ret = msgBox.exec();
// Close the message box shortly after the click to prevent accidental clicks
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
if (ret == QMessageBox::Yes)
{
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_EMCY_KILL);
sendMessage(message);
result = true;
}
return result;
}
void UAS::shutdown()
{
bool result = false;
QMessageBox msgBox;
msgBox.setIcon(QMessageBox::Critical);
msgBox.setText("Shutting down the UAS");
msgBox.setInformativeText("Do you want to shut down the onboard computer?");
msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel);
msgBox.setDefaultButton(QMessageBox::Cancel);
int ret = msgBox.exec();
// Close the message box shortly after the click to prevent accidental clicks
QTimer::singleShot(5000, &msgBox, SLOT(reject()));
if (ret == QMessageBox::Yes)
{
// If the active UAS is set, execute command
mavlink_message_t message;
// TODO Replace MG System ID with static function call and allow to change ID in GUI
message_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->getUASID(), (int)MAV_ACTION_SHUTDOWN);
sendMessage(message);
result = true;
}
}
/**
* @return The name of this system as string in human-readable form
*/
QString UAS::getUASName()
{
QString result;
if (name == "")
{
result = tr("MAV ") + result.sprintf("%03d", getUASID());
}
else
{
result = name;
}
return result;
}
void UAS::addLink(LinkInterface* link)
{
if (!links->contains(link))
{
links->append(link);
}
//links->append(link);
//qDebug() << " ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK";
}
/**
* @brief Get the links associated with this robot
*
**/
QList<LinkInterface*>* UAS::getLinks()
{
return links;
}
void UAS::setBattery(BatteryType type, int cells)
{
this->batteryType = type;
this->cells = cells;
switch (batteryType)
{
case NICD:
break;
case NIMH:
break;
case LIION:
break;
case LIPOLY:
fullVoltage = this->cells * UAS::lipoFull;
emptyVoltage = this->cells * UAS::lipoEmpty;
break;
case LIFE:
break;
case AGZN:
break;
}
}
int UAS::calculateTimeRemaining()
{
quint64 dt = MG::TIME::getGroundTimeNow() - startTime;
double seconds = dt / 1000.0f;
double voltDifference = startVoltage - currentVoltage;
if (voltDifference <= 0) voltDifference = 0.00000000001f;
double dischargePerSecond = voltDifference / seconds;
int remaining = static_cast<int>((currentVoltage - emptyVoltage) / dischargePerSecond);
// Can never be below 0
if (remaining < 0) remaining = 0;
return remaining;
}
double UAS::getChargeLevel()
{
return 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage));
}
void UAS::clearWaypointList()
{
mavlink_message_t message;
// FIXME
//messagePackRemoveWaypoints(MG::SYSTEM::ID, &message);
sendMessage(message);
qDebug() << "UAS clears Waypoints!";
}