From 5e3bc45d625d71478854c3f22fa55d15a01d8474 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 2 Aug 2024 13:20:45 +1200 Subject: [PATCH] Rework PX4 airspeed cal due to parameter name/functionality changes (#11707) Co-authored-by: Don Gagne --- 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 | 8 ++- src/VehicleSetup/VehicleComponent.cc | 2 +- src/VehicleSetup/VehicleComponent.h | 4 +- src/VehicleSetup/VehicleSummary.qml | 2 + 10 files changed, 65 insertions(+), 37 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMRadioComponent.cc b/src/AutoPilotPlugins/APM/APMRadioComponent.cc index 6f84182..a298341 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 b63f609..33089bb 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.cc +++ b/src/AutoPilotPlugins/AutoPilotPlugin.cc @@ -47,7 +47,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 130ab7b..615e5a8 100644 --- a/src/AutoPilotPlugins/AutoPilotPlugin.h +++ b/src/AutoPilotPlugins/AutoPilotPlugin.h @@ -54,8 +54,8 @@ public: 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 9ea71c3..83035fe 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc @@ -7,18 +7,10 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "SensorsComponent.h" #include "PX4AutoPilotPlugin.h" #include "SensorsComponentController.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"; @@ -27,6 +19,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 @@ -66,10 +67,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; + } } } @@ -82,7 +90,7 @@ QStringList SensorsComponent::setupCompleteChangedTriggerList(void) const triggers << _deviceIds << _magCalParam << _magEnabledParam; if (_vehicle->fixedWing() || _vehicle->vtol() || _vehicle->airship()) { - triggers << _airspeedCalParam << _airspeedBreakerParam; + triggers << _airspeedCalTriggerParams; } return triggers; @@ -105,3 +113,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 f72b425..2e880c1 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 @@ public: 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 50c0f8a..e8eace8 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 fef4094..caa5e2f 100644 --- a/src/AutoPilotPlugins/PX4/SensorsSetup.qml +++ b/src/AutoPilotPlugins/PX4/SensorsSetup.qml @@ -449,11 +449,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: { diff --git a/src/VehicleSetup/VehicleComponent.cc b/src/VehicleSetup/VehicleComponent.cc index a590c1f..267de5f 100644 --- a/src/VehicleSetup/VehicleComponent.cc +++ b/src/VehicleSetup/VehicleComponent.cc @@ -64,5 +64,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 1443e15..770e6f7 100644 --- a/src/VehicleSetup/VehicleComponent.h +++ b/src/VehicleSetup/VehicleComponent.h @@ -68,8 +68,8 @@ public: 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 9456c09..f253c05 100644 --- a/src/VehicleSetup/VehicleSummary.qml +++ b/src/VehicleSetup/VehicleSummary.qml @@ -146,6 +146,8 @@ Rectangle { anchors.fill: parent anchors.margins: ScreenTools.defaultFontPixelWidth source: modelData.summaryQmlSource + + property var vehicleComponent: modelData } } }