Merge pull request #5150 from DonLakeFlyer/CubicAltSlider

Cubic altitude slider

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 \

qgroundcontrol.qrc

@@ -196,6 +196,7 @@
         <file alias="App.SettingsGroup.json">src/Settings/App.SettingsGroup.json</file>
         <file alias="AutoConnect.SettingsGroup.json">src/Settings/AutoConnect.SettingsGroup.json</file>
         <file alias="FlightMap.SettingsGroup.json">src/Settings/FlightMap.SettingsGroup.json</file>
+        <file alias="Guided.SettingsGroup.json">src/Settings/Guided.SettingsGroup.json</file>
         <file alias="RTK.SettingsGroup.json">src/Settings/RTK.SettingsGroup.json</file>
         <file alias="Survey.SettingsGroup.json">src/MissionManager/Survey.SettingsGroup.json</file>
         <file alias="Units.SettingsGroup.json">src/Settings/Units.SettingsGroup.json</file>

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;

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,

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

src/Settings/Guided.SettingsGroup.json

+[
+{
+    "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
+}
+]

src/Settings/GuidedSettings.cc

+/****************************************************************************
+ *
+ *   (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 "GuidedSettings.h"
+#include "QGCPalette.h"
+#include "QGCApplication.h"
+
+#include <QQmlEngine>
+#include <QtQml>
+#include <QStandardPaths>
+
+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<GuidedSettings>("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;
+}

src/Settings/GuidedSettings.h

+/****************************************************************************
+ *
+ *   (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.
+ *
+ ****************************************************************************/
+
+#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

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);
 }

src/Settings/SettingsManager.h

@@ -20,6 +20,7 @@
 #include "VideoSettings.h"
 #include "FlightMapSettings.h"
 #include "RTKSettings.h"
+#include "GuidedSettings.h"
 
 #include <QVariantList>
 
@@ -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