Add CompassMot support (#3643)

src/AutoPilotPlugins/APM/APMSensorsComponent.qml

@@ -147,6 +147,7 @@ QGCView {
         progressBar:                progressBar
         compassButton:              compassButton
         accelButton:                accelButton
+        compassMotButton:           motorInterferenceButton
         nextButton:                 nextButton
         cancelButton:               cancelButton
         setOrientationsButton:      setOrientationsButton
@@ -171,6 +172,7 @@ QGCView {
                                                 "INS_GYROFFS_X", "INS_GYROFFS_Y", "INS_GYROFFS_Z",
                                                 "INS_GYR2OFFS_X", "INS_GYR2OFFS_Y", "INS_GYR2OFFS_Z",
                                                 "INS_GYR3OFFS_X", "INS_GYR3OFFS_Y", "INS_GYR3OFFS_Z" ]
+                showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
             } else if (_orientationDialogCalType == _calTypeCompass) {
                 _postCalibrationDialogText = qsTr("Compass calibration complete. ")
                 _postCalibrationDialogParams = [];
@@ -198,8 +200,8 @@ QGCView {
                     _postCalibrationDialogParams.push("COMPASS_OFS3_Y")
                     _postCalibrationDialogParams.push("COMPASS_OFS3_Z")
                 }
+                showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
             }
-            showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok)
         }
     }
 
@@ -376,6 +378,72 @@ QGCView {
         } // QGCViewDialog
     } // Component - orientationsDialogComponent
 
+    Component {
+        id: compassMotDialogComponent
+
+        QGCViewDialog {
+            id: compassMotDialog
+
+            function accept() {
+                controller.calibrateMotorInterference()
+                compassMotDialog.hideDialog()
+            }
+
+            QGCFlickable {
+                anchors.fill:   parent
+                contentHeight:  columnLayout.height
+                clip:           true
+
+                Column {
+                    id:                 columnLayout
+                    anchors.margins:    ScreenTools.defaultFontPixelWidth
+                    anchors.left:       parent.left
+                    anchors.right:      parent.right
+                    anchors.top:        parent.top
+                    spacing:            ScreenTools.defaultFontPixelHeight
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           "This is recommended for vehicles that have only an internal compass and on vehicles where there is significant interference on the compass from the motors, power wires, etc. " +
+                                        "CompassMot only works well if you have a battery current monitor because the magnetic interference is linear with current drawn. " +
+                                        "It is technically possible to set-up CompassMot using throttle but this is not recommended."
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           "Disconnect your props, flip them over and rotate them one position around the frame. " +
+                                        "In this configuration they should push the copter down into the ground when the throttle is raised."
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           "Secure the copter (perhaps with tape) so that it does not move."
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           "Turn on your transmitter and keep throttle at zero."
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           "Click Ok to start CompassMot calibration."
+                    }
+                } // Column
+            } // QGCFlickable
+        } // QGCViewDialog
+    }
+
     QGCViewPanel {
         id:             panel
         anchors.fill:   parent
@@ -411,6 +479,13 @@ QGCView {
             }
 
             QGCButton {
+                id:         motorInterferenceButton
+                width:      parent.buttonWidth
+                text:       qsTr("CompassMot")
+                onClicked:  showDialog(compassMotDialogComponent, qsTr("CompassMot - Compass Motor Interference Calibration"), qgcView.showDialogFullWidth, StandardButton.Cancel | StandardButton.Ok)
+            }
+
+            QGCButton {
                 id:         nextButton
                 width:      parent.buttonWidth
                 text:       qsTr("Next")

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -24,12 +24,14 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _progressBar(NULL),
     _compassButton(NULL),
     _accelButton(NULL),
+    _compassMotButton(NULL),
     _nextButton(NULL),
     _cancelButton(NULL),
     _setOrientationsButton(NULL),
     _showOrientationCalArea(false),
     _magCalInProgress(false),
     _accelCalInProgress(false),
+    _compassMotCalInProgress(false),
     _orientationCalDownSideDone(false),
     _orientationCalUpsideDownSideDone(false),
     _orientationCalLeftSideDone(false),
@@ -86,8 +88,9 @@ void APMSensorsComponentController::_startLogCalibration(void)
     
     _compassButton->setEnabled(false);
     _accelButton->setEnabled(false);
+    _compassMotButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
-    if (_accelCalInProgress) {
+    if (_accelCalInProgress || _compassMotCalInProgress) {
         _nextButton->setEnabled(true);
     }
     _cancelButton->setEnabled(false);
@@ -97,6 +100,7 @@ void APMSensorsComponentController::_startVisualCalibration(void)
 {
     _compassButton->setEnabled(false);
     _accelButton->setEnabled(false);
+    _compassMotButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
     _cancelButton->setEnabled(true);
 
@@ -133,14 +137,13 @@ void APMSensorsComponentController::_resetInternalState(void)
 
 void APMSensorsComponentController::_stopCalibration(APMSensorsComponentController::StopCalibrationCode code)
 {
-    if (_accelCalInProgress) {
+    _vehicle->setConnectionLostEnabled(true);
-        _vehicle->setConnectionLostEnabled(true);
-    }
 
     disconnect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage);
     
     _compassButton->setEnabled(true);
     _accelButton->setEnabled(true);
+    _compassMotButton->setEnabled(true);
     _setOrientationsButton->setEnabled(true);
     _nextButton->setEnabled(false);
     _cancelButton->setEnabled(false);
@@ -178,6 +181,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     
     _magCalInProgress = false;
     _accelCalInProgress = false;
+    _compassMotCalInProgress = false;
 }
 
 void APMSensorsComponentController::calibrateCompass(void)
@@ -188,12 +192,23 @@ void APMSensorsComponentController::calibrateCompass(void)
 
 void APMSensorsComponentController::calibrateAccel(void)
 {
+    _accelCalInProgress = true;
     _vehicle->setConnectionLostEnabled(false);
     _startLogCalibration();
-    _accelCalInProgress = true;
     _uas->startCalibration(UASInterface::StartCalibrationAccel);
 }
 
+void APMSensorsComponentController::calibrateMotorInterference(void)
+{
+    _compassMotCalInProgress = true;
+    _vehicle->setConnectionLostEnabled(false);
+    _startLogCalibration();
+    _appendStatusLog(tr("Raise the throttle slowly to between 50% ~ 75% (the props will spin!) for 5 ~ 10 seconds."));
+    _appendStatusLog(tr("Quickly bring the throttle back down to zero"));
+    _appendStatusLog(tr("Press the Next button to complete the calibration"));
+    _uas->startCalibration(UASInterface::StartCalibrationCompassMot);
+}
+
 void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
 {
     Q_UNUSED(compId);
@@ -457,6 +472,10 @@ void APMSensorsComponentController::nextClicked(void)
     mavlink_msg_command_ack_encode(qgcApp()->toolbox()->mavlinkProtocol()->getSystemId(), qgcApp()->toolbox()->mavlinkProtocol()->getComponentId(), &msg, &ack);
 
     _vehicle->sendMessageOnPriorityLink(msg);
+
+    if (_compassMotCalInProgress) {
+        _stopCalibration(StopCalibrationSuccess);
+    }
 }
 
 bool APMSensorsComponentController::compassSetupNeeded(void) const

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -34,6 +34,7 @@ public:
     
     Q_PROPERTY(QQuickItem* compassButton MEMBER _compassButton)
     Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
+    Q_PROPERTY(QQuickItem* compassMotButton MEMBER _compassMotButton)
     Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
     Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
     Q_PROPERTY(QQuickItem* setOrientationsButton MEMBER _setOrientationsButton)
@@ -76,6 +77,7 @@ public:
     
     Q_INVOKABLE void calibrateCompass(void);
     Q_INVOKABLE void calibrateAccel(void);
+    Q_INVOKABLE void calibrateMotorInterference(void);
     Q_INVOKABLE void cancelCalibration(void);
     Q_INVOKABLE void nextClicked(void);
     Q_INVOKABLE bool usingUDPLink(void);
@@ -122,6 +124,7 @@ private:
     QQuickItem* _progressBar;
     QQuickItem* _compassButton;
     QQuickItem* _accelButton;
+    QQuickItem* _compassMotButton;
     QQuickItem* _nextButton;
     QQuickItem* _cancelButton;
     QQuickItem* _setOrientationsButton;
@@ -131,6 +134,7 @@ private:
     
     bool _magCalInProgress;
     bool _accelCalInProgress;
+    bool _compassMotCalInProgress;
     
     bool _orientationCalDownSideDone;
     bool _orientationCalUpsideDownSideDone;

src/uas/UAS.cc

@@ -766,33 +766,36 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
     int escCal = 0;
 
     switch (calType) {
-        case StartCalibrationGyro:
+    case StartCalibrationGyro:
-            gyroCal = 1;
+        gyroCal = 1;
-            break;
+        break;
-        case StartCalibrationMag:
+    case StartCalibrationMag:
-            magCal = 1;
+        magCal = 1;
-            break;
+        break;
-        case StartCalibrationAirspeed:
+    case StartCalibrationAirspeed:
-            airspeedCal = 1;
+        airspeedCal = 1;
-            break;
+        break;
-        case StartCalibrationRadio:
+    case StartCalibrationRadio:
-            radioCal = 1;
+        radioCal = 1;
-            break;
+        break;
-        case StartCalibrationCopyTrims:
+    case StartCalibrationCopyTrims:
-            radioCal = 2;
+        radioCal = 2;
-            break;
+        break;
-        case StartCalibrationAccel:
+    case StartCalibrationAccel:
-            accelCal = 1;
+        accelCal = 1;
-            break;
+        break;
-        case StartCalibrationLevel:
+    case StartCalibrationLevel:
-            accelCal = 2;
+        accelCal = 2;
-            break;
+        break;
-        case StartCalibrationEsc:
+    case StartCalibrationEsc:
-            escCal = 1;
+        escCal = 1;
-            break;
+        break;
-        case StartCalibrationUavcanEsc:
+    case StartCalibrationUavcanEsc:
-            escCal = 2;
+        escCal = 2;
-            break;
+        break;
+    case StartCalibrationCompassMot:
+        airspeedCal = 1; // ArduPilot, bit of a hack
+        break;
     }
 
     mavlink_message_t msg;
@@ -808,7 +811,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
                                   0,                                // ground pressure
                                   radioCal,                         // radio cal
                                   accelCal,                         // accel cal
-                                  airspeedCal,                      // airspeed cal
+                                  airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot
                                   escCal);                          // esc cal
     _vehicle->sendMessageOnPriorityLink(msg);
 }

src/uas/UASInterface.h

@@ -119,7 +119,8 @@ public:
         StartCalibrationLevel,
         StartCalibrationEsc,
         StartCalibrationCopyTrims,
-        StartCalibrationUavcanEsc
+        StartCalibrationUavcanEsc,
+        StartCalibrationCompassMot,
     };
 
     enum StartBusConfigType {