actuators: implement rules/constraints

src/AutoPilotPlugins/PX4/ActuatorComponent.qml

@@ -109,7 +109,8 @@ SetupPage {
                                                 fact:           object.fact
                                                 Layout.row:     1 + channelIndex
                                                 Layout.column:  1 + index
-                                                visible:        object.config.visible && (_showAdvanced || !object.config.advanced)
+                                                visible:        object.config.visible && (_showAdvanced || !object.config.advanced) && object.visible
+                                                enabled:        object.enabled
                                             }
                                         }
                                     }

src/Vehicle/Actuators/Actuators.cc

@@ -24,6 +24,7 @@ Actuators::Actuators(QObject* parent, Vehicle* vehicle)
       _motorAssignment(nullptr, vehicle, _actuatorOutputs), _vehicle(vehicle)
 {
     connect(&_mixer, &Mixer::Mixers::paramChanged, this, &Actuators::parametersChanged);
+    connect(&_mixer, &Mixer::Mixers::geometryParamChanged, this, &Actuators::updateGeometryImage);
     qRegisterMetaType<Actuators*>("Actuators*");
     connect(&_motorAssignment, &MotorAssignment::activeChanged, this, &Actuators::motorAssignmentActiveChanged);
     connect(&_motorAssignment, &MotorAssignment::messageChanged, this, &Actuators::motorAssignmentMessageChanged);
@@ -534,6 +535,7 @@ bool Actuators::parseJson(const QJsonDocument &json)
                 QJsonValue parameter = parameterJson.toObject();
                 Mixer::MixerParameter mixerParameter{};
                 mixerParameter.param.parse(parameter);
+                mixerParameter.identifier = parameter["identifier"].toString();
                 QString function = parameter["function"].toString();
                 if (function == "posx") {
                     mixerParameter.function = Mixer::Function::PositionX;
@@ -590,7 +592,57 @@ bool Actuators::parseJson(const QJsonDocument &json)
         mixerOptions.append(option);
     }
 
-    _mixer.reset(actuatorTypes, mixerOptions, outputFunctions);
+    QList<Mixer::Rule> rules;
+    QJsonValue mixerRulesJson = mixerJson.toObject().value("rules");
+    QJsonArray mixerRulesJsonArr = mixerRulesJson.toArray();
+    for (const auto& mixerRuleJson : mixerRulesJsonArr) {
+        QJsonValue mixerRule = mixerRuleJson.toObject();
+        Mixer::Rule rule{};
+        rule.selectIdentifier = mixerRule["select-identifier"].toString();
+
+        QJsonArray identifiersJson = mixerRule["apply-identifiers"].toArray();
+        for (const auto& identifierJson : identifiersJson) {
+            rule.applyIdentifiers.append(identifierJson.toString());
+        }
+
+        QJsonObject itemsJson = mixerRule["items"].toObject();
+        for (const auto& itemKey : itemsJson.keys()) {
+            bool ok;
+            int key = itemKey.toInt(&ok);
+            if (ok) {
+                QJsonArray itemsArr = itemsJson.value(itemKey).toArray();
+                QList<Mixer::Rule::RuleItem> items{};
+                for (const auto& itemJson : itemsArr) {
+                    QJsonObject itemObj = itemJson.toObject();
+
+                    Mixer::Rule::RuleItem item{};
+                    if (itemObj.contains("min")) {
+                        item.hasMin = true;
+                        item.min = itemObj["min"].toDouble();
+                    }
+                    if (itemObj.contains("max")) {
+                        item.hasMax = true;
+                        item.max = itemObj["max"].toDouble();
+                    }
+                    if (itemObj.contains("default")) {
+                        item.hasDefault = true;
+                        item.defaultVal = itemObj["default"].toDouble();
+                    }
+                    item.hidden = itemObj["hidden"].toBool(false);
+                    item.disabled = itemObj["disabled"].toBool(false);
+                    items.append(item);
+                }
+                if (items.size() == rule.applyIdentifiers.size()) {
+                    rule.items[key] = items;
+                } else {
+                    qCWarning(ActuatorsConfigLog) << "Rules: unexpected num items in " << itemsArr << "expected:" << rule.applyIdentifiers.size();
+                }
+            }
+        }
+        rules.append(rule);
+    }
+
+    _mixer.reset(actuatorTypes, mixerOptions, outputFunctions, rules);
     _init = true;
     return true;
 }

src/Vehicle/Actuators/Mixer.cc

@@ -16,8 +16,9 @@
 using namespace Mixer;
 
 MixerChannel::MixerChannel(QObject *parent, const QString &label, int actuatorFunction, int paramIndex, int actuatorTypeIndex,
-        QmlObjectListModel &channelConfigs, ParameterManager* parameterManager, std::function<void(Fact*)> factAddedCb) :
-        QObject(parent), _label(label), _actuatorFunction(actuatorFunction), _paramIndex(paramIndex), _actuatorTypeIndex(actuatorTypeIndex)
+        QmlObjectListModel &channelConfigs, ParameterManager* parameterManager, const Rule* rule, std::function<void(Fact*)> factAddedCb) :
+        QObject(parent), _label(label), _actuatorFunction(actuatorFunction), _paramIndex(paramIndex), _actuatorTypeIndex(actuatorTypeIndex),
+        _rule(rule)
 {
     for (int i = 0; i < channelConfigs.count(); ++i) {
         auto channelConfig = channelConfigs.value<ChannelConfig*>(i);
@@ -57,9 +58,85 @@ MixerChannel::MixerChannel(QObject *parent, const QString &label, int actuatorFu
             qCDebug(ActuatorsConfigLog) << "ActuatorOutputChannel: Param does not exist:" << param;
         }
 
-        ChannelConfigInstance* instance = new ChannelConfigInstance(this, fact, *channelConfig);
+        // if we have a valid rule, check the identifiers
+        int applyIdentifierIdx = -1;
+        if (rule) {
+            if (channelConfig->identifier() == rule->selectIdentifier) {
+                _ruleSelectIdentifierIdx = _configInstances->count();
+                if (fact) {
+                    _currentSelectIdentifierValue = fact->rawValue().toInt();
+                }
+            } else {
+                for (int i = 0; i < rule->applyIdentifiers.size(); ++i) {
+                    if (channelConfig->identifier() == rule->applyIdentifiers[i]) {
+                        applyIdentifierIdx = i;
+                    }
+                }
+            }
+
+            if (fact) {
+                connect(fact, &Fact::rawValueChanged, this, [this]() { applyRule(); });
+            }
+        }
+
+        ChannelConfigInstance* instance = new ChannelConfigInstance(this, fact, *channelConfig, applyIdentifierIdx);
         _configInstances->append(instance);
     }
+
+    applyRule(true);
+}
+
+void MixerChannel::applyRule(bool noConstraints)
+{
+    if (!_rule || _ruleSelectIdentifierIdx == -1 || _applyingRule) {
+        return;
+    }
+    _applyingRule = true; // prevent recursive signals
+
+    Fact* selectFact = _configInstances->value<ChannelConfigInstance*>(_ruleSelectIdentifierIdx)->fact();
+    if (selectFact && selectFact->type() == FactMetaData::valueTypeInt32) {
+        const int value = selectFact->rawValue().toInt();
+        if (_rule->items.contains(value)) {
+            bool valueChanged = value != _currentSelectIdentifierValue;
+            const auto& items = _rule->items[value];
+            for (int i = 0; i < _configInstances->count(); ++i) {
+                ChannelConfigInstance* configInstance = _configInstances->value<ChannelConfigInstance*>(i);
+                if (configInstance->fact() && configInstance->ruleApplyIdentifierIdx() >= 0) {
+                    const Rule::RuleItem& ruleItem = items[configInstance->ruleApplyIdentifierIdx()];
+                    double factValue = configInstance->fact()->rawValue().toDouble();
+                    bool changed = false;
+                    if (ruleItem.hasMin && factValue < ruleItem.min) {
+                        factValue = ruleItem.min;
+                        changed = true;
+                    }
+                    if (ruleItem.hasMax && factValue > ruleItem.max) {
+                        factValue = ruleItem.max;
+                        changed = true;
+                    }
+                    if (valueChanged && ruleItem.hasDefault) {
+                        factValue = ruleItem.defaultVal;
+                        changed = true;
+                    }
+                    if (changed && !noConstraints) {
+                        // here we could notify the user that a constraint got applied...
+                        configInstance->fact()->setRawValue(factValue);
+                    }
+                    configInstance->setVisible(!ruleItem.hidden);
+                    configInstance->setEnabled(!ruleItem.disabled);
+                }
+            }
+
+        } else {
+            // no rule set for this value, just reset
+            for (int i = 0; i < _configInstances->count(); ++i) {
+                ChannelConfigInstance* configInstance = _configInstances->value<ChannelConfigInstance*>(i);
+                configInstance->setVisible(true);
+                configInstance->setEnabled(true);
+            }
+        }
+        _currentSelectIdentifierValue = value;
+    }
+    _applyingRule = false;
 }
 
 bool MixerChannel::getGeometry(const ActuatorTypes& actuatorTypes, const MixerOption::ActuatorGroup& group,
@@ -126,13 +203,14 @@ void MixerConfigGroup::addConfigParam(ConfigParameter* param)
 }
 
 void Mixers::reset(const ActuatorTypes& actuatorTypes, const MixerOptions& mixerOptions,
-        const QMap<int, OutputFunction>& functions)
+        const QMap<int, OutputFunction>& functions, const Rules& rules)
 {
     _groups->clearAndDeleteContents();
     _actuatorTypes = actuatorTypes;
     _mixerOptions = mixerOptions;
     _functions = functions;
     _functionsSpecificLabel.clear();
+    _rules = rules;
     _mixerConditions.clear();
     for (const auto& mixerOption : _mixerOptions) {
         _mixerConditions.append(Condition(mixerOption.option, _parameterManager));
@@ -188,8 +266,18 @@ void Mixers::update()
                     currentMixerGroup->addChannelConfig(new ChannelConfig(currentMixerGroup, param, true));
                 }
             }
+
+            const Rule* selectedRule{nullptr}; // at most 1 rule can be applied
             for (const auto& perItemParam : actuatorGroup.perItemParameters) {
                 currentMixerGroup->addChannelConfig(new ChannelConfig(currentMixerGroup, perItemParam, false));
+
+                if (!perItemParam.identifier.isEmpty()) {
+                    for (const auto& rule : _rules) {
+                        if (rule.selectIdentifier == perItemParam.identifier) {
+                            selectedRule = &rule;
+                        }
+                    }
+                }
             }
 
             // 'count' param
@@ -221,7 +309,7 @@ void Mixers::update()
                     }
                 }
                 MixerChannel* channel = new MixerChannel(currentMixerGroup, label, actuatorFunction, actuatorIdx, actuatorTypeIndex,
-                        *currentMixerGroup->channelConfigs(), _parameterManager, [this](Fact* fact) { subscribeFact(fact); });
+                        *currentMixerGroup->channelConfigs(), _parameterManager, selectedRule, [this](Fact* fact) { subscribeFact(fact, true); });
                 currentMixerGroup->addChannel(channel);
                 ++actuatorTypeIndex;
             }
@@ -287,11 +375,18 @@ Fact* Mixers::getFact(const QString& paramName)
 	return fact;
 }
 
-void Mixers::subscribeFact(Fact* fact)
+void Mixers::subscribeFact(Fact* fact, bool geometry)
 {
-    if (fact && !_subscribedFacts.contains(fact)) {
-        connect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
-        _subscribedFacts.insert(fact);
+    if (geometry) {
+        if (fact && !_subscribedFactsGeometry.contains(fact)) {
+            connect(fact, &Fact::rawValueChanged, this, &Mixers::geometryParamChanged);
+            _subscribedFactsGeometry.insert(fact);
+        }
+    } else {
+        if (fact && !_subscribedFacts.contains(fact)) {
+            connect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
+            _subscribedFacts.insert(fact);
+        }
     }
 }
 
@@ -301,4 +396,9 @@ void Mixers::unsubscribeFacts()
         disconnect(fact, &Fact::rawValueChanged, this, &Mixers::paramChanged);
     }
     _subscribedFacts.clear();
+
+    for (Fact* fact : _subscribedFactsGeometry) {
+        disconnect(fact, &Fact::rawValueChanged, this, &Mixers::geometryParamChanged);
+    }
+    _subscribedFactsGeometry.clear();
 }

src/Vehicle/Actuators/Mixer.h

@@ -12,6 +12,7 @@
 #include <QObject>
 #include <QString>
 #include <QList>
+#include <QMap>
 #include <QSet>
 
 #include "Common.h"
@@ -35,6 +36,7 @@ enum class Function {
 struct MixerParameter {
     Parameter param;
     Function function{Function::Unspecified};
+    QString identifier; ///< optional identifier for rules
     QList<float> values{}; ///< fixed values if not configurable (param.name == "")
 };
 
@@ -81,6 +83,25 @@ struct MixerOption
 
 using MixerOptions = QList<MixerOption>;
 
+struct Rule {
+    struct RuleItem {
+        bool hasMin{false};
+        bool hasMax{false};
+        bool hasDefault{false};
+
+        float min{};
+        float max{};
+        float defaultVal{};
+        bool hidden{false};
+        bool disabled{false};
+    };
+    QString selectIdentifier;
+    QList<QString> applyIdentifiers;
+    QMap<int, QList<RuleItem>> items;
+};
+
+using Rules = QList<Rule>;
+
 /**
  * Config parameters that apply to a group of actuators
  */
@@ -133,6 +154,7 @@ public:
     Parameter::DisplayOption displayOption() const { return _parameter.param.displayOption; }
     int indexOffset() const { return _parameter.param.indexOffset; }
 
+    const QString& identifier() const { return _parameter.identifier; }
 private:
     const MixerParameter _parameter;
     const bool _isActuatorTypeConfig; ///< actuator type config instead of mixer channel config
@@ -145,20 +167,38 @@ class ChannelConfigInstance : public QObject
 {
     Q_OBJECT
 public:
-    ChannelConfigInstance(QObject* parent, Fact* fact, ChannelConfig& config)
-        : QObject(parent), _fact(fact), _config(config) {}
+    ChannelConfigInstance(QObject* parent, Fact* fact, ChannelConfig& config, int ruleApplyIdentifierIdx)
+        : QObject(parent), _fact(fact), _config(config), _ruleApplyIdentifierIdx(ruleApplyIdentifierIdx) {}
 
     Q_PROPERTY(ChannelConfig* config      READ channelConfig    CONSTANT)
-    Q_PROPERTY(Fact* fact                 READ fact            CONSTANT)
+    Q_PROPERTY(Fact* fact                 READ fact             CONSTANT)
+    Q_PROPERTY(bool visible               READ visible          NOTIFY visibleChanged)
+    Q_PROPERTY(bool enabled               READ enabled          NOTIFY enabledChanged)
 
     ChannelConfig* channelConfig() const { return &_config; }
 
     Fact* fact() { return _fact; }
 
+    bool visible() const { return _visible; }
+    bool enabled() const { return _enabled; }
+
+    // controlled via rules
+    void setVisible(bool visible) { _visible = visible; emit visibleChanged(); }
+    void setEnabled(bool enabled) { _enabled = enabled; emit enabledChanged(); }
+
+    int ruleApplyIdentifierIdx() const { return _ruleApplyIdentifierIdx; }
+
+signals:
+    void visibleChanged();
+    void enabledChanged();
 private:
 
     Fact* _fact{nullptr};
     ChannelConfig& _config;
+    const int _ruleApplyIdentifierIdx;
+
+    bool _visible{true};
+    bool _enabled{true};
 };
 
 class MixerChannel : public QObject
@@ -167,7 +207,7 @@ class MixerChannel : public QObject
 public:
     MixerChannel(QObject* parent, const QString& label, int actuatorFunction,
             int paramIndex, int actuatorTypeIndex, QmlObjectListModel& channelConfigs, ParameterManager* parameterManager,
-            std::function<void(Fact*)> factAddedCb);
+            const Rule* rule, std::function<void(Fact*)> factAddedCb);
 
     Q_PROPERTY(QString label                            READ label               CONSTANT)
     Q_PROPERTY(QmlObjectListModel* configInstances      READ configInstances     NOTIFY configInstancesChanged)
@@ -180,16 +220,24 @@ public:
     bool getGeometry(const ActuatorTypes& actuatorTypes, const MixerOption::ActuatorGroup& group,
             ActuatorGeometry& geometry) const;
 
+public slots:
+    void applyRule(bool noConstraints = false);
+
 signals:
     void configInstancesChanged();
+
 private:
 
     const QString _label;
     const int _actuatorFunction;
     const int _paramIndex;
     const int _actuatorTypeIndex;
+    const Rule* _rule;
 
     QmlObjectListModel* _configInstances = new QmlObjectListModel(this); ///< list of ChannelConfigInstance*
+    int _ruleSelectIdentifierIdx{-1};
+    int _currentSelectIdentifierValue{};
+    bool _applyingRule{false};
 };
 
 class MixerConfigGroup : public QObject
@@ -253,7 +301,7 @@ public:
     };
 
     void reset(const ActuatorTypes& actuatorTypes, const MixerOptions& mixerOptions,
-            const QMap<int, OutputFunction>& functions);
+            const QMap<int, OutputFunction>& functions, const Rules& rules);
 
     Q_PROPERTY(QmlObjectListModel* groups         READ groups        NOTIFY groupsChanged)
 
@@ -289,14 +337,16 @@ public slots:
 signals:
     void groupsChanged();
     void paramChanged();
+    void geometryParamChanged();
 
 private:
 
     Fact* getFact(const QString& paramName);
-    void subscribeFact(Fact* fact);
+    void subscribeFact(Fact* fact, bool geometry=false);
     void unsubscribeFacts();
 
     QSet<Fact*> _subscribedFacts{};
+    QSet<Fact*> _subscribedFactsGeometry{};
 
     ActuatorTypes _actuatorTypes;
     MixerOptions _mixerOptions;
@@ -308,6 +358,8 @@ private:
 
     QMap<int, QString> _functionsSpecificLabel; ///< function with specific label, e.g. 'Front Left Motor (Motor 1)'
     ParameterManager* _parameterManager{nullptr};
+
+    Rules _rules;
 };
 
 } // namespace Mixer