From 12c771520f511e0930ec57dca880df1fdb50d61e Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 27 Jun 2016 19:29:08 -0700
Subject: [PATCH] Fix airspeed checking (#3672)

---
 src/AutoPilotPlugins/PX4/SensorsComponent.cc | 23 ++++++++++++++------
 src/AutoPilotPlugins/PX4/SensorsComponent.h  |  2 ++
 2 files changed, 18 insertions(+), 7 deletions(-)

diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.cc b/src/AutoPilotPlugins/PX4/SensorsComponent.cc
index 34c8dba8e..7645f81b4 100644
--- a/src/AutoPilotPlugins/PX4/SensorsComponent.cc
+++ b/src/AutoPilotPlugins/PX4/SensorsComponent.cc
@@ -16,13 +16,14 @@
 #include "QGCQmlWidgetHolder.h"
 #include "SensorsComponentController.h"
 
-const char* SensorsComponent::_airspeedBreaker = "CBRK_AIRSPD_CHK";
+const char* SensorsComponent::_airspeedBreaker =    "CBRK_AIRSPD_CHK";
+const char* SensorsComponent::_airspeedCal =        "SENS_DPRES_OFF";
 
 SensorsComponent::SensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent) :
     VehicleComponent(vehicle, autopilot, parent),
     _name(tr("Sensors"))
 {
-
+    _deviceIds << QStringLiteral("CAL_MAG0_ID") << QStringLiteral("CAL_GYRO0_ID") << QStringLiteral("CAL_ACC0_ID");
 }
 
 QString SensorsComponent::name(void) const
@@ -48,12 +49,20 @@ bool SensorsComponent::requiresSetup(void) const
 
 bool SensorsComponent::setupComplete(void) const
 {
-    foreach(const QString &triggerParam, setupCompleteChangedTriggerList()) {
-        if (triggerParam != _airspeedBreaker && _autopilot->getParameterFact(FactSystem::defaultComponentId, triggerParam)->rawValue().toFloat() == 0.0f) {
+    foreach (const QString &triggerParam, _deviceIds) {
+        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, triggerParam)->rawValue().toFloat() == 0.0f) {
             return false;
         }
     }
 
+    if (_vehicle->fixedWing() || _vehicle->vtol()) {
+        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, _airspeedBreaker)->rawValue().toInt() != 162128) {
+            if (_autopilot->getParameterFact(FactSystem::defaultComponentId, _airspeedCal)->rawValue().toFloat() == 0.0f) {
+                return false;
+            }
+        }
+    }
+
     return true;
 }
 
@@ -61,9 +70,9 @@ QStringList SensorsComponent::setupCompleteChangedTriggerList(void) const
 {
     QStringList triggers;
     
-    triggers << "CAL_MAG0_ID" << "CAL_GYRO0_ID" << "CAL_ACC0_ID" << "CBRK_AIRSPD_CHK";
-    if ((_vehicle->fixedWing() || _vehicle->vtol()) && _autopilot->getParameterFact(FactSystem::defaultComponentId, _airspeedBreaker)->rawValue().toInt() != 162128) {
-        triggers << "SENS_DPRES_OFF";
+    triggers << _deviceIds;
+    if (_vehicle->fixedWing() || _vehicle->vtol()) {
+        triggers << _airspeedCal << _airspeedBreaker;
     }
     
     return triggers;
diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.h b/src/AutoPilotPlugins/PX4/SensorsComponent.h
index 514ce631f..7525420bd 100644
--- a/src/AutoPilotPlugins/PX4/SensorsComponent.h
+++ b/src/AutoPilotPlugins/PX4/SensorsComponent.h
@@ -40,8 +40,10 @@ public:
 private:
     const QString   _name;
     QVariantList    _summaryItems;
+    QStringList     _deviceIds;
 
     static const char* _airspeedBreaker;
+    static const char* _airspeedCal;
 };
 
 #endif
-- 
2.22.0