Rework PX4 airspeed cal due to parameter name/functionality changes (#…

…11707) Co-authored-by: Don Gagne <[email protected]>
mavlink · Aug 2, 2024 · 5e3bc45 · 5e3bc45
1 parent 70daedb
commit 5e3bc45
Show file tree

Hide file tree

Showing 10 changed files with 65 additions and 37 deletions.
diff --git 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
@@ -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
@@ -54,8 +54,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
@@ -7,18 +7,10 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <[email protected]>
-
 #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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -68,8 +68,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
@@ -146,6 +146,8 @@ Rectangle {
                                 anchors.fill:       parent
                                 anchors.margins:    ScreenTools.defaultFontPixelWidth
                                 source:             modelData.summaryQmlSource
+
+                                property var vehicleComponent: modelData
                             }
                         }
                     }