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
116
117
118
119
120
121
122
/****************************************************************************
*
* (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
*
* QGroundControl is licensed according to the terms in the file
* COPYING.md in the root of the source code directory.
*
****************************************************************************/
#include "APMSensorsComponent.h"
#include "APMAutoPilotPlugin.h"
#include "APMSensorsComponentController.h"
#include "APMAirframeComponent.h"
#include "ParameterManager.h"
// These two list must be kept in sync
APMSensorsComponent::APMSensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) :
VehicleComponent(vehicle, autopilot, parent),
_name(tr("Sensors"))
{
}
QString APMSensorsComponent::name(void) const
{
return _name;
}
QString APMSensorsComponent::description(void) const
{
return tr("Sensors Setup is used to calibrate the sensors within your vehicle.");
}
QString APMSensorsComponent::iconResource(void) const
{
return "/qmlimages/SensorsComponentIcon.png";
}
bool APMSensorsComponent::requiresSetup(void) const
{
return true;
}
bool APMSensorsComponent::setupComplete(void) const
{
return !compassSetupNeeded() && !accelSetupNeeded();
}
QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
{
QStringList triggers;
// Compass triggers
triggers << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3")
<< QStringLiteral("COMPASS_USE") << QStringLiteral("COMPASS_USE2") << QStringLiteral("COMPASS_USE3")
<< QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_X")
<< QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_X")
<< QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_X");
// Accelerometer triggers
triggers << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");
return triggers;
}
QUrl APMSensorsComponent::setupSource(void) const
{
return QUrl::fromUserInput("qrc:/qml/APMSensorsComponent.qml");
}
QUrl APMSensorsComponent::summaryQmlSource(void) const
{
return QUrl::fromUserInput("qrc:/qml/APMSensorsComponentSummary.qml");
}
bool APMSensorsComponent::compassSetupNeeded(void) const
{
const size_t cCompass = 3;
const size_t cOffset = 3;
QStringList rgDevicesIds;
QStringList rgCompassUse;
QStringList rgOffsets[cCompass];
rgDevicesIds << QStringLiteral("COMPASS_DEV_ID") << QStringLiteral("COMPASS_DEV_ID2") << QStringLiteral("COMPASS_DEV_ID3");
rgCompassUse << QStringLiteral("COMPASS_USE") << QStringLiteral("COMPASS_USE2") << QStringLiteral("COMPASS_USE3");
rgOffsets[0] << QStringLiteral("COMPASS_OFS_X") << QStringLiteral("COMPASS_OFS_Y") << QStringLiteral("COMPASS_OFS_Z");
rgOffsets[1] << QStringLiteral("COMPASS_OFS2_X") << QStringLiteral("COMPASS_OFS2_Y") << QStringLiteral("COMPASS_OFS2_Z");
rgOffsets[2] << QStringLiteral("COMPASS_OFS3_X") << QStringLiteral("COMPASS_OFS3_Y") << QStringLiteral("COMPASS_OFS3_Z");
for (size_t i=0; i<cCompass; i++) {
if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgDevicesIds[i])->rawValue().toInt() != 0 &&
_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgCompassUse[i])->rawValue().toInt() != 0) {
for (size_t j=0; j<cOffset; j++) {
if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
return true;
}
}
}
}
return false;
}
bool APMSensorsComponent::accelSetupNeeded(void) const
{
QStringList rgOffsets;
// The best we can do is test the first accel which will always be there. We don't have enough information to know
// whether any of the other accels are available.
rgOffsets << QStringLiteral("INS_ACCOFFS_X") << QStringLiteral("INS_ACCOFFS_Y") << QStringLiteral("INS_ACCOFFS_Z");
int zeroCount = 0;
for (int i=0; i<rgOffsets.count(); i++) {
if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, rgOffsets[i])->rawValue().toFloat() == 0.0f) {
zeroCount++;
}
}
return zeroCount == rgOffsets.count();
}