Level Horizon support

src/AutoPilotPlugins/APM/APMSensorsComponent.qml

@@ -157,6 +157,7 @@ SetupPage {
                 compassButton:              compassButton
                 accelButton:                accelButton
                 compassMotButton:           motorInterferenceButton
+                levelButton:                levelHorizonButton
                 nextButton:                 nextButton
                 cancelButton:               cancelButton
                 setOrientationsButton:      setOrientationsButton
@@ -453,6 +454,26 @@ SetupPage {
                 } // QGCViewDialog
             } // Component - compassMotDialogComponent
 
+            Component {
+                id: levelHorizonDialogComponent
+
+                QGCViewDialog {
+                    id: levelHorizonDialog
+
+                    function accept() {
+                        controller.levelHorizon()
+                        levelHorizonDialog.hideDialog()
+                    }
+
+                    QGCLabel {
+                        anchors.left:   parent.left
+                        anchors.right:  parent.right
+                        wrapMode:       Text.WordWrap
+                        text:           qsTr("To level the horizon you need to place the vehicle in its level flight position and press Ok.")
+                    }
+                } // QGCViewDialog
+            } // Component - levelHorizonDialogComponent
+
             Column {
                 spacing:            ScreenTools.defaultFontPixelHeight / 2
                 Layout.alignment:   Qt.AlignLeft | Qt.AlignTop
@@ -483,6 +504,22 @@ SetupPage {
                     }
                 }
 
+                QGCButton {
+                    id:     levelHorizonButton
+                    width:  parent.buttonWidth
+                    text:   _levelHorizonText
+
+                    readonly property string _levelHorizonText: qsTr("Level Horizon")
+
+                    onClicked: {
+                        if (controller.accelSetupNeeded) {
+                            showMessage(_levelHorizonText, qsTr("Accelerometer must be calibrated prior to Level Horizon."), StandardButton.Ok)
+                        } else {
+                            showDialog(levelHorizonDialogComponent, _levelHorizonText, qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok)
+                        }
+                    }
+                }
+
                 QGCButton {
                     id:         motorInterferenceButton
                     width:      parent.buttonWidth

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -26,6 +26,7 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _compassButton(NULL),
     _accelButton(NULL),
     _compassMotButton(NULL),
+    _levelButton(NULL),
     _nextButton(NULL),
     _cancelButton(NULL),
     _setOrientationsButton(NULL),
@@ -33,6 +34,7 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _magCalInProgress(false),
     _accelCalInProgress(false),
     _compassMotCalInProgress(false),
+    _levelInProgress(false),
     _orientationCalDownSideDone(false),
     _orientationCalUpsideDownSideDone(false),
     _orientationCalLeftSideDone(false),
@@ -66,6 +68,8 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
 
     _sensorsComponent = apmPlugin->sensorsComponent();
     connect(_sensorsComponent, &VehicleComponent::setupCompleteChanged, this, &APMSensorsComponentController::setupNeededChanged);
+
+    connect(qgcApp()->toolbox()->mavlinkProtocol(), &MAVLinkProtocol::messageReceived, this, &APMSensorsComponentController::_mavlinkMessageReceived);
 }
 
 /// Appends the specified text to the status log area in the ui
@@ -90,6 +94,7 @@ void APMSensorsComponentController::_startLogCalibration(void)
     _compassButton->setEnabled(false);
     _accelButton->setEnabled(false);
     _compassMotButton->setEnabled(false);
+    _levelButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
     if (_accelCalInProgress || _compassMotCalInProgress) {
         _nextButton->setEnabled(true);
@@ -102,6 +107,7 @@ void APMSensorsComponentController::_startVisualCalibration(void)
     _compassButton->setEnabled(false);
     _accelButton->setEnabled(false);
     _compassMotButton->setEnabled(false);
+    _levelButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
     _cancelButton->setEnabled(true);
 
@@ -145,6 +151,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _compassButton->setEnabled(true);
     _accelButton->setEnabled(true);
     _compassMotButton->setEnabled(true);
+    _levelButton->setEnabled(true);
     _setOrientationsButton->setEnabled(true);
     _nextButton->setEnabled(false);
     _cancelButton->setEnabled(false);
@@ -168,6 +175,10 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
         emit calibrationComplete();
         break;
 
+    case StopCalibrationSuccessShowLog:
+        emit calibrationComplete();
+        break;
+
     case StopCalibrationCancelled:
         emit resetStatusTextArea();
         _hideAllCalAreas();
@@ -183,6 +194,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _magCalInProgress = false;
     _accelCalInProgress = false;
     _compassMotCalInProgress = false;
+    _levelInProgress = false;
 }
 
 void APMSensorsComponentController::calibrateCompass(void)
@@ -210,6 +222,15 @@ void APMSensorsComponentController::calibrateMotorInterference(void)
     _uas->startCalibration(UASInterface::StartCalibrationCompassMot);
 }
 
+void APMSensorsComponentController::levelHorizon(void)
+{
+    _levelInProgress = true;
+    _vehicle->setConnectionLostEnabled(false);
+    _startLogCalibration();
+    _appendStatusLog(tr("Hold the vehicle in its level flight position."));
+    _uas->startCalibration(UASInterface::StartCalibrationLevel);
+}
+
 void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
 {
     Q_UNUSED(compId);
@@ -495,3 +516,30 @@ bool APMSensorsComponentController::usingUDPLink(void)
 {
     return _vehicle->priorityLink()->getLinkConfiguration()->type() == LinkConfiguration::TypeUdp;
 }
+
+void APMSensorsComponentController::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message)
+{
+    Q_UNUSED(link);
+
+    if (message.sysid != _vehicle->id()) {
+        return;
+    }
+
+    if (message.msgid == MAVLINK_MSG_ID_COMMAND_ACK && _levelInProgress) {
+        mavlink_command_ack_t commandAck;
+        mavlink_msg_command_ack_decode(&message, &commandAck);
+
+        if (commandAck.command == MAV_CMD_PREFLIGHT_CALIBRATION) {
+            switch (commandAck.result) {
+            case MAV_RESULT_ACCEPTED:
+                _appendStatusLog(tr("Level horizon complete"));
+                _stopCalibration(StopCalibrationSuccessShowLog);
+                break;
+            default:
+                _appendStatusLog(tr("Level horizon failed"));
+                _stopCalibration(StopCalibrationFailed);
+                break;
+            }
+        }
+    }
+}

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -35,6 +35,7 @@ public:
     Q_PROPERTY(QQuickItem* compassButton MEMBER _compassButton)
     Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
     Q_PROPERTY(QQuickItem* compassMotButton MEMBER _compassMotButton)
+    Q_PROPERTY(QQuickItem* levelButton MEMBER _levelButton)
     Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton)
     Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
     Q_PROPERTY(QQuickItem* setOrientationsButton MEMBER _setOrientationsButton)
@@ -78,6 +79,7 @@ public:
     Q_INVOKABLE void calibrateCompass(void);
     Q_INVOKABLE void calibrateAccel(void);
     Q_INVOKABLE void calibrateMotorInterference(void);
+    Q_INVOKABLE void levelHorizon(void);
     Q_INVOKABLE void cancelCalibration(void);
     Q_INVOKABLE void nextClicked(void);
     Q_INVOKABLE bool usingUDPLink(void);
@@ -99,7 +101,8 @@ signals:
     
 private slots:
     void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
-    
+    void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
+
 private:
     void _startLogCalibration(void);
     void _startVisualCalibration(void);
@@ -110,6 +113,7 @@ private:
     
     enum StopCalibrationCode {
         StopCalibrationSuccess,
+        StopCalibrationSuccessShowLog,
         StopCalibrationFailed,
         StopCalibrationCancelled
     };
@@ -125,6 +129,7 @@ private:
     QQuickItem* _compassButton;
     QQuickItem* _accelButton;
     QQuickItem* _compassMotButton;
+    QQuickItem* _levelButton;
     QQuickItem* _nextButton;
     QQuickItem* _cancelButton;
     QQuickItem* _setOrientationsButton;
@@ -135,6 +140,7 @@ private:
     bool _magCalInProgress;
     bool _accelCalInProgress;
     bool _compassMotCalInProgress;
+    bool _levelInProgress;
     
     bool _orientationCalDownSideDone;
     bool _orientationCalUpsideDownSideDone;