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
#include "AccelCalibrationConfig.h"
AccelCalibrationConfig::AccelCalibrationConfig(QWidget *parent) : AP2ConfigWidget(parent)
{
ui.setupUi(this);
connect(ui.calibrateAccelButton,SIGNAL(clicked()),this,SLOT(calibrateButtonClicked()));
connect(UASManager::instance(),SIGNAL(activeUASSet(UASInterface*)),this,SLOT(activeUASSet(UASInterface*)));
activeUASSet(UASManager::instance()->getActiveUAS());
m_accelAckCount=0;
}
AccelCalibrationConfig::~AccelCalibrationConfig()
{
}
void AccelCalibrationConfig::activeUASSet(UASInterface *uas)
{
if (m_uas)
{
disconnect(m_uas,SIGNAL(textMessageReceived(int,int,int,QString)),this,SLOT(uasTextMessageReceived(int,int,int,QString)));
}
AP2ConfigWidget::activeUASSet(uas);
if (!uas)
{
return;
}
connect(m_uas,SIGNAL(textMessageReceived(int,int,int,QString)),this,SLOT(uasTextMessageReceived(int,int,int,QString)));
}
void AccelCalibrationConfig::calibrateButtonClicked()
{
if (!m_uas)
{
showNullMAVErrorMessageBox();
return;
}
if (m_accelAckCount == 0)
{
MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
int confirm = 0;
float param1 = 0.0;
float param2 = 0.0;
float param3 = 0.0;
float param4 = 0.0;
float param5 = 1.0;
float param6 = 0.0;
float param7 = 0.0;
int component = 1;
m_uas->executeCommand(command, confirm, param1, param2, param3, param4, param5, param6, param7, component);
}
else if (m_accelAckCount <= 5)
{
m_uas->executeCommandAck(m_accelAckCount++,true);
}
else
{
m_uas->executeCommandAck(m_accelAckCount++,true);
ui.calibrateAccelButton->setText("Calibrate\nAccelerometer");
}
}
void AccelCalibrationConfig::uasTextMessageReceived(int uasid, int componentid, int severity, QString text)
{
//command received: " Severity 1
//Place APM Level and press any key" severity 5
if (severity == 5)
{
//This is a calibration instruction
if (m_accelAckCount == 0)
{
//Calibration Sucessful\r"
ui.calibrateAccelButton->setText("Any\nKey");
m_accelAckCount++;
}
if (m_accelAckCount == 7)
{
//All finished
//ui.outputLabel->setText(ui.outputLabel->text() + "\n" + text);
ui.outputLabel->setText(text);
m_accelAckCount++;
}
if (m_accelAckCount == 8)
{
if (text.contains("Calibration") && text.contains("successful"))
{
m_accelAckCount = 0;
}
ui.outputLabel->setText(ui.outputLabel->text() + "\n" + text);
}
else
{
ui.outputLabel->setText(text);
}
}
}