diff --git a/src/AutoPilotPlugins/PX4/ActuatorComponent.qml b/src/AutoPilotPlugins/PX4/ActuatorComponent.qml index 6bd85ef26c1..34430f4f13b 100644 --- a/src/AutoPilotPlugins/PX4/ActuatorComponent.qml +++ b/src/AutoPilotPlugins/PX4/ActuatorComponent.qml @@ -252,7 +252,7 @@ SetupPage { stopTimer(); for (var channelIdx=0; channelIdx mid - snapRange/2 && value < mid) { + value = mid + + } else if (value < mid + snapRange/2 && value > mid) { + value = mid + + // } else if(value < channel.defaultValue - snapRange/2) { + // value = channel.defaultValue - snapRange + + // } else if(value > channel.defaultValue + snapRange/2) { + // value = channel.defaultValue + snapRange + + + } } + sendTimer.start() } @@ -69,9 +95,31 @@ Column { running: false onTriggered: { var sendValue = channelSlider.value; - if (sendValue < channel.min - snapRange/2) { - sendValue = channel.defaultValue; + + if(isStandardMotor){ + + if (sendValue < channel.min - snapRange/2) { + sendValue = channel.defaultValue; + } + + } + else if(isBidirectionalMotor){ + + var mid = (channel.max + channel.min)/2 + if (sendValue > mid - snapRange/2 && sendValue < mid) { + sendValue = channel.defaultValue + } + else if (sendValue < mid + snapRange/2 && sendValue > mid) { + sendValue = channel.defaultValue + } + else if(sendValue > mid + snapRange/2){ + sendValue = sendValue - snapRange/2 + } + else if(sendValue < mid - snapRange/2){ + sendValue = sendValue + snapRange/2 + } } + root.actuatorValueChanged(sendValue, channelSlider.value) } } diff --git a/src/Vehicle/Actuators/ActuatorTesting.cc b/src/Vehicle/Actuators/ActuatorTesting.cc index bee1fd31b3a..c1af3554dcd 100644 --- a/src/Vehicle/Actuators/ActuatorTesting.cc +++ b/src/Vehicle/Actuators/ActuatorTesting.cc @@ -41,14 +41,14 @@ void ActuatorTest::updateFunctions(const QList &actuators) Actuator* motorActuator{nullptr}; for (const auto& actuator : actuators) { - if (actuator->isMotor()) { + if (actuator->isMotor() && !actuator->isBidirectional()) { motorActuator = actuator; } _actuators->append(actuator); } if (motorActuator) { - _allMotorsActuator = new Actuator(this, tr("All Motors"), motorActuator->min(), motorActuator->max(), motorActuator->defaultValue(), - motorActuator->function(), true); + _allMotorsActuator = new Actuator(this, tr("All Standard Motors"), motorActuator->min(), motorActuator->max(), motorActuator->defaultValue(), + motorActuator->function(), true, false); } resetStates(); diff --git a/src/Vehicle/Actuators/ActuatorTesting.h b/src/Vehicle/Actuators/ActuatorTesting.h index 1c52f386229..03ef91e2401 100644 --- a/src/Vehicle/Actuators/ActuatorTesting.h +++ b/src/Vehicle/Actuators/ActuatorTesting.h @@ -24,9 +24,9 @@ class Actuator : public QObject { Q_OBJECT public: - Actuator(QObject* parent, const QString& label, float min, float max, float defaultValue, int function, bool isMotor) + Actuator(QObject* parent, const QString& label, float min, float max, float defaultValue, int function, bool isMotor, bool isBidirectional) : QObject(parent), _label(label), _min(min), _max(max), _defaultValue(defaultValue), _function(function), - _isMotor(isMotor) {} + _isMotor(isMotor), _isBidirectional(isBidirectional) {} Q_PROPERTY(QString label READ label CONSTANT) Q_PROPERTY(float min READ min CONSTANT) @@ -34,6 +34,7 @@ class Actuator : public QObject Q_PROPERTY(float defaultValue READ defaultValue CONSTANT) Q_PROPERTY(int function READ function CONSTANT) Q_PROPERTY(bool isMotor READ isMotor CONSTANT) + Q_PROPERTY(bool isBidirectional READ isBidirectional CONSTANT) const QString& label() const { return _label; } float min() const { return _min; } @@ -41,6 +42,7 @@ class Actuator : public QObject float defaultValue() const { return _defaultValue; } int function() const { return _function; } bool isMotor() const { return _isMotor; } + bool isBidirectional() const { return _isBidirectional; } private: const QString _label; @@ -49,6 +51,7 @@ class Actuator : public QObject const float _defaultValue; const int _function; const bool _isMotor; + const bool _isBidirectional; }; class ActuatorTest : public QObject diff --git a/src/Vehicle/Actuators/Actuators.cc b/src/Vehicle/Actuators/Actuators.cc index 01231d4b47a..188cbeba42c 100644 --- a/src/Vehicle/Actuators/Actuators.cc +++ b/src/Vehicle/Actuators/Actuators.cc @@ -200,6 +200,32 @@ void Actuators::parametersChanged() QList actuators; QSet uniqueConfiguredFunctions; const Mixer::ActuatorTypes &actuatorTypes = _mixer.actuatorTypes(); + + quint8 bitset_bidirectional = 0; + for (const auto& actuatorTypeName : actuatorTypes.keys()) { + + const Mixer::ActuatorType& actuatorType = actuatorTypes[actuatorTypeName]; + bool isMotor = ActuatorGeometry::typeFromStr(actuatorTypeName) == ActuatorGeometry::Type::Motor; + + if(isMotor){ + + auto parameterIter = actuatorTypes[actuatorTypeName].perItemParams.constBegin(); + while(parameterIter != actuatorTypes[actuatorTypeName].perItemParams.constEnd() ){ + + qDebug() << "testinng: " << parameterIter->name; + // qDebug() << "testinng: " << parameterIter->function; + if(parameterIter->function == Parameter::Function::Reversible){ + QString bidirectional_param(parameterIter->name); + bitset_bidirectional = getFact(bidirectional_param)->rawValue().toInt(); + qDebug() << "bid :" << bitset_bidirectional; + break; + } + parameterIter++; + } + } + } + + int num_motor = 0; for (int function : allFunctions) { if (uniqueConfiguredFunctions.find(function) != uniqueConfiguredFunctions.end()) { // only add once continue; @@ -221,9 +247,30 @@ void Actuators::parametersChanged() const Mixer::ActuatorType& actuatorType = actuatorTypes[actuatorTypeName]; if (function >= actuatorType.functionMin && function <= actuatorType.functionMax) { bool isMotor = ActuatorGeometry::typeFromStr(actuatorTypeName) == ActuatorGeometry::Type::Motor; + + bool isBidirectional = false; + float min_value = actuatorType.values.min; + float default_value = actuatorType.values.defaultVal; + // float max_value = actuatorType.values.max; + if(isMotor && actuatorType.values.reversible){ + + quint8 is_bidi = (bitset_bidirectional >> num_motor) & 0b1; + + if(is_bidi){ + + isBidirectional = true; + min_value = -actuatorType.values.max; + // default_value = 0.0; + } + num_motor++; + // qDebug() << "reversible: " << actuatorType.values.reversible << " " << isBidirectional; + } + + // qDebug() << "testinng: " << actuatorType.reversible << actuatorType.values.min << actuatorType.values.max << actuatorType.values.defaultVal; + actuators.append( - new ActuatorTesting::Actuator(&_actuatorTest, label, actuatorType.values.min, actuatorType.values.max, - actuatorType.values.defaultVal, function, isMotor)); + new ActuatorTesting::Actuator(&_actuatorTest, label, min_value, actuatorType.values.max, + default_value, function, isMotor, isBidirectional)); found = true; break; } @@ -232,7 +279,7 @@ void Actuators::parametersChanged() const Mixer::ActuatorType& actuatorType = actuatorTypes["DEFAULT"]; actuators.append( new ActuatorTesting::Actuator(&_actuatorTest, label, actuatorType.values.min, actuatorType.values.max, - actuatorType.values.defaultVal, function, false)); + actuatorType.values.defaultVal, function, false, false)); } } } @@ -565,8 +612,11 @@ bool Actuators::parseJson(const QJsonDocument &json) QJsonArray perItemParametersJson = actuatorTypeVal["per-item-parameters"].toArray(); for (const auto&& perItemParameterJson : perItemParametersJson) { QJsonValue perItemParameter = perItemParameterJson.toObject(); + + // Lets print something here to identify Parameter param{}; param.parse(perItemParameter); + qDebug() << "perItemParameter: " << param.name << " " << param.label; actuatorType.perItemParams.append(param); } actuatorTypes[actuatorTypeName] = actuatorType; diff --git a/src/Vehicle/Actuators/Common.cc b/src/Vehicle/Actuators/Common.cc index 18c8d61bb87..ca237ce0156 100644 --- a/src/Vehicle/Actuators/Common.cc +++ b/src/Vehicle/Actuators/Common.cc @@ -27,6 +27,15 @@ void Parameter::parse(const QJsonValue& jsonValue) } else if (displayOptionStr != "") { qCDebug(ActuatorsConfigLog) << "Unknown param display option (show-as):" << displayOptionStr; } + + QString functionStr = jsonValue["function"].toString(); + + if (functionStr == "reversible") { + function = Function::Reversible; + } else if (functionStr == "") { + function = Function::Default; + } + advanced = jsonValue["advanced"].toBool(false); } diff --git a/src/Vehicle/Actuators/Common.h b/src/Vehicle/Actuators/Common.h index 5680653806b..fab1d449a9b 100644 --- a/src/Vehicle/Actuators/Common.h +++ b/src/Vehicle/Actuators/Common.h @@ -32,10 +32,16 @@ struct Parameter { Bitset, ///< integer displayed as boolean (checkbox), where the index defines the bit }; + enum class Function { + Default, + Reversible, + }; + QString label{}; QString name{}; ///< vehicle parameter name, this may have an index in the form '${i}' int indexOffset{}; ///< extra offset to the ${i} index, or bitset shift offset DisplayOption displayOption{DisplayOption::Default}; + Function function{Function::Default}; bool advanced{false}; ///< whether this should only be shown as advanced option void parse(const QJsonValue& jsonValue);