From 59be6d630ff485afbad29e05c51a8ad671349588 Mon Sep 17 00:00:00 2001 From: DonLakeFlyer Date: Sun, 11 Jun 2017 09:53:06 -0700 Subject: [PATCH] Support preflight barometer calibration --- .../APM/APMSensorsComponent.qml | 184 +++++++++--------- .../APM/APMSensorsComponentController.cc | 28 +-- .../APM/APMSensorsComponentController.h | 13 +- .../Common/RadioComponent.qml | 1 - .../APM/ArduSubFirmwarePlugin.cc | 5 - .../APM/ArduSubFirmwarePlugin.h | 2 - src/FirmwarePlugin/FirmwarePlugin.cc | 5 - src/FirmwarePlugin/FirmwarePlugin.h | 4 - src/Vehicle/Vehicle.cc | 5 - src/Vehicle/Vehicle.h | 2 - 10 files changed, 102 insertions(+), 147 deletions(-) diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index e465e08bd..7c7e3eef7 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -30,10 +30,9 @@ SetupPage { Component { id: sensorsPageComponent - RowLayout { - width: availableWidth - height: availableHeight - spacing: ScreenTools.defaultFontPixelWidth / 2 + Item { + width: availableWidth + height: availableHeight // Help text which is shown both in the status text area prior to pressing a cal button and in the // pre-calibration dialog. @@ -74,10 +73,13 @@ SetupPage { readonly property int _calTypeCompass: 1 ///< Calibrate compass readonly property int _calTypeAccel: 2 ///< Calibrate accel readonly property int _calTypeSet: 3 ///< Set orientations only + readonly property int _buttonWidth: ScreenTools.defaultFontPixelWidth * 15 property bool _orientationsDialogShowCompass: true property string _orientationDialogHelp: orientationHelpSet property int _orientationDialogCalType + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + property real _margins: ScreenTools.defaultFontPixelHeight / 2 function showOrientationsDialog(calType) { var dialogTitle @@ -117,14 +119,8 @@ SetupPage { factPanel: sensorsPage.viewPanel statusLog: statusTextArea progressBar: progressBar - compassButton: compassButton - accelButton: accelButton - compassMotButton: motorInterferenceButton - levelButton: levelHorizonButton - calibratePressureButton: calibratePressureButton nextButton: nextButton cancelButton: cancelButton - setOrientationsButton: setOrientationsButton orientationCalAreaHelpText: orientationCalAreaHelpText property var rgCompassCalFitness: [ controller.compass1CalFitness, controller.compass2CalFitness, controller.compass3CalFitness ] @@ -152,6 +148,10 @@ SetupPage { break } } + + onSetAllCalButtonsEnabled: { + buttonColumn.enabled = enabled + } } Component.onCompleted: { @@ -462,112 +462,122 @@ SetupPage { anchors.left: parent.left anchors.right: parent.right wrapMode: Text.WordWrap - text: qsTr("Pressure calibration will set the depth to zero at the current pressure reading.") + text: _helpText + + readonly property string _altText: _activeVehicle.sub ? qsTr("depth") : qsTr("altitude") + readonly property string _helpText: qsTr("Pressure calibration will set the %1 to zero at the current pressure reading. %2").arg(_altText).arg(_helpTextFW) + readonly property string _helpTextFW: _activeVehicle.fixedWing ? qsTr("Cover airspeed sensor for calibration.") : "" } } // QGCViewDialog } // Component - calibratePressureDialogComponent - /// Left button column - Column { - spacing: ScreenTools.defaultFontPixelHeight / 2 - Layout.alignment: Qt.AlignLeft | Qt.AlignTop + QGCFlickable { + id: buttonFlickable + anchors.left: parent.left + anchors.top: parent.top + anchors.bottom: parent.bottom + width: _buttonWidth + contentHeight: nextCancelColumn.y + nextCancelColumn.height + _margins - readonly property int buttonWidth: ScreenTools.defaultFontPixelWidth * 15 + // Calibration button column - Calibratin buttons are kept in a separate column from Next/Cancel buttons + // so we can enable/disable them all as a group + Column { + id: buttonColumn + spacing: _margins + Layout.alignment: Qt.AlignLeft | Qt.AlignTop - IndicatorButton { - id: accelButton - width: parent.buttonWidth - text: qsTr("Accelerometer") - indicatorGreen: !accelCalNeeded + IndicatorButton { + width: _buttonWidth + text: qsTr("Accelerometer") + indicatorGreen: !accelCalNeeded - onClicked: showOrientationsDialog(_calTypeAccel) - } + onClicked: showOrientationsDialog(_calTypeAccel) + } + + IndicatorButton { + width: _buttonWidth + text: qsTr("Compass") + indicatorGreen: !compassCalNeeded - IndicatorButton { - id: compassButton - width: parent.buttonWidth - text: qsTr("Compass") - indicatorGreen: !compassCalNeeded - - onClicked: { - if (controller.accelSetupNeeded) { - showMessage(qsTr("Calibrate Compass"), qsTr("Accelerometer must be calibrated prior to Compass."), StandardButton.Ok) - } else { - showOrientationsDialog(_calTypeCompass) + onClicked: { + if (controller.accelSetupNeeded) { + showMessage(qsTr("Calibrate Compass"), qsTr("Accelerometer must be calibrated prior to Compass."), StandardButton.Ok) + } else { + showOrientationsDialog(_calTypeCompass) + } } } - } - QGCButton { - id: levelHorizonButton - width: parent.buttonWidth - text: _levelHorizonText + QGCButton { + width: _buttonWidth + text: _levelHorizonText - readonly property string _levelHorizonText: qsTr("Level Horizon") + 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) + 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: calibratePressureButton - width: parent.buttonWidth - text: _calibratePressureText - visible: _activeVehicle ? _activeVehicle.supportsCalibratePressure : false + QGCButton { + width: _buttonWidth + text: _calibratePressureText + onClicked: showDialog(calibratePressureDialogComponent, _calibratePressureText, qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) - readonly property string _calibratePressureText: qsTr("Calibrate Pressure") - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle - - onClicked: { - showDialog(calibratePressureDialogComponent, _calibratePressureText, qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) + readonly property string _calibratePressureText: _activeVehicle.fixedWing ? qsTr("Cal Baro/Airspeed") : qsTr("Calibrate Pressure") } - } - - QGCButton { - id: motorInterferenceButton - width: parent.buttonWidth - text: qsTr("CompassMot") - visible: _activeVehicle ? _activeVehicle.supportsMotorInterference : false - property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + QGCButton { + width: _buttonWidth + text: qsTr("CompassMot") + visible: _activeVehicle ? _activeVehicle.supportsMotorInterference : false - onClicked: showDialog(compassMotDialogComponent, qsTr("CompassMot - Compass Motor Interference Calibration"), qgcView.showDialogFullWidth, StandardButton.Cancel | StandardButton.Ok) - } + onClicked: showDialog(compassMotDialogComponent, qsTr("CompassMot - Compass Motor Interference Calibration"), qgcView.showDialogFullWidth, StandardButton.Cancel | StandardButton.Ok) + } - QGCButton { - id: nextButton - width: parent.buttonWidth - text: qsTr("Next") - enabled: false - onClicked: controller.nextClicked() - } + QGCButton { + width: _buttonWidth + text: qsTr("Sensor Settings") + onClicked: showOrientationsDialog(_calTypeSet) + } + } // Column - Cal Buttons - QGCButton { - id: cancelButton - width: parent.buttonWidth - text: qsTr("Cancel") - enabled: false - onClicked: controller.cancelCalibration() - } + Column { + id: nextCancelColumn + anchors.topMargin: buttonColumn.spacing + anchors.top: buttonColumn.bottom + anchors.left: buttonColumn.left + spacing: buttonColumn.spacing + + QGCButton { + id: nextButton + width: _buttonWidth + text: qsTr("Next") + enabled: false + onClicked: controller.nextClicked() + } - QGCButton { - id: setOrientationsButton - width: parent.buttonWidth - text: qsTr("Sensor Settings") - onClicked: showOrientationsDialog(_calTypeSet) + QGCButton { + id: cancelButton + width: _buttonWidth + text: qsTr("Cancel") + enabled: false + onClicked: controller.cancelCalibration() + } } - } // Column - Buttons + } // QGCFlickable - buttons /// Right column - cal area Column { + anchors.leftMargin: _margins anchors.top: parent.top anchors.bottom: parent.bottom - Layout.fillWidth: true + anchors.left: buttonFlickable.right + anchors.right: parent.right ProgressBar { id: progressBar diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index e3e6ca5e3..adcc5d0c1 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -27,14 +27,8 @@ APMSensorsComponentController::APMSensorsComponentController(void) : _sensorsComponent(NULL) , _statusLog(NULL) , _progressBar(NULL) - , _compassButton(NULL) - , _accelButton(NULL) - , _compassMotButton(NULL) - , _levelButton(NULL) - , _calibratePressureButton(NULL) , _nextButton(NULL) , _cancelButton(NULL) - , _setOrientationsButton(NULL) , _showOrientationCalArea(false) , _calTypeInProgress(CalTypeNone) , _orientationCalDownSideDone(false) @@ -110,12 +104,7 @@ void APMSensorsComponentController::_startLogCalibration(void) connect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage); - _compassButton->setEnabled(false); - _accelButton->setEnabled(false); - _compassMotButton->setEnabled(false); - _levelButton->setEnabled(false); - _calibratePressureButton->setEnabled(false); - _setOrientationsButton->setEnabled(false); + emit setAllCalButtonsEnabled(false); if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) { _nextButton->setEnabled(true); } @@ -124,13 +113,9 @@ void APMSensorsComponentController::_startLogCalibration(void) void APMSensorsComponentController::_startVisualCalibration(void) { - _compassButton->setEnabled(false); - _accelButton->setEnabled(false); - _compassMotButton->setEnabled(false); - _levelButton->setEnabled(false); - _calibratePressureButton->setEnabled(false); - _setOrientationsButton->setEnabled(false); + emit setAllCalButtonsEnabled(false); _cancelButton->setEnabled(true); + _nextButton->setEnabled(false); _resetInternalState(); @@ -169,12 +154,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll disconnect(_uas, &UASInterface::textMessageReceived, this, &APMSensorsComponentController::_handleUASTextMessage); - _compassButton->setEnabled(true); - _accelButton->setEnabled(true); - _compassMotButton->setEnabled(true); - _levelButton->setEnabled(true); - _calibratePressureButton->setEnabled(true); - _setOrientationsButton->setEnabled(true); + emit setAllCalButtonsEnabled(true); _nextButton->setEnabled(false); _cancelButton->setEnabled(false); diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index 16e0fd23e..6733abf88 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h @@ -34,14 +34,8 @@ public: Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) - 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* calibratePressureButton MEMBER _calibratePressureButton) Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton) - Q_PROPERTY(QQuickItem* setOrientationsButton MEMBER _setOrientationsButton) Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText) Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged) @@ -135,6 +129,7 @@ signals: void compass1CalFitnessChanged(double compass1CalFitness); void compass2CalFitnessChanged(double compass2CalFitness); void compass3CalFitnessChanged(double compass3CalFitness); + void setAllCalButtonsEnabled(bool enabled); private slots: void _handleUASTextMessage(int uasId, int compId, int severity, QString text); @@ -168,14 +163,8 @@ private: QQuickItem* _statusLog; QQuickItem* _progressBar; - QQuickItem* _compassButton; - QQuickItem* _accelButton; - QQuickItem* _compassMotButton; - QQuickItem* _levelButton; - QQuickItem* _calibratePressureButton; QQuickItem* _nextButton; QQuickItem* _cancelButton; - QQuickItem* _setOrientationsButton; QQuickItem* _orientationCalAreaHelpText; bool _showOrientationCalArea; diff --git a/src/AutoPilotPlugins/Common/RadioComponent.qml b/src/AutoPilotPlugins/Common/RadioComponent.qml index 3d08aa1b4..c5fb6c910 100644 --- a/src/AutoPilotPlugins/Common/RadioComponent.qml +++ b/src/AutoPilotPlugins/Common/RadioComponent.qml @@ -418,7 +418,6 @@ SetupPage { QGCButton { text: qsTr("Copy Trims") - visible: QGroundControl.multiVehicleManager.activeVehicle.px4Firmware onClicked: showDialog(copyTrimsDialogComponent, dialogTitle, radioPage.showDialogDefaultWidth, StandardButton.Ok | StandardButton.Cancel) } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 992323af3..5e0857327 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -132,11 +132,6 @@ bool ArduSubFirmwarePlugin::supportsJSButton(void) return true; } -bool ArduSubFirmwarePlugin::supportsCalibratePressure(void) -{ - return true; -} - bool ArduSubFirmwarePlugin::supportsMotorInterference(void) { return false; diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h index 58123f35b..8307da038 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h @@ -79,8 +79,6 @@ public: bool supportsJSButton(void); - bool supportsCalibratePressure(void); - bool supportsMotorInterference(void); QString brandImageIndoor(const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImageSub"); } diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index d07ace1ed..19a494528 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -139,11 +139,6 @@ bool FirmwarePlugin::supportsRadio(void) return true; } -bool FirmwarePlugin::supportsCalibratePressure(void) -{ - return false; -} - bool FirmwarePlugin::supportsMotorInterference(void) { return true; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 2740e7cde..995a9d1e3 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -170,10 +170,6 @@ public: /// to be assigned via parameters in firmware. Default is false. virtual bool supportsJSButton(void); - /// Returns true if the firmware supports calibrating the pressure sensor so the altitude will read - /// zero at the current pressure. Default is false. - virtual bool supportsCalibratePressure(void); - /// Returns true if the firmware supports calibrating motor interference offsets for the compass /// (CompassMot). Default is true. virtual bool supportsMotorInterference(void); diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index d007bd914..98111351a 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1959,11 +1959,6 @@ bool Vehicle::supportsJSButton(void) const return _firmwarePlugin->supportsJSButton(); } -bool Vehicle::supportsCalibratePressure(void) const -{ - return _firmwarePlugin->supportsCalibratePressure(); -} - bool Vehicle::supportsMotorInterference(void) const { return _firmwarePlugin->supportsMotorInterference(); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 477af21d8..faddcdf14 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -282,7 +282,6 @@ public: Q_PROPERTY(bool supportsThrottleModeCenterZero READ supportsThrottleModeCenterZero CONSTANT) Q_PROPERTY(bool supportsJSButton READ supportsJSButton CONSTANT) Q_PROPERTY(bool supportsRadio READ supportsRadio CONSTANT) - Q_PROPERTY(bool supportsCalibratePressure READ supportsCalibratePressure CONSTANT) Q_PROPERTY(bool supportsMotorInterference READ supportsMotorInterference CONSTANT) Q_PROPERTY(bool autoDisconnect MEMBER _autoDisconnect NOTIFY autoDisconnectChanged) Q_PROPERTY(QString prearmError READ prearmError WRITE setPrearmError NOTIFY prearmErrorChanged) @@ -519,7 +518,6 @@ public: bool supportsThrottleModeCenterZero(void) const; bool supportsRadio(void) const; bool supportsJSButton(void) const; - bool supportsCalibratePressure(void) const; bool supportsMotorInterference(void) const; void setGuidedMode(bool guidedMode); -- 2.22.0