diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index bb87f6985f408875725ee5f8afd3d805b9d7e526..a0dcf688a5d2548647f5b86c38b04f928363b435 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/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 diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 988ee8863045e803eba0768d490dc66002898bc4..b68c7f90ada89f3d38895df05723962736e409ee 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/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); @@ -171,6 +178,10 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll emit calibrationComplete(); break; + case StopCalibrationSuccessShowLog: + emit calibrationComplete(); + break; + case StopCalibrationCancelled: emit resetStatusTextArea(); _hideAllCalAreas(); @@ -186,6 +197,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll _magCalInProgress = false; _accelCalInProgress = false; _compassMotCalInProgress = false; + _levelInProgress = false; } void APMSensorsComponentController::calibrateCompass(void) @@ -213,6 +225,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); @@ -498,3 +519,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; + } + } + } +} diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index efd4d0338218386e00a6f3611e79ae902ce272dd..ca7e7a350b64f07471e19964261e3ef16fe486a3 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/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;