Merge pull request #2388 from DonLakeFlyer/APMTrigger

Better setup required triggers for APM Compass and Accel

src/AutoPilotPlugins/APM/APMSensorsComponent.cc

@@ -57,13 +57,7 @@ bool APMSensorsComponent::requiresSetup(void) const
 
 bool APMSensorsComponent::setupComplete(void) const
 {
-    foreach(QString triggerParam, setupCompleteChangedTriggerList()) {
-        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, triggerParam)->rawValue().toFloat() == 0.0f) {
-            return false;
-        }
-    }
-
-    return true;
+    return !compassSetupNeeded() && !accelSetupNeeded();
 }
 
 QString APMSensorsComponent::setupStateDescription(void) const
@@ -82,8 +76,17 @@ QStringList APMSensorsComponent::setupCompleteChangedTriggerList(void) const
 {
     QStringList triggers;
     
-    triggers << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X"
-             << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z";
+    // Compass triggers
+    triggers << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3"
+             << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X"
+             << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X"
+             << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X";
+
+    // Acceleromter triggers
+    triggers << "INS_USE" << "INS_USE2" << "INS_USE3"
+             << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z"
+             << "INS_ACC2OFFS_X" << "INS_ACC2OFFS_Y" << "INS_ACC2OFFS_Z"
+             << "INS_ACC3OFFS_X" << "INS_ACC3OFFS_Y" << "INS_ACC3OFFS_Z";
 
     return triggers;
 }
@@ -114,3 +117,54 @@ QString APMSensorsComponent::prerequisiteSetup(void) const
     
     return QString();
 }
+
+bool APMSensorsComponent::compassSetupNeeded(void) const
+{
+    const size_t cCompass = 3;
+    const size_t cOffset = 3;
+    QStringList devicesIds;
+    QStringList rgOffsets[cCompass];
+
+    devicesIds << "COMPASS_DEV_ID" << "COMPASS_DEV_ID2" << "COMPASS_DEV_ID3";
+    rgOffsets[0] << "COMPASS_OFS_X" << "COMPASS_OFS_X" << "COMPASS_OFS_X";
+    rgOffsets[1] << "COMPASS_OFS2_X" << "COMPASS_OFS2_X" << "COMPASS_OFS2_X";
+    rgOffsets[2] << "COMPASS_OFS3_X" << "COMPASS_OFS3_X" << "COMPASS_OFS3_X";
+
+    for (size_t i=0; i<cCompass; i++) {
+        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, devicesIds[i])->rawValue().toInt() != 0) {
+            for (size_t j=0; j<cOffset; j++) {
+                if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+
+bool APMSensorsComponent::accelSetupNeeded(void) const
+{
+    const size_t cAccel = 3;
+    const size_t cOffset = 3;
+    QStringList insUse;
+    QStringList rgOffsets[cAccel];
+
+    insUse << "INS_USE" << "INS_USE2" << "INS_USE3";
+    rgOffsets[0] << "INS_ACCOFFS_X" << "INS_ACCOFFS_Y" << "INS_ACCOFFS_Z";
+    rgOffsets[1] << "INS_ACC2OFFS_X" << "INS_ACC2OFFS_Y" << "INS_ACC2OFFS_Z";
+    rgOffsets[2] << "INS_ACC3OFFS_X" << "INS_ACC3OFFS_Y" << "INS_ACC3OFFS_Z";
+
+    for (size_t i=0; i<cAccel; i++) {
+        if (_autopilot->getParameterFact(FactSystem::defaultComponentId, insUse[i])->rawValue().toInt() != 0) {
+            for (size_t j=0; j<cOffset; j++) {
+                if (_autopilot->getParameterFact(FactSystem::defaultComponentId, rgOffsets[i][j])->rawValue().toFloat() == 0.0f) {
+                    return true;
+                }
+            }
+        }
+    }
+
+    return false;
+}
+

src/AutoPilotPlugins/APM/APMSensorsComponent.h

@@ -32,7 +32,10 @@ class APMSensorsComponent : public APMComponent
     
 public:
     APMSensorsComponent(Vehicle* vehicle, AutoPilotPlugin* autopilot, QObject* parent = NULL);
-    
+
+    bool compassSetupNeeded(void) const;
+    bool accelSetupNeeded(void) const;
+
     // Virtuals from APMComponent
     virtual QStringList setupCompleteChangedTriggerList(void) const;
     

src/AutoPilotPlugins/APM/APMSensorsComponent.qml

@@ -112,24 +112,8 @@ QGCView {
 
     property Fact sens_board_rot:   controller.getParameterFact(-1, "AHRS_ORIENTATION")
 
-    /*
-    property Fact cal_gyro0_id:     controller.getParameterFact(-1, "CAL_GYRO0_ID")
-    property Fact cal_acc0_id:      controller.getParameterFact(-1, "CAL_ACC0_ID")
-
-    property Fact sens_board_x_off: controller.getParameterFact(-1, "SENS_BOARD_X_OFF")
-    property Fact sens_dpres_off:   controller.getParameterFact(-1, "SENS_DPRES_OFF")
-    */
-
-    property Fact ins_accoffs_x:    controller.getParameterFact(-1, "INS_ACCOFFS_X")
-    property Fact ins_accoffs_y:    controller.getParameterFact(-1, "INS_ACCOFFS_Y")
-    property Fact ins_accoffs_z:    controller.getParameterFact(-1, "INS_ACCOFFS_Z")
-
-    property Fact compass_ofs_x:    controller.getParameterFact(-1, "COMPASS_OFS_X")
-    property Fact compass_ofs_y:    controller.getParameterFact(-1, "COMPASS_OFS_Y")
-    property Fact compass_ofs_z:    controller.getParameterFact(-1, "COMPASS_OFS_Z")
-
-    property bool accelCalNeeded:   ins_accoffs_x.value == 0 && ins_accoffs_y.value == 0 && ins_accoffs_z.value == 0
-    property bool compassCalNeeded: compass_ofs_x.value == 0 && compass_ofs_y.value == 0 && compass_ofs_y.value == 0
+    property bool accelCalNeeded:   controller.accelSetupNeeded
+    property bool compassCalNeeded: controller.compassSetupNeeded
 
     // Id > = signals compass available, rot < 0 signals internal compass
     property bool showCompass0Rot: cal_mag0_id.value > 0 && cal_mag0_rot.value >= 0

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -25,6 +25,7 @@
 #include "QGCMAVLink.h"
 #include "UAS.h"
 #include "QGCApplication.h"
+#include "APMAutoPilotPlugin.h"
 
 #include <QVariant>
 #include <QQmlProperty>
@@ -68,7 +69,10 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _orientationCalTailDownSideRotate(false),
     _waitingForCancel(false)
 {
+    APMAutoPilotPlugin * apmPlugin = qobject_cast<APMAutoPilotPlugin*>(_vehicle->autopilotPlugin());
 
+    _sensorsComponent = apmPlugin->sensorsComponent();
+    connect(apmPlugin, &APMAutoPilotPlugin::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged);
 }
 
 /// Appends the specified text to the status log area in the ui
@@ -180,12 +184,6 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _gyroCalInProgress = false;
 }
 
-void APMSensorsComponentController::calibrateGyro(void)
-{
-    _startLogCalibration();
-    _uas->startCalibration(UASInterface::StartCalibrationGyro);
-}
-
 void APMSensorsComponentController::calibrateCompass(void)
 {
     _startLogCalibration();
@@ -198,18 +196,6 @@ void APMSensorsComponentController::calibrateAccel(void)
     _uas->startCalibration(UASInterface::StartCalibrationAccel);
 }
 
-void APMSensorsComponentController::calibrateLevel(void)
-{
-    _startLogCalibration();
-    _uas->startCalibration(UASInterface::StartCalibrationLevel);
-}
-
-void APMSensorsComponentController::calibrateAirspeed(void)
-{
-    _startLogCalibration();
-    _uas->startCalibration(UASInterface::StartCalibrationAirspeed);
-}
-
 void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
 {
     Q_UNUSED(compId);
@@ -467,3 +453,13 @@ void APMSensorsComponentController::nextClicked(void)
 
     _vehicle->sendMessage(msg);
 }
+
+bool APMSensorsComponentController::compassSetupNeeded(void) const
+{
+    return _sensorsComponent->compassSetupNeeded();
+}
+
+bool APMSensorsComponentController::accelSetupNeeded(void) const
+{
+    return _sensorsComponent->accelSetupNeeded();
+}

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -29,6 +29,7 @@
 #include "UASInterface.h"
 #include "FactPanelController.h"
 #include "QGCLoggingCategory.h"
+#include "APMSensorsComponent.h"
 
 Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog)
 
@@ -50,7 +51,10 @@ public:
     Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
     Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
     Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText)
-    
+
+    Q_PROPERTY(bool compassSetupNeeded  READ compassSetupNeeded NOTIFY setupNeededChanged)
+    Q_PROPERTY(bool accelSetupNeeded    READ accelSetupNeeded   NOTIFY setupNeededChanged)
+
     Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged)
     
     Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged)
@@ -84,14 +88,14 @@ public:
     Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
     
     Q_INVOKABLE void calibrateCompass(void);
-    Q_INVOKABLE void calibrateGyro(void);
     Q_INVOKABLE void calibrateAccel(void);
-    Q_INVOKABLE void calibrateLevel(void);
-    Q_INVOKABLE void calibrateAirspeed(void);
     Q_INVOKABLE void cancelCalibration(void);
     Q_INVOKABLE void nextClicked(void);
 
     bool fixedWing(void);
+
+    bool compassSetupNeeded(void) const;
+    bool accelSetupNeeded(void) const;
     
 signals:
     void showGyroCalAreaChanged(void);
@@ -103,6 +107,7 @@ signals:
     void resetStatusTextArea(void);
     void waitingForCancelChanged(void);
     void setCompassRotations(void);
+    void setupNeededChanged(void);
     
 private slots:
     void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
@@ -124,6 +129,8 @@ private:
     
     void _updateAndEmitShowOrientationCalArea(bool show);
 
+    APMSensorsComponent*    _sensorsComponent;
+
     QQuickItem* _statusLog;
     QQuickItem* _progressBar;
     QQuickItem* _compassButton;

src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml

-import QtQuick 2.2
-import QtQuick.Controls 1.2
-import QtQuick.Controls.Styles 1.2
+import QtQuick                  2.5
+import QtQuick.Controls         1.2
+import QtQuick.Controls.Styles  1.2
 
-import QGroundControl.FactSystem 1.0
-import QGroundControl.FactControls 1.0
-import QGroundControl.Controls 1.0
-import QGroundControl.Palette 1.0
+import QGroundControl.FactSystem    1.0
+import QGroundControl.FactControls  1.0
+import QGroundControl.Controls      1.0
+import QGroundControl.Palette       1.0
+import QGroundControl.Controllers   1.0
 
 /*
     IMPORTANT NOTE: Any changes made here must also be made to SensorsComponentSummary.qml
@@ -17,18 +18,10 @@ FactPanel {
     color:          qgcPal.windowShadeDark
 
     QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
-    FactPanelController { id: controller; factPanel: panel }
+    APMSensorsComponentController { id: controller; factPanel: panel }
 
-    property Fact ins_accoffs_x:    controller.getParameterFact(-1, "INS_ACCOFFS_X")
-    property Fact ins_accoffs_y:    controller.getParameterFact(-1, "INS_ACCOFFS_Y")
-    property Fact ins_accoffs_z:    controller.getParameterFact(-1, "INS_ACCOFFS_Z")
-
-    property Fact compass_ofs_x:    controller.getParameterFact(-1, "COMPASS_OFS_X")
-    property Fact compass_ofs_y:    controller.getParameterFact(-1, "COMPASS_OFS_Y")
-    property Fact compass_ofs_z:    controller.getParameterFact(-1, "COMPASS_OFS_Z")
-
-    property bool accelCalNeeded:   ins_accoffs_x.value == 0 && ins_accoffs_y.value == 0 && ins_accoffs_z.value == 0
-    property bool compassCalNeeded: compass_ofs_x.value == 0 && compass_ofs_y.value == 0 && compass_ofs_y.value == 0
+    property bool accelCalNeeded:   controller.accelSetupNeeded
+    property bool compassCalNeeded: controller.compassSetupNeeded
 
     Column {
         anchors.fill:       parent

src/comm/APMArduCopterMockLink.params

@@ -187,9 +187,9 @@
 1	1	GPS_SBP_LOGMASK	-256	4
 1	1	GPS_TYPE	1	2
 1	1	GPS_TYPE2	0	2
-1	1	INS_ACC2OFFS_X	0	9
-1	1	INS_ACC2OFFS_Y	0	9
-1	1	INS_ACC2OFFS_Z	0	9
+1	1	INS_ACC2OFFS_X	10	9
+1	1	INS_ACC2OFFS_Y	10	9
+1	1	INS_ACC2OFFS_Z	10	9
 1	1	INS_ACC2SCAL_X	1	9
 1	1	INS_ACC2SCAL_Y	1	9
 1	1	INS_ACC2SCAL_Z	1	9

src/comm/APMArduPlaneMockLink.params

@@ -231,9 +231,9 @@
 1	1	HIL_MODE	0	2
 1	1	HIL_SERVOS	0	2
 1	1	INITIAL_MODE	0	2
-1	1	INS_ACC2OFFS_X	0	9
-1	1	INS_ACC2OFFS_Y	0	9
-1	1	INS_ACC2OFFS_Z	0	9
+1	1	INS_ACC2OFFS_X	10	9
+1	1	INS_ACC2OFFS_Y	10	9
+1	1	INS_ACC2OFFS_Z	10	9
 1	1	INS_ACC2SCAL_X	1	9
 1	1	INS_ACC2SCAL_Y	1	9
 1	1	INS_ACC2SCAL_Z	1	9