diff --git a/qgroundcontrol.pro b/qgroundcontrol.pro
index dd2a9c8777c3a80792e4996ceb935997d947d942..7096843eddf003bd6190e2c335b9f29ead16a8af 100644
--- a/qgroundcontrol.pro
+++ b/qgroundcontrol.pro
@@ -517,6 +517,7 @@ HEADERS += \
src/Settings/AppSettings.h \
src/Settings/AutoConnectSettings.h \
src/Settings/FlightMapSettings.h \
+ src/Settings/GuidedSettings.h \
src/Settings/RTKSettings.h \
src/Settings/SettingsGroup.h \
src/Settings/SettingsManager.h \
@@ -699,6 +700,7 @@ SOURCES += \
src/Settings/AppSettings.cc \
src/Settings/AutoConnectSettings.cc \
src/Settings/FlightMapSettings.cc \
+ src/Settings/GuidedSettings.cc \
src/Settings/RTKSettings.cc \
src/Settings/SettingsGroup.cc \
src/Settings/SettingsManager.cc \
diff --git a/qgroundcontrol.qrc b/qgroundcontrol.qrc
index 098ec02e246a0d740cb10deadafdd045bc5fc776..b947baa6072c1c923e6e9d16cbb05ba3001dbd49 100644
--- a/qgroundcontrol.qrc
+++ b/qgroundcontrol.qrc
@@ -196,6 +196,7 @@
src/Settings/App.SettingsGroup.json
src/Settings/AutoConnect.SettingsGroup.json
src/Settings/FlightMap.SettingsGroup.json
+ src/Settings/Guided.SettingsGroup.json
src/Settings/RTK.SettingsGroup.json
src/MissionManager/Survey.SettingsGroup.json
src/Settings/Units.SettingsGroup.json
diff --git a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
index 4d07f66673ac31ab1ce182c16f0986e2e9d2cb13..8b6cc9f9a32a6665db1a34edb893cb1f260c2d59 100644
--- a/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
+++ b/src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
@@ -172,6 +172,20 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
{
+ QString takeoffAltParam("PILOT_TKOFF_ALT");
+
+ if (!vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, takeoffAltParam)) {
+ qgcApp()->showMessage(tr("Unable to takeoff, %1 parameter missing.").arg(takeoffAltParam));
+ return;
+ }
+ Fact* takeoffAltFact = vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, takeoffAltParam);
+ float takeoffAlt = takeoffAltFact->rawValue().toDouble();
+ if (takeoffAlt <= 0) {
+ takeoffAlt = 2.5;
+ } else {
+ takeoffAlt /= 100; // centimeters -> meters
+ }
+
if (!_armVehicleAndValidate(vehicle)) {
qgcApp()->showMessage(tr("Unable to takeoff: Vehicle failed to arm."));
return;
@@ -181,7 +195,7 @@ void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle)
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
- 2.5);
+ takeoffAlt);
}
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
@@ -203,15 +217,6 @@ void ArduCopterFirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double
return;
}
- // Don't allow altitude to fall below 3 meters above home
- double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
- if (altitudeChange <= 0 && currentAltRel <= 3) {
- return;
- }
- if (currentAltRel + altitudeChange < 3) {
- altitudeChange = 3 - currentAltRel;
- }
-
setGuidedMode(vehicle, true);
mavlink_message_t msg;
diff --git a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
index 161f8b77a6f59bc522b5e3fd900dd03c4630af83..fdcda89581bf9d3c9598849045b5823621500cc4 100644
--- a/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/PX4/PX4FirmwarePlugin.cc
@@ -435,16 +435,8 @@ void PX4FirmwarePlugin::guidedModeChangeAltitude(Vehicle* vehicle, double altitu
return;
}
- // Don't allow altitude to fall below 3 meters above home
double currentAltRel = vehicle->altitudeRelative()->rawValue().toDouble();
- double newAltRel = currentAltRel;
- if (altitudeChange <= 0 && currentAltRel <= 3) {
- return;
- }
- if (currentAltRel + altitudeChange < 3) {
- altitudeChange = 3 - currentAltRel;
- }
- newAltRel = currentAltRel + altitudeChange;
+ double newAltRel = currentAltRel + altitudeChange;
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_DO_REPOSITION,
diff --git a/src/FlightDisplay/GuidedAltitudeSlider.qml b/src/FlightDisplay/GuidedAltitudeSlider.qml
index feafee1a0ab637d8c57a05e64d41e4b8f3cd9d78..a55c13077097bf5042f042b4b522f5d700eb0d6a 100644
--- a/src/FlightDisplay/GuidedAltitudeSlider.qml
+++ b/src/FlightDisplay/GuidedAltitudeSlider.qml
@@ -21,18 +21,27 @@ Rectangle {
readonly property real _maxAlt: 121.92 // 400 feet
readonly property real _minAlt: 3
- property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
- property real _vehicleAltitude: _activeVehicle ? _activeVehicle.altitudeRelative.rawValue : 0
- property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing : false
- property real _sliderMaxAlt: _fixedWing ? _maxAlt : Math.min(_vehicleAltitude + 10, _maxAlt)
- property real _sliderMinAlt: _fixedWing ? _minAlt : Math.max(_vehicleAltitude - 10, _minAlt)
+ property var _guidedSettings: QGroundControl.settingsManager.guidedSettings
+ property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
+ property real _vehicleAltitude: _activeVehicle ? _activeVehicle.altitudeRelative.rawValue : 0
+ property bool _fixedWing: _activeVehicle ? _activeVehicle.fixedWing : false
+ property real _sliderMaxAlt: _fixedWing ? _guidedSettings.fixedWingMaximumAltitude.value : _guidedSettings.vehicleMaximumAltitude.value
+ property real _sliderMinAlt: _fixedWing ? _guidedSettings.fixedWingMinimumAltitude.value : _guidedSettings.vehicleMinimumAltitude.value
function reset() {
- altSlider.value = Math.min(Math.max(altSlider.minimumValue, 0), altSlider.maximumValue)
+ altSlider.value = 0
}
function getValue() {
- return altSlider.value
+ return altField.newAltitude - _vehicleAltitude
+ }
+
+ function log10(value) {
+ if (value === 0) {
+ return 0
+ } else {
+ return Math.log(value) / Math.LN10
+ }
}
Column {
@@ -55,7 +64,11 @@ Rectangle {
anchors.horizontalCenter: parent.horizontalCenter
text: Math.abs(newAltitude.toFixed(1)) + " " + QGroundControl.appSettingsDistanceUnitsString
- property real newAltitude: QGroundControl.metersToAppSettingsDistanceUnits(_root._vehicleAltitude + altSlider.value).toFixed(1)
+ property real altGainRange: Math.max(_sliderMaxAlt - _vehicleAltitude, 0)
+ property real altLossRange: Math.max(_vehicleAltitude - _sliderMinAlt, 0)
+ property real altExp: Math.pow(altSlider.value, 3)
+ property real altLossGain: altExp * (altSlider.value > 0 ? altGainRange : altLossRange)
+ property real newAltitude: _vehicleAltitude + altLossGain // QGroundControl.metersToAppSettingsDistanceUnits(_root._vehicleAltitude + altSlider.value).toFixed(1)
}
}
@@ -67,9 +80,9 @@ Rectangle {
anchors.left: parent.left
anchors.right: parent.right
orientation: Qt.Vertical
- minimumValue: _root._sliderMinAlt - _root._vehicleAltitude
- maximumValue: _root._sliderMaxAlt - _root._vehicleAltitude
- zeroCentered: true
+ minimumValue: -1
+ maximumValue: 1
+ zeroCentered: true
rotation: 180
// We want slide up to be positive values
diff --git a/src/Settings/Guided.SettingsGroup.json b/src/Settings/Guided.SettingsGroup.json
new file mode 100644
index 0000000000000000000000000000000000000000..f1e8270810223b08736236899fc066122a7ca9f4
--- /dev/null
+++ b/src/Settings/Guided.SettingsGroup.json
@@ -0,0 +1,26 @@
+[
+{
+ "name": "FixedWingMinimumAltitude",
+ "type": "double",
+ "units": "m",
+ "defaultValue": 10
+},
+{
+ "name": "FixedWingMaximumAltitude",
+ "type": "double",
+ "units": "m",
+ "defaultValue": 121.92
+},
+{
+ "name": "VehicleMinimumAltitude",
+ "type": "double",
+ "units": "m",
+ "defaultValue": 2
+},
+{
+ "name": "VehicleMaximumAltitude",
+ "type": "double",
+ "units": "m",
+ "defaultValue": 121.92
+}
+]
diff --git a/src/Settings/GuidedSettings.cc b/src/Settings/GuidedSettings.cc
new file mode 100644
index 0000000000000000000000000000000000000000..3aa93e2af65f3ea434eb2b2b0a6706c3836ede60
--- /dev/null
+++ b/src/Settings/GuidedSettings.cc
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * (c) 2009-2016 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#include "GuidedSettings.h"
+#include "QGCPalette.h"
+#include "QGCApplication.h"
+
+#include
+#include
+#include
+
+const char* GuidedSettings::guidedSettingsGroupName = "Guided";
+const char* GuidedSettings::fixedWingMinimumAltitudeName = "FixedWingMinimumAltitude";
+const char* GuidedSettings::fixedWingMaximumAltitudeName = "FixedWingMaximumAltitude";
+const char* GuidedSettings::vehicleMinimumAltitudeName = "VehicleMinimumAltitude";
+const char* GuidedSettings::vehicleMaximumAltitudeName = "VehicleMaximumAltitude";
+
+GuidedSettings::GuidedSettings(QObject* parent)
+ : SettingsGroup(guidedSettingsGroupName, QString() /* root settings group */, parent)
+ , _fixedWingMinimumAltitudeFact (NULL)
+ , _fixedWingMaximumAltitudeFact (NULL)
+ , _vehicleMinimumAltitudeFact (NULL)
+ , _vehicleMaximumAltitudeFact (NULL)
+{
+ QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
+ qmlRegisterUncreatableType("QGroundControl.SettingsManager", 1, 0, "GuidedSettings", "Reference only");
+}
+
+Fact* GuidedSettings::fixedWingMinimumAltitude(void)
+{
+ if (!_fixedWingMinimumAltitudeFact) {
+ _fixedWingMinimumAltitudeFact = _createSettingsFact(fixedWingMinimumAltitudeName);
+ }
+
+ return _fixedWingMinimumAltitudeFact;
+}
+
+Fact* GuidedSettings::fixedWingMaximumAltitude(void)
+{
+ if (!_fixedWingMaximumAltitudeFact) {
+ _fixedWingMaximumAltitudeFact = _createSettingsFact(fixedWingMaximumAltitudeName);
+ }
+
+ return _fixedWingMaximumAltitudeFact;
+}
+
+Fact* GuidedSettings::vehicleMinimumAltitude(void)
+{
+ if (!_vehicleMinimumAltitudeFact) {
+ _vehicleMinimumAltitudeFact = _createSettingsFact(vehicleMinimumAltitudeName);
+ }
+ return _vehicleMinimumAltitudeFact;
+}
+
+Fact* GuidedSettings::vehicleMaximumAltitude(void)
+{
+ if (!_vehicleMaximumAltitudeFact) {
+ _vehicleMaximumAltitudeFact = _createSettingsFact(vehicleMaximumAltitudeName);
+ }
+ return _vehicleMaximumAltitudeFact;
+}
diff --git a/src/Settings/GuidedSettings.h b/src/Settings/GuidedSettings.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc6b29de02dc7226fd0b1cece86d8402dfb13f30
--- /dev/null
+++ b/src/Settings/GuidedSettings.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * (c) 2009-2016 QGROUNDCONTROL PROJECT
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+#ifndef GuidedSettings_H
+#define GuidedSettings_H
+
+#include "SettingsGroup.h"
+#include "QGCMAVLink.h"
+
+class GuidedSettings : public SettingsGroup
+{
+ Q_OBJECT
+
+public:
+ GuidedSettings(QObject* parent = NULL);
+
+ // These min/max altitudes are used by the guided altitude slider
+ Q_PROPERTY(Fact* fixedWingMinimumAltitude READ fixedWingMinimumAltitude CONSTANT)
+ Q_PROPERTY(Fact* fixedWingMaximumAltitude READ fixedWingMaximumAltitude CONSTANT)
+ Q_PROPERTY(Fact* vehicleMinimumAltitude READ vehicleMinimumAltitude CONSTANT)
+ Q_PROPERTY(Fact* vehicleMaximumAltitude READ vehicleMaximumAltitude CONSTANT)
+
+ Fact* fixedWingMinimumAltitude (void);
+ Fact* fixedWingMaximumAltitude (void);
+ Fact* vehicleMinimumAltitude (void);
+ Fact* vehicleMaximumAltitude (void);
+
+ static const char* guidedSettingsGroupName;
+
+ static const char* fixedWingMinimumAltitudeName;
+ static const char* fixedWingMaximumAltitudeName;
+ static const char* vehicleMinimumAltitudeName;
+ static const char* vehicleMaximumAltitudeName;
+
+private:
+ SettingsFact* _fixedWingMinimumAltitudeFact;
+ SettingsFact* _fixedWingMaximumAltitudeFact;
+ SettingsFact* _vehicleMinimumAltitudeFact;
+ SettingsFact* _vehicleMaximumAltitudeFact;
+};
+
+#endif
diff --git a/src/Settings/SettingsManager.cc b/src/Settings/SettingsManager.cc
index 0f3608ddac19f70004545e006f515d048f0d1185..d2e8e10cbb9e6af6381a8e3c017c20a90b0adaed 100644
--- a/src/Settings/SettingsManager.cc
+++ b/src/Settings/SettingsManager.cc
@@ -14,12 +14,13 @@
SettingsManager::SettingsManager(QGCApplication* app, QGCToolbox* toolbox)
: QGCTool(app, toolbox)
- , _appSettings(NULL)
- , _unitsSettings(NULL)
- , _autoConnectSettings(NULL)
- , _videoSettings(NULL)
- , _flightMapSettings(NULL)
- , _rtkSettings(NULL)
+ , _appSettings (NULL)
+ , _unitsSettings (NULL)
+ , _autoConnectSettings (NULL)
+ , _videoSettings (NULL)
+ , _flightMapSettings (NULL)
+ , _rtkSettings (NULL)
+ , _guidedSettings (NULL)
{
}
@@ -36,4 +37,5 @@ void SettingsManager::setToolbox(QGCToolbox *toolbox)
_videoSettings = new VideoSettings(this);
_flightMapSettings = new FlightMapSettings(this);
_rtkSettings = new RTKSettings(this);
+ _guidedSettings = new GuidedSettings(this);
}
diff --git a/src/Settings/SettingsManager.h b/src/Settings/SettingsManager.h
index 05b09930df01a773cfe86e685b3c42f1fc406bb6..c38cd725c308fd69f64b3c3d7bb3f0ce736d3d24 100644
--- a/src/Settings/SettingsManager.h
+++ b/src/Settings/SettingsManager.h
@@ -20,6 +20,7 @@
#include "VideoSettings.h"
#include "FlightMapSettings.h"
#include "RTKSettings.h"
+#include "GuidedSettings.h"
#include
@@ -36,7 +37,8 @@ public:
Q_PROPERTY(QObject* autoConnectSettings READ autoConnectSettings CONSTANT)
Q_PROPERTY(QObject* videoSettings READ videoSettings CONSTANT)
Q_PROPERTY(QObject* flightMapSettings READ flightMapSettings CONSTANT)
- Q_PROPERTY(QObject* rtkSettings READ rtkSettings CONSTANT)
+ Q_PROPERTY(QObject* rtkSettings READ rtkSettings CONSTANT)
+ Q_PROPERTY(QObject* guidedSettings READ guidedSettings CONSTANT)
// Override from QGCTool
virtual void setToolbox(QGCToolbox *toolbox);
@@ -47,6 +49,7 @@ public:
VideoSettings* videoSettings (void) { return _videoSettings; }
FlightMapSettings* flightMapSettings (void) { return _flightMapSettings; }
RTKSettings* rtkSettings (void) { return _rtkSettings; }
+ GuidedSettings* guidedSettings (void) { return _guidedSettings; }
private:
AppSettings* _appSettings;
@@ -55,6 +58,7 @@ private:
VideoSettings* _videoSettings;
FlightMapSettings* _flightMapSettings;
RTKSettings* _rtkSettings;
+ GuidedSettings* _guidedSettings;
};
#endif