Merge pull request #7417 from DonLakeFlyer/SimpleFlightModes

ArduPilot: Simple flight modes

ChangeLog.md

@@ -6,6 +6,8 @@ Note: This file only contains high level features or important fixes.
 
 ### 3.6.0 - Daily Build
 
+* ArduPilot: Copter - Add suppor for Simple and Super Simple flight modes
+* ArduPilot: Flight Mode setup - Switch Options were not showing up for all firmware revs
 * ArduCopter: Add PID Tuning page to Tuning Setup
 * ArduPilot: Copter - Advanced Tuning support
 * ArduPilot: Rover - Frame setup support

src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc

@@ -14,13 +14,48 @@
 #include <QVariant>
 #include <QQmlProperty>
 
+bool APMFlightModesComponentController::_typeRegistered = false;
+
 APMFlightModesComponentController::APMFlightModesComponentController(void)
-    : _activeFlightMode(0)
-    , _channelCount(Vehicle::cMaxRcChannels)
+    : _rover                (_vehicle->rover())
+    , _activeFlightMode     (0)
+    , _channelCount         (Vehicle::cMaxRcChannels)
+    , _simpleMode           (SimpleModeStandard)
+    , _simpleModeFact       (_rover ? nullptr : getParameterFact(-1, "SIMPLE"))
+    , _superSimpleModeFact  (_rover ? nullptr : getParameterFact(-1, "SUPER_SIMPLE"))
+    , _simpleModesSupported (_simpleModeFact && _superSimpleModeFact)
 {
+    if (!_typeRegistered) {
+        qmlRegisterUncreatableType<APMFlightModesComponentController>("QGroundControl.Controllers", 1, 0, "APMFlightModesComponentController", "Reference only");
+    }
+
     _modeParamPrefix = _vehicle->rover() ? QStringLiteral("MODE") : QStringLiteral("FLTMODE");
     _modeChannelParam = _vehicle->rover() ? QStringLiteral("MODE_CH") : QStringLiteral("FLTMODE_CH");
 
+    _simpleModeNames << tr("Off") << tr("Simple") << tr("Super-Simple") << tr("Custom");
+    for (int i=0; i<_cFltModes; i++) {
+        _simpleModeEnabled.append(QVariant(false));
+        _superSimpleModeEnabled.append(QVariant(false));
+    }
+
+    if (!_rover) {
+        _setupSimpleModeEnabled();
+
+        uint8_t simpleModeValue = static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
+        uint8_t superSimpleModeValue = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
+        if (simpleModeValue == 0 && superSimpleModeValue == 0) {
+            _simpleMode = SimpleModeStandard;
+        } else if (simpleModeValue == _allSimpleBits && superSimpleModeValue == 0) {
+            _simpleMode = SimpleModeSimple;
+        } else if (simpleModeValue == 0 && superSimpleModeValue == _allSimpleBits) {
+            _simpleMode = SimpleModeSuperSimple;
+        } else {
+            _simpleMode = SimpleModeCustom;
+        }
+
+        connect(this, &APMFlightModesComponentController::simpleModeChanged, this, &APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode);
+    }
+
     QStringList usedParams;
     for (int i=1; i<7; i++) {
         usedParams << QStringLiteral("%1%2").arg(_modeParamPrefix).arg(i);
@@ -29,8 +64,10 @@ APMFlightModesComponentController::APMFlightModesComponentController(void)
         return;
     }
 
-    _rgChannelOptionEnabled << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false) << QVariant(false);
-    
+    for (int i=0; i<_cChannelOptions; i++) {
+        _rgChannelOptionEnabled.append(QVariant(false));
+    }
+
     connect(_vehicle, &Vehicle::rcChannelsChanged, this, &APMFlightModesComponentController::_rcChannelsChanged);
 }
 
@@ -65,7 +102,7 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
     }
     emit activeFlightModeChanged(_activeFlightMode);
 
-    for (int i=0; i<6; i++) {
+    for (int i=0; i<_cChannelOptions; i++) {
         _rgChannelOptionEnabled[i] = QVariant(false);
         channelValue = pwmValues[i+6];
         if (channelValue > 1800) {
@@ -74,3 +111,66 @@ void APMFlightModesComponentController::_rcChannelsChanged(int channelCount, int
     }
     emit channelOptionEnabledChanged();
 }
+
+void APMFlightModesComponentController::_updateSimpleParamsFromSimpleMode(void)
+{
+    int newSimpleModeValue = 0;
+    int newSuperSimpleModeValue = 0;
+
+    if (_simpleMode == SimpleModeSimple) {
+        newSimpleModeValue = _allSimpleBits;
+    } else if (_simpleMode == SimpleModeSuperSimple) {
+        newSuperSimpleModeValue = _allSimpleBits;
+    }
+
+    for (int i=0; i<_cFltModes; i++) {
+        _simpleModeEnabled[i] =         false;
+        _superSimpleModeEnabled[i] =    false;
+    }
+    emit simpleModeEnabledChanged();
+    emit superSimpleModeEnabledChanged();
+
+    _simpleModeFact->setRawValue(newSimpleModeValue);
+    _superSimpleModeFact->setRawValue(newSuperSimpleModeValue);
+}
+
+void APMFlightModesComponentController::setSimpleMode(int fltModeIndex, bool enabled)
+{
+    if (fltModeIndex < _cFltModes) {
+        uint8_t mode = static_cast<uint8_t>(_simpleModeFact->rawValue().toInt());
+        if (enabled) {
+            mode |= 1 << fltModeIndex;
+        } else {
+            mode &= ~(1 << fltModeIndex);
+        }
+        _simpleModeFact->setRawValue(mode);
+    }
+}
+
+void APMFlightModesComponentController::setSuperSimpleMode(int fltModeIndex, bool enabled)
+{
+    if (fltModeIndex < _cFltModes) {
+        uint8_t mode = static_cast<uint8_t>(_superSimpleModeFact->rawValue().toInt());
+        if (enabled) {
+            mode |= 1 << fltModeIndex;
+        } else {
+            mode &= ~(1 << fltModeIndex);
+        }
+        _superSimpleModeFact->setRawValue(mode);
+    }
+}
+
+void APMFlightModesComponentController::_setupSimpleModeEnabled(void)
+{
+    uint8_t simpleMode =        static_cast<uint8_t>(_simpleModeFact->rawValue().toUInt());
+    uint8_t superSimpleMode =   static_cast<uint8_t>(_superSimpleModeFact->rawValue().toUInt());
+
+    for (int i=0; i<_cFltModes; i++) {
+        uint8_t bitSet = static_cast<uint8_t>(1 << i);
+        _simpleModeEnabled[i] = !!(simpleMode & bitSet);
+        _superSimpleModeEnabled[i] = !!(superSimpleMode & bitSet);
+    }
+
+    emit simpleModeEnabledChanged();
+    emit superSimpleModeEnabledChanged();
+}

src/AutoPilotPlugins/APM/APMFlightModesComponentController.h

@@ -27,30 +27,66 @@ class APMFlightModesComponentController : public FactPanelController
     Q_OBJECT
     
 public:
+    enum SimpleModeValues {
+        SimpleModeStandard = 0,
+        SimpleModeSimple,
+        SimpleModeSuperSimple,
+        SimpleModeCustom
+    };
+    Q_ENUM(SimpleModeValues)
+
     APMFlightModesComponentController(void);
     
-    Q_PROPERTY(QString  modeParamPrefix             MEMBER _modeParamPrefix     CONSTANT)
-    Q_PROPERTY(QString  modeChannelParam            MEMBER _modeChannelParam    CONSTANT)
-    Q_PROPERTY(int      activeFlightMode            READ activeFlightMode       NOTIFY activeFlightModeChanged)
-    Q_PROPERTY(int      channelCount                MEMBER _channelCount        CONSTANT)
-    Q_PROPERTY(QVariantList channelOptionEnabled    READ channelOptionEnabled   NOTIFY channelOptionEnabledChanged)
+    Q_PROPERTY(QString      modeParamPrefix         MEMBER _modeParamPrefix         CONSTANT)
+    Q_PROPERTY(QString      modeChannelParam        MEMBER _modeChannelParam        CONSTANT)
+    Q_PROPERTY(int          activeFlightMode        READ activeFlightMode           NOTIFY activeFlightModeChanged)
+    Q_PROPERTY(int          channelCount            MEMBER _channelCount            CONSTANT)
+    Q_PROPERTY(QVariantList channelOptionEnabled    READ channelOptionEnabled       NOTIFY channelOptionEnabledChanged)
+    Q_PROPERTY(bool         simpleModesSupported    MEMBER _simpleModesSupported    CONSTANT)
+    Q_PROPERTY(QStringList  simpleModeNames         MEMBER _simpleModeNames         CONSTANT)
+    Q_PROPERTY(int          simpleMode              MEMBER _simpleMode              NOTIFY simpleModeChanged)
+    Q_PROPERTY(QVariantList simpleModeEnabled       MEMBER _simpleModeEnabled       NOTIFY simpleModeEnabledChanged)
+    Q_PROPERTY(QVariantList superSimpleModeEnabled  MEMBER _superSimpleModeEnabled  NOTIFY superSimpleModeEnabledChanged)
+
+    Q_INVOKABLE void setSimpleMode(int fltModeIndex, bool enabled);
+    Q_INVOKABLE void setSuperSimpleMode(int fltModeIndex, bool enabled);
 
     int activeFlightMode(void) const { return _activeFlightMode; }
     QVariantList channelOptionEnabled(void) const { return _rgChannelOptionEnabled; }
 
 signals:
-    void activeFlightModeChanged(int activeFlightMode);
-    void channelOptionEnabledChanged(void);
-    
+    void activeFlightModeChanged        (int activeFlightMode);
+    void channelOptionEnabledChanged    (void);
+    void simpleModeChanged              (int simpleMode);
+    void simpleModeEnabledChanged       (void);
+    void superSimpleModeEnabledChanged  (void);
+
 private slots:
-    void _rcChannelsChanged(int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
-    
+    void _rcChannelsChanged                     (int channelCount, int pwmValues[Vehicle::cMaxRcChannels]);
+    void _updateSimpleParamsFromSimpleMode      (void);
+    void _setupSimpleModeEnabled     (void);
+
 private:
+    bool            _rover;
     QString         _modeParamPrefix;
     QString         _modeChannelParam;
     int             _activeFlightMode;
     int             _channelCount;
     QVariantList    _rgChannelOptionEnabled;
+    QStringList     _simpleModeNames;
+    int             _simpleMode;
+    Fact*           _simpleModeFact;
+    Fact*           _superSimpleModeFact;
+    bool            _simpleModesSupported;
+    QVariantList    _simpleModeEnabled;
+    QVariantList    _superSimpleModeEnabled;
+
+    static const uint8_t    _allSimpleBits =    0x3F;
+    static const int        _cChannelOptions =  10;
+    static const int        _cSimpleModeBits =  8;
+    static const int        _cFltModes =        6;
+
+    static bool _typeRegistered;
 };
 
 #endif

src/AutoPilotPlugins/APM/APMPowerComponent.qml

@@ -516,7 +516,7 @@ SetupPage {
                     QGCLabel {
                         width:      parent.width
                         wrapMode:   Text.WordWrap
-                        text:       qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new voltage multiplier.")
+                        text:       qsTr("Measure battery voltage using an external voltmeter and enter the value below. Click Calculate to set the new adjusted voltage multiplier.")
                     }
 
                     Grid {
@@ -537,7 +537,7 @@ SetupPage {
                     }
 
                     QGCButton {
-                        text: "Calculate"
+                        text: qsTr("Calculate And Set")
 
                         onClicked:  {
                             var measuredVoltageValue = parseFloat(measuredVoltage.text)
@@ -599,7 +599,7 @@ SetupPage {
                     }
 
                     QGCButton {
-                        text: "Calculate"
+                        text: qsTr("Calculate And Set")
 
                         onClicked:  {
                             var measuredCurrentValue = parseFloat(measuredCurrent.text)

src/Vehicle/Vehicle.cc

@@ -3409,7 +3409,23 @@ QString Vehicle::firmwareVersionTypeString(void) const
 void Vehicle::rebootVehicle()
 {
     _autoDisconnect = true;
-    sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
+
+    mavlink_message_t       msg;
+    mavlink_command_long_t  cmd;
+
+    memset(&cmd, 0, sizeof(cmd));
+    cmd.target_system =     _id;
+    cmd.target_component =  _defaultComponentId;
+    cmd.command =           MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN;
+    cmd.confirmation =      0;
+    cmd.param1 =            1;
+    cmd.param2 = cmd.param3 = cmd.param4 = cmd.param5 = cmd.param6 = cmd.param7 = 0;
+    mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
+                                         _mavlink->getComponentId(),
+                                         priorityLink()->mavlinkChannel(),
+                                         &msg,
+                                         &cmd);
+    sendMessageOnLink(priorityLink(), msg);
 }
 
 void Vehicle::setSoloFirmware(bool soloFirmware)