From 4d1dcceba16f37786e84dd86bffb60f29796193d Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 11 Jun 2024 07:38:50 -0700 Subject: [PATCH] Rework PX4 airspeed cal due to parameter name/functionality changes --- src/AutoPilotPlugins/APM/APMRadioComponent.cc | 2 +- src/AutoPilotPlugins/AutoPilotPlugin.cc | 2 +- src/AutoPilotPlugins/AutoPilotPlugin.h | 4 +- src/AutoPilotPlugins/PX4/SensorsComponent.cc | 57 ++++++++++++++----- src/AutoPilotPlugins/PX4/SensorsComponent.h | 11 ++-- .../PX4/SensorsComponentSummaryFixedWing.qml | 10 +--- src/AutoPilotPlugins/PX4/SensorsSetup.qml | 10 ++-- src/VehicleSetup/VehicleComponent.cc | 2 +- src/VehicleSetup/VehicleComponent.h | 4 +- src/VehicleSetup/VehicleSummary.qml | 2 + 10 files changed, 66 insertions(+), 38 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMRadioComponent.cc b/src/AutoPilotPlugins/APM/APMRadioComponent.cc index 19f40b60b32..c1a30d19811 100644 --- a/src/AutoPilotPlugins/APM/APMRadioComponent.cc +++ b/src/AutoPilotPlugins/APM/APMRadioComponent.cc @@ -124,7 +124,7 @@ void APMRadioComponent::_connectSetupTriggers(void) void APMRadioComponent::_triggerChanged(void) { - emit setupCompleteChanged(setupComplete()); + emit setupCompleteChanged(); // Control mapping may have changed so we need to reset triggers _connectSetupTriggers(); diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.cc b/src/AutoPilotPlugins/AutoPilotPlugin.cc index 0a37d0429e9..6821a699693 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -49,7 +49,7 @@ void AutoPilotPlugin::_recalcSetupComplete(void) if (_setupComplete != newSetupComplete) { _setupComplete = newSetupComplete; - emit setupCompleteChanged(_setupComplete); + emit setupCompleteChanged(); } } diff --git a/src/AutoPilotPlugins/AutoPilotPlugin.h b/src/AutoPilotPlugins/AutoPilotPlugin.h index 44174275408..3f9ae919ad9 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -50,8 +50,8 @@ class AutoPilotPlugin : public QObject bool setupComplete(void) const; signals: - void setupCompleteChanged(bool setupComplete); - void vehicleComponentsChanged(void); + void setupCompleteChanged (void); + void vehicleComponentsChanged (void); protected: /// All access to AutoPilotPugin objects is through getInstanceForAutoPilotPlugin diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.cc b/src/AutoPilotPlugins/PX4/SensorsComponent.cc index ee44b2ca13c..91c0f6178e0 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc @@ -7,19 +7,11 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "SensorsComponent.h" #include "FactSystem.h" #include "ParameterManager.h" #include "Vehicle.h" -const char* SensorsComponent::_airspeedBreakerParam = "CBRK_AIRSPD_CHK"; -const char* SensorsComponent::_airspeedDisabledParam = "FW_ARSP_MODE"; -const char* SensorsComponent::_airspeedCalParam = "SENS_DPRES_OFF"; - const char* SensorsComponent::_magEnabledParam = "SYS_HAS_MAG"; const char* SensorsComponent::_magCalParam = "CAL_MAG0_ID"; @@ -28,6 +20,15 @@ SensorsComponent::SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, _name(tr("Sensors")) { _deviceIds = QStringList({QStringLiteral("CAL_GYRO0_ID"), QStringLiteral("CAL_ACC0_ID") }); + + if (_vehicle->fixedWing() || _vehicle->vtol() || _vehicle->airship()) { + _airspeedCalTriggerParams << "SENS_DPRES_OFF"; + if (_vehicle->firmwareMajorVersion() >= 1 && _vehicle->firmwareMinorVersion() >= 14) { + _airspeedCalTriggerParams << "SYS_HAS_NUM_ASPD"; + } else { + _airspeedCalTriggerParams << "FW_ARSP_MODE" << "CBRK_AIRSPD_CHK"; + } + } } QString SensorsComponent::name(void) const @@ -67,10 +68,17 @@ bool SensorsComponent::setupComplete(void) const } if (_vehicle->fixedWing() || _vehicle->vtol() || _vehicle->airship()) { - if (!_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _airspeedDisabledParam)->rawValue().toBool() && - _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _airspeedBreakerParam)->rawValue().toInt() != 162128 && - _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, _airspeedCalParam)->rawValue().toFloat() == 0.0f) { - return false; + if (_vehicle->firmwareMajorVersion() >= 1 && _vehicle->firmwareMinorVersion() >= 14) { + if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "SYS_HAS_NUM_ASPD")->rawValue().toBool() && + _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF")->rawValue().toFloat() == 0.0f) { + return false; + } + } else { + if (!_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FW_ARSP_MODE")->rawValue().toBool() && + _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "CBRK_AIRSPD_CHK")->rawValue().toInt() != 162128 && + _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF")->rawValue().toFloat() == 0.0f) { + return false; + } } } @@ -83,7 +91,7 @@ QStringList SensorsComponent::setupCompleteChangedTriggerList(void) const triggers << _deviceIds << _magCalParam << _magEnabledParam; if (_vehicle->fixedWing() || _vehicle->vtol() || _vehicle->airship()) { - triggers << _airspeedCalParam << _airspeedBreakerParam; + triggers << _airspeedCalTriggerParams; } return triggers; @@ -106,3 +114,26 @@ QUrl SensorsComponent::summaryQmlSource(void) const return QUrl::fromUserInput(summaryQml); } + + bool SensorsComponent::_airspeedCalSupported(void) const + { + if (_vehicle->fixedWing() || _vehicle->vtol() || _vehicle->airship()) { + if (_vehicle->firmwareMajorVersion() >= 1 && _vehicle->firmwareMinorVersion() >= 14) { + if (_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "SYS_HAS_NUM_ASPD")->rawValue().toBool()) { + return true; + } + } else { + if (!_vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "FW_ARSP_MODE")->rawValue().toBool() && + _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "CBRK_AIRSPD_CHK")->rawValue().toInt() != 162128) { + return true; + } + } + } + + return false; + } + + bool SensorsComponent::_airspeedCalRequired(void) const + { + return _airspeedCalSupported() && _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, "SENS_DPRES_OFF")->rawValue().toFloat() == 0.0f; + } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.h b/src/AutoPilotPlugins/PX4/SensorsComponent.h index f72b4255f3d..2e880c144fc 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.h +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.h @@ -24,6 +24,9 @@ class SensorsComponent : public VehicleComponent public: SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = nullptr); + Q_PROPERTY(bool airspeedCalSupported READ _airspeedCalSupported STORED false NOTIFY setupCompleteChanged) + Q_PROPERTY(bool airspeedCalRequired READ _airspeedCalRequired STORED false NOTIFY setupCompleteChanged) + // Virtuals from VehicleComponent QStringList setupCompleteChangedTriggerList(void) const override; @@ -37,13 +40,13 @@ class SensorsComponent : public VehicleComponent virtual QUrl summaryQmlSource(void) const override; private: + bool _airspeedCalSupported (void) const; + bool _airspeedCalRequired (void) const; + const QString _name; QVariantList _summaryItems; QStringList _deviceIds; - - static const char* _airspeedDisabledParam; - static const char* _airspeedBreakerParam; - static const char* _airspeedCalParam; + QStringList _airspeedCalTriggerParams; static const char* _magEnabledParam; static const char* _magCalParam; diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml b/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml index f6d3ac3eb5f..523857fea55 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponentSummaryFixedWing.qml @@ -18,12 +18,6 @@ Item { property Fact mag0IdFact: controller.getParameterFact(-1, "CAL_MAG0_ID") property Fact gyro0IdFact: controller.getParameterFact(-1, "CAL_GYRO0_ID") property Fact accel0IdFact: controller.getParameterFact(-1, "CAL_ACC0_ID") - property Fact dpressOffFact: controller.getParameterFact(-1, "SENS_DPRES_OFF") - property Fact airspeedDisabledFact: controller.getParameterFact(-1, "FW_ARSP_MODE") - property Fact airspeedBreakerFact: controller.getParameterFact(-1, "CBRK_AIRSPD_CHK") - - property bool _airspeedVisible: airspeedDisabledFact.value == 0 && airspeedBreakerFact.value !== 162128 - property bool _airspeedCalRequired: _airspeedVisible && dpressOffFact.value === 0 Column { anchors.fill: parent @@ -45,8 +39,8 @@ Item { VehicleSummaryRow { labelText: qsTr("Airspeed:") - visible: _airspeedVisible - valueText: _airspeedCalRequired ? qsTr("Setup required") : qsTr("Ready") + visible: vehicleComponent.airspeedCalSupported + valueText: vehicleComponent.airspeedCalRequired ? qsTr("Setup required") : qsTr("Ready") } } } diff --git a/src/AutoPilotPlugins/PX4/SensorsSetup.qml b/src/AutoPilotPlugins/PX4/SensorsSetup.qml index a30231c9acd..7cbfef11b09 100644 --- a/src/AutoPilotPlugins/PX4/SensorsSetup.qml +++ b/src/AutoPilotPlugins/PX4/SensorsSetup.qml @@ -450,11 +450,9 @@ Item { id: airspeedButton width: _buttonWidth text: qsTr("Airspeed") - visible: (controller.vehicle.fixedWing || controller.vehicle.vtol || controller.vehicle.airship) && - controller.getParameterFact(-1, "FW_ARSP_MODE").value == 0 && - controller.getParameterFact(-1, "CBRK_AIRSPD_CHK").value !== 162128 && - QGroundControl.corePlugin.options.showSensorCalibrationAirspeed && - showSensorCalibrationAirspeed + visible: vehicleComponent.airspeedCalSupported && + QGroundControl.corePlugin.options.showSensorCalibrationAirspeed && + showSensorCalibrationAirspeed indicatorGreen: sens_dpres_off.value !== 0 onClicked: { @@ -522,7 +520,7 @@ Item { height: parent.height readOnly: true text: statusTextAreaDefaultText - color: qgcPal.Text + color: qgcPal.text background: Rectangle { color: qgcPal.windowShade } } diff --git a/src/VehicleSetup/VehicleComponent.cc b/src/VehicleSetup/VehicleComponent.cc index 35a2d5ace54..f53540065dc 100644 --- a/src/VehicleSetup/VehicleComponent.cc +++ b/src/VehicleSetup/VehicleComponent.cc @@ -69,5 +69,5 @@ void VehicleComponent::setupTriggerSignals(void) void VehicleComponent::_triggerUpdated(QVariant /*value*/) { - emit setupCompleteChanged(setupComplete()); + emit setupCompleteChanged(); } diff --git a/src/VehicleSetup/VehicleComponent.h b/src/VehicleSetup/VehicleComponent.h index 2a39f2e51fc..3e54b93fab3 100644 --- a/src/VehicleSetup/VehicleComponent.h +++ b/src/VehicleSetup/VehicleComponent.h @@ -69,8 +69,8 @@ class VehicleComponent : public QObject virtual void setupTriggerSignals(void); signals: - void setupCompleteChanged(bool setupComplete); - void setupSourceChanged(void); + void setupCompleteChanged (void); + void setupSourceChanged (void); protected slots: void _triggerUpdated(QVariant value); diff --git a/src/VehicleSetup/VehicleSummary.qml b/src/VehicleSetup/VehicleSummary.qml index 7b8a29bae59..8fae5067680 100644 --- a/src/VehicleSetup/VehicleSummary.qml +++ b/src/VehicleSetup/VehicleSummary.qml @@ -145,6 +145,8 @@ Rectangle { anchors.fill: parent anchors.margins: ScreenTools.defaultFontPixelWidth source: modelData.summaryQmlSource + + property var vehicleComponent: modelData } } }