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
/****************************************************************************
*
* (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 "PX4GeoFenceManager.h"
#include "Vehicle.h"
#include "FirmwarePlugin.h"
#include "ParameterManager.h"
PX4GeoFenceManager::PX4GeoFenceManager(Vehicle* vehicle)
: GeoFenceManager(vehicle)
, _firstParamLoadComplete(false)
, _circleRadiusFact(NULL)
{
connect(_vehicle->getParameterManager(), &ParameterManager::parametersReady, this, &PX4GeoFenceManager::_parametersReady);
}
PX4GeoFenceManager::~PX4GeoFenceManager()
{
}
void PX4GeoFenceManager::_parametersReady(void)
{
if (!_firstParamLoadComplete) {
_firstParamLoadComplete = true;
_circleRadiusFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, QStringLiteral("GF_MAX_HOR_DIST"));
connect(_circleRadiusFact, &Fact::rawValueChanged, this, &PX4GeoFenceManager::_circleRadiusRawValueChanged);
emit circleRadiusChanged(circleRadius());
QStringList paramNames;
QStringList paramLabels;
paramNames << QStringLiteral("GF_ACTION") << QStringLiteral("GF_MAX_HOR_DIST") << QStringLiteral("GF_MAX_VER_DIST");
paramLabels << QStringLiteral("Action:") << QStringLiteral("Radius:") << QStringLiteral("Max Altitude:");
_params.clear();
_paramLabels.clear();
for (int i=0; i<paramNames.count(); i++) {
QString paramName = paramNames[i];
if (_vehicle->parameterExists(FactSystem::defaultComponentId, paramName)) {
Fact* paramFact = _vehicle->getParameterFact(FactSystem::defaultComponentId, paramName);
_params << QVariant::fromValue(paramFact);
_paramLabels << paramLabels[i];
}
}
emit paramsChanged(_params);
emit paramLabelsChanged(_paramLabels);
emit circleSupportedChanged(circleSupported());
qCDebug(GeoFenceManagerLog) << "fenceSupported:circleSupported:polygonSupported:breachReturnSupported" <<
fenceSupported() << circleSupported() << polygonSupported() << breachReturnSupported();
}
}
float PX4GeoFenceManager::circleRadius(void) const
{
if (_circleRadiusFact) {
return _circleRadiusFact->rawValue().toFloat();
} else {
return 0.0;
}
}
void PX4GeoFenceManager::_circleRadiusRawValueChanged(QVariant value)
{
emit circleRadiusChanged(value.toFloat());
emit circleSupportedChanged(circleSupported());
}
bool PX4GeoFenceManager::circleSupported(void) const
{
if (_circleRadiusFact) {
return _circleRadiusFact->rawValue().toFloat() >= 0.0;
}
return false;
}