Skip to content

Commit ae079ed

Browse files
HTRamseyHTRamsey
committed
Vehicle: Add Terrain Altitude Warning
1 parent 1f3f2bf commit ae079ed

File tree

9 files changed

+118
-5
lines changed

9 files changed

+118
-5
lines changed

‎src/FlightDisplay/VehicleWarnings.qml‎

Lines changed: 36 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,18 +12,28 @@ import QtQuick
1212
import QGroundControl
1313
import QGroundControl.ScreenTools
1414
import QGroundControl.Controls
15+
import QGroundControl.FactSystem
1516

1617
Rectangle {
1718
anchors.margins: -ScreenTools.defaultFontPixelHeight
1819
height: warningsCol.height
1920
width: warningsCol.width
2021
color: Qt.rgba(1, 1, 1, 0.5)
2122
radius: ScreenTools.defaultFontPixelWidth / 2
22-
visible: _noGPSLockVisible || _prearmErrorVisible
23+
visible: _noGPSLockVisible || _prearmErrorVisible || (_showAltitudeWarning && (_vehicleAltitudeBelowMin || _vehicleAltitudeAboveMax || !_terrainDataAvailable.value))
2324

2425
property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
2526
property bool _noGPSLockVisible: _activeVehicle && _activeVehicle.requiresGpsFix && !_activeVehicle.coordinate.isValid
26-
property bool _prearmErrorVisible: _activeVehicle && !_activeVehicle.armed && _activeVehicle.prearmError && !_activeVehicle.healthAndArmingCheckReport.supported
27+
property bool _prearmErrorVisible: _activeVehicle && !_activeVehicle.armed && _activeVehicle.prearmError
28+
29+
property Fact _altitudeWarnThresholdEnabled: QGroundControl.settingsManager.flyViewSettings.altitudeWarnThresholdEnabled
30+
property Fact _altitudeWarnMinAGL: QGroundControl.settingsManager.flyViewSettings.altitudeWarnMinAGL
31+
property Fact _altitudeWarnMaxAGL: QGroundControl.settingsManager.flyViewSettings.altitudeWarnMaxAGL
32+
property Fact _altitudeAboveTerrain: _activeVehicle ? _activeVehicle.altitudeAboveTerr : null
33+
property Fact _terrainDataAvailable: _activeVehicle ? _activeVehicle.terrainDataAvailable : null
34+
property bool _vehicleAltitudeBelowMin: _altitudeAboveTerrain ? (_altitudeAboveTerrain.value < _altitudeWarnMinAGL.value) : false
35+
property bool _vehicleAltitudeAboveMax: _altitudeAboveTerrain ? (_altitudeAboveTerrain.value > _altitudeWarnMaxAGL.value) : false
36+
property bool _showAltitudeWarning: _activeVehicle && _activeVehicle.flying && !_activeVehicle.landing && _altitudeWarnThresholdEnabled.value
2737

2838
Column {
2939
id: warningsCol
@@ -45,6 +55,30 @@ Rectangle {
4555
text: _activeVehicle ? _activeVehicle.prearmError : ""
4656
}
4757

58+
QGCLabel {
59+
anchors.horizontalCenter: parent.horizontalCenter
60+
visible: _showAltitudeWarning && _terrainDataAvailable.value && _vehicleAltitudeBelowMin
61+
color: "black"
62+
font.pointSize: ScreenTools.largeFontPointSize
63+
text: visible ? qsTr("Altitude below minimum threshold of %1 %2 above terrain: (%3 %4)").arg(_altitudeWarnMinAGL.value).arg(_altitudeWarnMinAGL.units).arg(_altitudeAboveTerrain.value.toFixed(2)).arg(_altitudeAboveTerrain.units) : ""
64+
}
65+
66+
QGCLabel {
67+
anchors.horizontalCenter: parent.horizontalCenter
68+
visible: _showAltitudeWarning && _terrainDataAvailable.value && _vehicleAltitudeAboveMax
69+
color: "black"
70+
font.pointSize: ScreenTools.largeFontPointSize
71+
text: visible ? qsTr("Altitude above maximum threshold of %1 %2 above terrain: (%3 %4)").arg(_altitudeWarnMaxAGL.value).arg(_altitudeWarnMaxAGL.units).arg(_altitudeAboveTerrain.value.toFixed(2)).arg(_altitudeAboveTerrain.units) : ""
72+
}
73+
74+
QGCLabel {
75+
anchors.horizontalCenter: parent.horizontalCenter
76+
visible: _showAltitudeWarning && !_terrainDataAvailable.value
77+
color: "black"
78+
font.pointSize: ScreenTools.largeFontPointSize
79+
text: qsTr("Terrain data not available at vehicle pos, altitude warning thresholds are disabled")
80+
}
81+
4882
QGCLabel {
4983
anchors.horizontalCenter: parent.horizontalCenter
5084
visible: _prearmErrorVisible

‎src/Settings/FlyView.SettingsGroup.json‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,33 @@
102102
"min": 3,
103103
"max": 60,
104104
"default": 10
105+
},
106+
{
107+
"name": "altitudeWarnThresholdEnabled",
108+
"shortDesc": "Altitude warning threshold enabled",
109+
"longDesc": "If this option is enabled, warn user if drone flies above or below altitude threshold.",
110+
"type": "bool",
111+
"default": false
112+
},
113+
{
114+
"name": "altitudeWarnMinAGL",
115+
"shortDesc": "Minimum altitude AGL for warning",
116+
"longDesc": "Minimum altitude warning threshold in meters above ground level.",
117+
"type": "double",
118+
"default": 2.0,
119+
"min": 0.0,
120+
"units": "m",
121+
"decimalPlaces": 1
122+
},
123+
{
124+
"name": "altitudeWarnMaxAGL",
125+
"shortDesc": "Minimum altitude AGL for warning",
126+
"longDesc": "Minimum altitude warning threshold in meters above ground level.",
127+
"type": "double",
128+
"default": 121.92,
129+
"min": 0.0,
130+
"units": "m",
131+
"decimalPlaces": 1
105132
}
106133
]
107134
}

‎src/Settings/FlyViewSettings.cc‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,3 +31,6 @@ DECLARE_SETTINGSFACT(FlyViewSettings, updateHomePosition)
3131
DECLARE_SETTINGSFACT(FlyViewSettings, instrumentQmlFile2)
3232
DECLARE_SETTINGSFACT(FlyViewSettings, requestControlAllowTakeover)
3333
DECLARE_SETTINGSFACT(FlyViewSettings, requestControlTimeout)
34+
DECLARE_SETTINGSFACT(FlyViewSettings, altitudeWarnThresholdEnabled)
35+
DECLARE_SETTINGSFACT(FlyViewSettings, altitudeWarnMinAGL)
36+
DECLARE_SETTINGSFACT(FlyViewSettings, altitudeWarnMaxAGL)

‎src/Settings/FlyViewSettings.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,4 +34,7 @@ class FlyViewSettings : public SettingsGroup
3434
DEFINE_SETTINGFACT(instrumentQmlFile2)
3535
DEFINE_SETTINGFACT(requestControlAllowTakeover)
3636
DEFINE_SETTINGFACT(requestControlTimeout)
37+
DEFINE_SETTINGFACT(altitudeWarnThresholdEnabled)
38+
DEFINE_SETTINGFACT(altitudeWarnMinAGL)
39+
DEFINE_SETTINGFACT(altitudeWarnMaxAGL)
3740
};

‎src/UI/AppSettings/FlyViewSettings.qml‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -114,6 +114,34 @@ SettingsPage {
114114
}
115115
}
116116

117+
SettingsGroupLayout {
118+
Layout.fillWidth: true
119+
heading: qsTr("Altitude above ground level warning threshold")
120+
121+
FactCheckBoxSlider {
122+
Layout.fillWidth: true
123+
text: qsTr("Altitude warning enabled")
124+
visible: fact.visible
125+
fact: _flyViewSettings.altitudeWarnThresholdEnabled
126+
}
127+
128+
LabelledFactTextField {
129+
Layout.fillWidth: true
130+
label: qsTr("Minimum Altitude")
131+
fact: _flyViewSettings.altitudeWarnMinAGL
132+
visible: fact.visible
133+
enabled: _flyViewSettings.altitudeWarnThresholdEnabled.value
134+
}
135+
136+
LabelledFactTextField {
137+
Layout.fillWidth: true
138+
label: qsTr("Maximum Altitude")
139+
fact: _flyViewSettings.altitudeWarnMaxAGL
140+
visible: fact.visible
141+
enabled: _flyViewSettings.altitudeWarnThresholdEnabled.value
142+
}
143+
}
144+
117145
SettingsGroupLayout {
118146
Layout.fillWidth: true
119147
heading: qsTr("Guided Commands")

‎src/Vehicle/FactGroups/VehicleFact.json‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -87,6 +87,12 @@
8787
"decimalPlaces": 1,
8888
"units": "m"
8989
},
90+
{
91+
"name": "terrainDataAvailable",
92+
"shortDesc": "Terrain Data Available",
93+
"type": "bool",
94+
"defaultValue": false
95+
},
9096
{
9197
"name": "flightDistance",
9298
"shortDesc": "Flight Distance",

‎src/Vehicle/FactGroups/VehicleFactGroup.cc‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ VehicleFactGroup::VehicleFactGroup(QObject *parent)
3030
_addFact(&_altitudeRelativeFact);
3131
_addFact(&_altitudeAMSLFact);
3232
_addFact(&_altitudeAboveTerrFact);
33+
_addFact(&_terrainDataAvailableFact);
3334
_addFact(&_altitudeTuningFact);
3435
_addFact(&_altitudeTuningSetpointFact);
3536
_addFact(&_xTrackErrorFact);

‎src/Vehicle/FactGroups/VehicleFactGroup.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@ class VehicleFactGroup : public FactGroup
2727
Q_PROPERTY(Fact *altitudeRelative READ altitudeRelative CONSTANT)
2828
Q_PROPERTY(Fact *altitudeAMSL READ altitudeAMSL CONSTANT)
2929
Q_PROPERTY(Fact *altitudeAboveTerr READ altitudeAboveTerr CONSTANT)
30+
Q_PROPERTY(Fact *terrainDataAvailable READ terrainDataAvailable CONSTANT)
3031
Q_PROPERTY(Fact *altitudeTuning READ altitudeTuning CONSTANT)
3132
Q_PROPERTY(Fact *altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT)
3233
Q_PROPERTY(Fact *xTrackError READ xTrackError CONSTANT)
@@ -61,6 +62,7 @@ class VehicleFactGroup : public FactGroup
6162
Fact *altitudeRelative() { return &_altitudeRelativeFact; }
6263
Fact *altitudeAMSL() { return &_altitudeAMSLFact; }
6364
Fact *altitudeAboveTerr() { return &_altitudeAboveTerrFact; }
65+
Fact *terrainDataAvailable() { return &_terrainDataAvailableFact; }
6466
Fact *altitudeTuning() { return &_altitudeTuningFact; }
6567
Fact *altitudeTuningSetpoint() { return &_altitudeTuningSetpointFact; }
6668
Fact *xTrackError() { return &_xTrackErrorFact; }
@@ -105,6 +107,7 @@ class VehicleFactGroup : public FactGroup
105107
Fact _altitudeRelativeFact = Fact(0, QStringLiteral("altitudeRelative"), FactMetaData::valueTypeDouble);
106108
Fact _altitudeAMSLFact = Fact(0, QStringLiteral("altitudeAMSL"), FactMetaData::valueTypeDouble);
107109
Fact _altitudeAboveTerrFact = Fact(0, QStringLiteral("altitudeAboveTerr"), FactMetaData::valueTypeDouble);
110+
Fact _terrainDataAvailableFact = Fact(0, QStringLiteral("terrainDataAvailable"), FactMetaData::valueTypeBool);
108111
Fact _altitudeTuningFact = Fact(0, QStringLiteral("altitudeTuning"), FactMetaData::valueTypeDouble);
109112
Fact _altitudeTuningSetpointFact = Fact(0, QStringLiteral("altitudeTuningSetpoint"), FactMetaData::valueTypeDouble);
110113
Fact _xTrackErrorFact = Fact(0, QStringLiteral("xTrackError"), FactMetaData::valueTypeDouble);

‎src/Vehicle/Vehicle.cc‎

Lines changed: 11 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -3766,11 +3766,19 @@ void Vehicle::_updateAltAboveTerrain()
37663766
void Vehicle::_altitudeAboveTerrainReceived(bool success, QList<double> heights)
37673767
{
37683768
if (!success) {
3769-
qCDebug(VehicleLog) << "_altitudeAboveTerrainReceived: terrain data not available for vehicle coordinate";
3769+
qCDebug(VehicleLog) << "terrain data not available for vehicle coordinate";
3770+
if (_coordinate.isValid() &&
3771+
SettingsManager::instance()->flyViewSettings()->altitudeWarnThresholdEnabled()->rawValue().toBool() && _flying) {
3772+
_terrainDataAvailableFact.setRawValue(false);
3773+
}
3774+
_altitudeAboveTerrFact.setRawValue(qQNaN()); // Reset the altitude above terrain fact to NaN
37703775
} else {
3776+
if (!_terrainDataAvailableFact.rawValue().toBool()) {
3777+
_terrainDataAvailableFact.setRawValue(true);
3778+
}
37713779
// Query was succesful, save the data.
3772-
double terrainAltitude = heights[0];
3773-
double altitudeAboveTerrain = altitudeAMSL()->rawValue().toDouble() - terrainAltitude;
3780+
const double terrainAltitude = heights[0];
3781+
const double altitudeAboveTerrain = altitudeAMSL()->rawValue().toDouble() - terrainAltitude;
37743782
_altitudeAboveTerrFact.setRawValue(altitudeAboveTerrain);
37753783
}
37763784
// Clean up

0 commit comments

Comments
 (0)