Ardupilot onboard compass cal support

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -20,6 +20,7 @@
 #include "APMCompassCal.h"
 
 Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog)
+Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerVerboseLog)
 
 /// Sensors Component MVC Controller for SensorsComponent.qml.
 class APMSensorsComponentController : public FactPanelController
@@ -28,6 +29,7 @@ class APMSensorsComponentController : public FactPanelController
     
 public:
     APMSensorsComponentController(void);
+    ~APMSensorsComponentController();
 
     Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog)
     Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar)
@@ -75,7 +77,15 @@ public:
     Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged)
     
     Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
-    
+
+    Q_PROPERTY(bool compass1CalSucceeded READ compass1CalSucceeded NOTIFY compass1CalSucceededChanged)
+    Q_PROPERTY(bool compass2CalSucceeded READ compass2CalSucceeded NOTIFY compass2CalSucceededChanged)
+    Q_PROPERTY(bool compass3CalSucceeded READ compass3CalSucceeded NOTIFY compass3CalSucceededChanged)
+
+    Q_PROPERTY(double compass1CalFitness READ compass1CalFitness NOTIFY compass1CalFitnessChanged)
+    Q_PROPERTY(double compass2CalFitness READ compass2CalFitness NOTIFY compass2CalFitnessChanged)
+    Q_PROPERTY(double compass3CalFitness READ compass3CalFitness NOTIFY compass3CalFitnessChanged)
+
     Q_INVOKABLE void calibrateCompass(void);
     Q_INVOKABLE void calibrateAccel(void);
     Q_INVOKABLE void calibrateMotorInterference(void);
@@ -86,7 +96,25 @@ public:
 
     bool compassSetupNeeded(void) const;
     bool accelSetupNeeded(void) const;
-    
+
+    typedef enum {
+        CalTypeAccel,
+        CalTypeOnboardCompass,
+        CalTypeOffboardCompass,
+        CalTypeLevelHorizon,
+        CalTypeCompassMot,
+        CalTypeNone
+    } CalType_t;
+    Q_ENUM(CalType_t)
+
+    bool compass1CalSucceeded(void) const { return _rgCompassCalSucceeded[0]; }
+    bool compass2CalSucceeded(void) const { return _rgCompassCalSucceeded[1]; }
+    bool compass3CalSucceeded(void) const { return _rgCompassCalSucceeded[2]; }
+
+    double compass1CalFitness(void) const { return _rgCompassCalFitness[0]; }
+    double compass2CalFitness(void) const { return _rgCompassCalFitness[1]; }
+    double compass3CalFitness(void) const { return _rgCompassCalFitness[2]; }
+
 signals:
     void showGyroCalAreaChanged(void);
     void showOrientationCalAreaChanged(void);
@@ -97,11 +125,18 @@ signals:
     void resetStatusTextArea(void);
     void waitingForCancelChanged(void);
     void setupNeededChanged(void);
-    void calibrationComplete(void);
-    
+    void calibrationComplete(CalType_t calType);
+    void compass1CalSucceededChanged(bool compass1CalSucceeded);
+    void compass2CalSucceededChanged(bool compass2CalSucceeded);
+    void compass3CalSucceededChanged(bool compass3CalSucceeded);
+    void compass1CalFitnessChanged(double compass1CalFitness);
+    void compass2CalFitnessChanged(double compass2CalFitness);
+    void compass3CalFitnessChanged(double compass3CalFitness);
+
 private slots:
     void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
+    void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle);
 
 private:
     void _startLogCalibration(void);
@@ -110,7 +145,11 @@ private:
     void _refreshParams(void);
     void _hideAllCalAreas(void);
     void _resetInternalState(void);
-    
+    void _handleCommandAck(mavlink_message_t& message);
+    void _handleMagCalProgress(mavlink_message_t& message);
+    void _handleMagCalReport(mavlink_message_t& message);
+    void _restorePreviousCompassCalFitness(void);
+
     enum StopCalibrationCode {
         StopCalibrationSuccess,
         StopCalibrationSuccessShowLog,
@@ -137,11 +176,13 @@ private:
     
     bool _showOrientationCalArea;
     
-    bool _magCalInProgress;
-    bool _accelCalInProgress;
-    bool _compassMotCalInProgress;
-    bool _levelInProgress;
-    
+    CalType_t _calTypeInProgress;
+
+    uint8_t _rgCompassCalProgress[3];
+    bool    _rgCompassCalComplete[3];
+    bool    _rgCompassCalSucceeded[3];
+    float   _rgCompassCalFitness[3];
+
     bool _orientationCalDownSideDone;
     bool _orientationCalUpsideDownSideDone;
     bool _orientationCalLeftSideDone;
@@ -171,6 +212,10 @@ private:
     bool _orientationCalTailDownSideRotate;
     
     bool _waitingForCancel;
+
+    bool _restoreCompassCalFitness;
+    float _previousCompassCalFitness;
+    static const char* _compassCalFitnessParam;
     
     static const int _supportedFirmwareCalVersion = 2;
 };

src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml

@@ -7,6 +7,7 @@ import QGroundControl.FactControls  1.0
 import QGroundControl.Controls      1.0
 import QGroundControl.Palette       1.0
 import QGroundControl.Controllers   1.0
+import QGroundControl.ArduPilot     1.0
 
 /*
     IMPORTANT NOTE: Any changes made here must also be made to SensorsComponentSummary.qml
@@ -18,55 +19,35 @@ FactPanel {
     color:          qgcPal.windowShadeDark
 
     QGCPalette { id: qgcPal; colorGroupEnabled: enabled }
-    APMSensorsComponentController { id: controller; factPanel: panel }
-
-    property Fact compass1IdFact:   controller.getParameterFact(-1, "COMPASS_DEV_ID")
-    property Fact compass2IdFact:   controller.getParameterFact(-1, "COMPASS_DEV_ID2")
-    property Fact compass3IdFact:   controller.getParameterFact(-1, "COMPASS_DEV_ID3")
-
-    property Fact compass1OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS_X")
-    property Fact compass1OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS_Y")
-    property Fact compass1OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS_Z")
-    property Fact compass2OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS2_X")
-    property Fact compass2OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS2_Y")
-    property Fact compass2OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS2_Z")
-    property Fact compass3OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS3_X")
-    property Fact compass3OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS3_Y")
-    property Fact compass3OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS3_Z")
-
-    property bool compass1Available: compass1IdFact.value !== 0
-    property bool compass2Available: compass2IdFact.value !== 0
-    property bool compass3Available: compass3IdFact.value !== 0
 
-    property bool compass1Calibrated: compass1Available ? compass1OfsXFact.value != 0.0  && compass1OfsYFact.value != 0.0  &&compass1OfsZFact.value != 0.0 : false
-    property bool compass2Calibrated: compass2Available ? compass2OfsXFact.value != 0.0  && compass2OfsYFact.value != 0.0  &&compass2OfsZFact.value != 0.0 : false
-    property bool compass3Calibrated: compass3Available ? compass3OfsXFact.value != 0.0  && compass3OfsYFact.value != 0.0  &&compass3OfsZFact.value != 0.0 : false
+    APMSensorsComponentController { id: controller; factPanel: panel }
 
-    property bool compassCalNeeded: controller.compassSetupNeeded
+    APMSensorParams {
+        id:                     sensorParams
+        factPanelController:    controller
+    }
 
     Column {
         anchors.fill:       parent
 
-        VehicleSummaryRow {
-            labelText: qsTr("Compass 1:")
-            visible:    compass1Available
-            valueText:  compass1Calibrated ? qsTr("Ready") : qsTr("Setup required")
-        }
-
-        VehicleSummaryRow {
-            labelText: qsTr("Compass 2:")
-            visible:    compass2Available
-            valueText:  compass2Calibrated ? qsTr("Ready") : qsTr("Setup required")
-        }
-
-        VehicleSummaryRow {
-            labelText: qsTr("Compass 3:")
-            visible:    compass3Available
-            valueText:  compass3Calibrated ? qsTr("Ready") : qsTr("Setup required")
+        Repeater {
+            model: 3
+
+            VehicleSummaryRow {
+                labelText:  qsTr("Compass ") + (index + 1) + ":"
+                valueText:  sensorParams.rgCompassAvailable[index] ?
+                                (sensorParams.rgCompassCalibrated[index] ?
+                                     (sensorParams.rgCompassPrimary[index] ? "Primary" : "Secondary") +
+                                     (sensorParams.rgCompassExternalParamAvailable[index] ?
+                                          (sensorParams.rgCompassExternal[index] ? ", External" : ", Internal" ) :
+                                          "") :
+                                     qsTr("Setup required")) :
+                                qsTr("Not installed")
+            }
         }
 
         VehicleSummaryRow {
-            labelText: qsTr("Accelerometer:")
+            labelText: qsTr("Accelerometer(s):")
             valueText: controller.accelSetupNeeded ? qsTr("Setup required") : qsTr("Ready")
         }
     }

src/FirmwarePlugin/APM/APMSensorParams.qml

+/****************************************************************************
+ *
+ *   (c) 2009-2016 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ *
+ * QGroundControl is licensed according to the terms in the file
+ * COPYING.md in the root of the source code directory.
+ *
+ ****************************************************************************/
+
+import QtQuick 2.2
+
+import QGroundControl.FactSystem 1.0
+
+/// This is used to handle the various differences between firmware versions and missing parameters in each in a standard way.
+Item {
+    property var factPanelController  ///< Must be specified by consumer of control
+
+    property Fact _noFact: Fact { }
+
+    property Fact compassPrimaryFact:               factPanelController.getParameterFact(-1, "COMPASS_PRIMARY")
+    property bool compass1Primary:                  compassPrimaryFact.rawValue == 0
+    property bool compass2Primary:                  compassPrimaryFact.rawValue == 1
+    property bool compass3Primary:                  compassPrimaryFact.rawValue == 2
+    property var  rgCompassPrimary:                 [ compass1Primary, compass2Primary, compass3Primary ]
+
+    property Fact compass1Id:                       factPanelController.getParameterFact(-1, "COMPASS_DEV_ID")
+    property Fact compass2Id:                       factPanelController.getParameterFact(-1, "COMPASS_DEV_ID2")
+    property Fact compass3Id:                       factPanelController.getParameterFact(-1, "COMPASS_DEV_ID3")
+
+    property bool compass1Available:                compass1Id.value > 0
+    property bool compass2Available:                compass2Id.value > 0
+    property bool compass3Available:                compass3Id.value > 0
+    property var  rgCompassAvailable:               [ compass1Available, compass2Available, compass3Available ]
+
+    property bool compass1RotParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_ORIENT")
+    property bool compass2RotParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_ORIENT2")
+    property bool compass3RotParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_ORIENT3")
+    property var  rgCompassRotParamAvailable:       [ compass1RotParamAvailable, compass2RotParamAvailable, compass3RotParamAvailable ]
+
+    property Fact compass1RotFact:                  compass2RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT") : _noFact
+    property Fact compass2RotFact:                  compass2RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT2") : _noFact
+    property Fact compass3RotFact:                  compass3RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT3") : _noFact
+    property var  rgCompassRotFact:                 [ compass1RotFact, compass2RotFact, compass3RotFact ]
+
+    property bool compass1UseParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_USE")
+    property bool compass2UseParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_USE2")
+    property bool compass3UseParamAvailable:        factPanelController.parameterExists(-1, "COMPASS_USE3")
+    property var  rgCompassUseParamAvailable:       [ compass1UseParamAvailable, compass2UseParamAvailable, compass3UseParamAvailable ]
+
+    property Fact compass1UseFact:                  compass1UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE") : _noFact
+    property Fact compass2UseFact:                  compass2UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE2") : _noFact
+    property Fact compass3UseFact:                  compass3UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE3") : _noFact
+    property var  rgCompassUseFact:                 [ compass1UseFact, compass2UseFact, compass3UseFact ]
+
+    property bool compass1Use:                      compass1UseParamAvailable ? compass1UseFact.value : true
+    property bool compass2Use:                      compass2UseParamAvailable ? compass2UseFact.value : true
+    property bool compass3Use:                      compass3UseParamAvailable ? compass3UseFact.value : true
+
+    property bool compass1ExternalParamAvailable:   factPanelController.parameterExists(-1, "COMPASS_EXTERNAL")
+    property bool compass2ExternalParamAvailable:   factPanelController.parameterExists(-1, "COMPASS_EXTERN2")
+    property bool compass3ExternalParamAvailable:   factPanelController.parameterExists(-1, "COMPASS_EXTERN3")
+    property var  rgCompassExternalParamAvailable:  [ compass1ExternalParamAvailable, compass2ExternalParamAvailable, compass3ExternalParamAvailable ]
+
+    property Fact compass1ExternalFact:             compass1ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERNAL") : _noFact
+    property Fact compass2ExternalFact:             compass2ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERN2") : _noFact
+    property Fact compass3ExternalFact:             compass3ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERN3") : _noFact
+
+    property bool compass1External:                 !!compass1ExternalFact.rawValue
+    property bool compass2External:                 !!compass2ExternalFact.rawValue
+    property bool compass3External:                 !!compass3ExternalFact.rawValue
+    property var  rgCompassExternal:                [ compass1External, compass2External, compass3External ]
+
+    property Fact compass1OfsXFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS_X")
+    property Fact compass1OfsYFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS_Y")
+    property Fact compass1OfsZFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS_Z")
+    property Fact compass2OfsXFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS2_X")
+    property Fact compass2OfsYFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS2_Y")
+    property Fact compass2OfsZFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS2_Z")
+    property Fact compass3OfsXFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS3_X")
+    property Fact compass3OfsYFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS3_Y")
+    property Fact compass3OfsZFact:                 factPanelController.getParameterFact(-1, "COMPASS_OFS3_Z")
+
+    property bool compass1Calibrated:               compass1Available ? compass1OfsXFact.value != 0.0  && compass1OfsYFact.value != 0.0  &&compass1OfsZFact.value != 0.0 : false
+    property bool compass2Calibrated:               compass2Available ? compass2OfsXFact.value != 0.0  && compass2OfsYFact.value != 0.0  &&compass2OfsZFact.value != 0.0 : false
+    property bool compass3Calibrated:               compass3Available ? compass3OfsXFact.value != 0.0  && compass3OfsYFact.value != 0.0  &&compass3OfsZFact.value != 0.0 : false
+    property var  rgCompassCalibrated:              [ compass1Calibrated, compass2Calibrated, compass3Calibrated ]
+}

src/FirmwarePlugin/APM/QGroundControl.ArduPilot.qmldir

+Module QGroundControl.ArduPilot
+
+APMSensorParams 1.0 APMSensorParams.qml

src/comm/MockLink.cc

@@ -188,14 +188,14 @@ void MockLink::_loadParams(void)
 
     if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) {
         if (_vehicleType == MAV_TYPE_FIXED_WING) {
-            paramFile.setFileName(":/unittest/APMArduPlaneMockLink.params");
+            paramFile.setFileName(":/MockLink/APMArduPlaneMockLink.params");
         } else if (_vehicleType == MAV_TYPE_SUBMARINE ) {
-            paramFile.setFileName(":/unittest/APMArduSubMockLink.params");
+            paramFile.setFileName(":/MockLink/APMArduSubMockLink.params");
         } else {
-            paramFile.setFileName(":/unittest/APMArduCopterMockLink.params");
+            paramFile.setFileName(":/MockLink/APMArduCopterMockLink.params");
         }
     } else {
-        paramFile.setFileName(":/unittest/PX4MockLink.params");
+        paramFile.setFileName(":/MockLink/PX4MockLink.params");
     }