Merge pull request #4594 from bluerobotics/calibrate-pressure

Add calibrate pressure button in sensors page, enabled only for ArduSub

src/AutoPilotPlugins/APM/APMSensorsComponent.qml

@@ -14,6 +14,7 @@ import QtQuick.Controls.Styles  1.2
 import QtQuick.Dialogs          1.2
 import QtQuick.Layouts          1.2
 
+import QGroundControl               1.0
 import QGroundControl.FactSystem    1.0
 import QGroundControl.FactControls  1.0
 import QGroundControl.Palette       1.0
@@ -120,6 +121,7 @@ SetupPage {
                 accelButton:                accelButton
                 compassMotButton:           motorInterferenceButton
                 levelButton:                levelHorizonButton
+                calibratePressureButton:    calibratePressureButton
                 nextButton:                 nextButton
                 cancelButton:               cancelButton
                 setOrientationsButton:      setOrientationsButton
@@ -445,6 +447,26 @@ SetupPage {
                 } // QGCViewDialog
             } // Component - levelHorizonDialogComponent
 
+            Component {
+                id: calibratePressureDialogComponent
+
+                QGCViewDialog {
+                    id: calibratePressureDialog
+
+                    function accept() {
+                        controller.calibratePressure()
+                        calibratePressureDialog.hideDialog()
+                    }
+
+                    QGCLabel {
+                        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.")
+                    }
+                } // QGCViewDialog
+            } // Component - calibratePressureDialogComponent
+
             /// Left button column
             Column {
                 spacing:            ScreenTools.defaultFontPixelHeight / 2
@@ -492,6 +514,20 @@ SetupPage {
                     }
                 }
 
+                QGCButton {
+                    id:     calibratePressureButton
+                    width:  parent.buttonWidth
+                    text:   _calibratePressureText
+                    visible: _activeVehicle ? _activeVehicle.supportsCalibratePressure : false
+
+                    readonly property string _calibratePressureText: qsTr("Calibrate Pressure")
+                    property var  _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle
+
+                    onClicked: {
+                        showDialog(calibratePressureDialogComponent, _calibratePressureText, qgcView.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok)
+                    }
+                }
+
                 QGCButton {
                     id:         motorInterferenceButton
                     width:      parent.buttonWidth

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -30,6 +30,7 @@ APMSensorsComponentController::APMSensorsComponentController(void)
     , _accelButton(NULL)
     , _compassMotButton(NULL)
     , _levelButton(NULL)
+    , _calibratePressureButton(NULL)
     , _nextButton(NULL)
     , _cancelButton(NULL)
     , _setOrientationsButton(NULL)
@@ -101,6 +102,7 @@ void APMSensorsComponentController::_startLogCalibration(void)
     _accelButton->setEnabled(false);
     _compassMotButton->setEnabled(false);
     _levelButton->setEnabled(false);
+    _calibratePressureButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
     if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) {
         _nextButton->setEnabled(true);
@@ -114,6 +116,7 @@ void APMSensorsComponentController::_startVisualCalibration(void)
     _accelButton->setEnabled(false);
     _compassMotButton->setEnabled(false);
     _levelButton->setEnabled(false);
+    _calibratePressureButton->setEnabled(false);
     _setOrientationsButton->setEnabled(false);
     _cancelButton->setEnabled(true);
 
@@ -158,6 +161,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _accelButton->setEnabled(true);
     _compassMotButton->setEnabled(true);
     _levelButton->setEnabled(true);
+    _calibratePressureButton->setEnabled(true);
     _setOrientationsButton->setEnabled(true);
     _nextButton->setEnabled(false);
     _cancelButton->setEnabled(false);
@@ -318,6 +322,15 @@ void APMSensorsComponentController::levelHorizon(void)
     _uas->startCalibration(UASInterface::StartCalibrationLevel);
 }
 
+void APMSensorsComponentController::calibratePressure(void)
+{
+    _calTypeInProgress = CalTypePressure;
+    _vehicle->setConnectionLostEnabled(false);
+    _startLogCalibration();
+    _appendStatusLog(tr("Requesting pressure calibration..."));
+    _uas->startCalibration(UASInterface::StartCalibrationPressure);
+}
+
 void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text)
 {
     Q_UNUSED(compId);
@@ -628,6 +641,24 @@ void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message
             }
         }
     }
+
+    if (_calTypeInProgress == CalTypePressure) {
+        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("Pressure calibration success"));
+                _stopCalibration(StopCalibrationSuccessShowLog);
+                break;
+            default:
+                _appendStatusLog(tr("Pressure calibration fail"));
+                _stopCalibration(StopCalibrationFailed);
+                break;
+            }
+        }
+    }
 }
 
 void APMSensorsComponentController::_handleMagCalProgress(mavlink_message_t& message)

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -38,6 +38,7 @@ public:
     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)
@@ -90,6 +91,7 @@ public:
     Q_INVOKABLE void calibrateAccel(void);
     Q_INVOKABLE void calibrateMotorInterference(void);
     Q_INVOKABLE void levelHorizon(void);
+    Q_INVOKABLE void calibratePressure(void);
     Q_INVOKABLE void cancelCalibration(void);
     Q_INVOKABLE void nextClicked(void);
     Q_INVOKABLE bool usingUDPLink(void);
@@ -103,6 +105,7 @@ public:
         CalTypeOffboardCompass,
         CalTypeLevelHorizon,
         CalTypeCompassMot,
+        CalTypePressure,
         CalTypeNone
     } CalType_t;
     Q_ENUM(CalType_t)
@@ -169,6 +172,7 @@ private:
     QQuickItem* _accelButton;
     QQuickItem* _compassMotButton;
     QQuickItem* _levelButton;
+    QQuickItem* _calibratePressureButton;
     QQuickItem* _nextButton;
     QQuickItem* _cancelButton;
     QQuickItem* _setOrientationsButton;

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc

@@ -96,3 +96,8 @@ bool ArduSubFirmwarePlugin::supportsJSButton(void)
 {
     return true;
 }
+
+bool ArduSubFirmwarePlugin::supportsCalibratePressure(void)
+{
+    return true;
+}

src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h

@@ -79,6 +79,8 @@ public:
 
     bool supportsJSButton(void);
 
+    bool supportsCalibratePressure(void);
+
     QString brandImage(const Vehicle* vehicle) const { Q_UNUSED(vehicle); return QStringLiteral("/qmlimages/APM/BrandImageSub"); }
     const FirmwarePlugin::remapParamNameMajorVersionMap_t& paramNameRemapMajorVersionMap(void) const final { return _remapParamName; }
     int remapParamNameHigestMinorVersionNumber(int majorVersionNumber) const final;

src/FirmwarePlugin/FirmwarePlugin.cc

@@ -134,6 +134,11 @@ bool FirmwarePlugin::supportsRadio(void)
     return true;
 }
 
+bool FirmwarePlugin::supportsCalibratePressure(void)
+{
+    return false;
+}
+
 bool FirmwarePlugin::supportsJSButton(void)
 {
     return false;

src/FirmwarePlugin/FirmwarePlugin.h

@@ -163,6 +163,10 @@ 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);
+
     /// Called before any mavlink message is processed by Vehicle such that the firmwre plugin
     /// can adjust any message characteristics. This is handy to adjust or differences in mavlink
     /// spec implementations such that the base code can remain mavlink generic.

src/Vehicle/Vehicle.cc

@@ -1742,6 +1742,11 @@ bool Vehicle::supportsJSButton(void) const
     return _firmwarePlugin->supportsJSButton();
 }
 
+bool Vehicle::supportsCalibratePressure(void) const
+{
+    return _firmwarePlugin->supportsCalibratePressure();
+}
+
 void Vehicle::_setCoordinateValid(bool coordinateValid)
 {
     if (coordinateValid != _coordinateValid) {

src/Vehicle/Vehicle.h

@@ -253,6 +253,7 @@ 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                 autoDisconnect          MEMBER _autoDisconnect                                      NOTIFY autoDisconnectChanged)
     Q_PROPERTY(QString              prearmError             READ prearmError            WRITE setPrearmError            NOTIFY prearmErrorChanged)
     Q_PROPERTY(int                  motorCount              READ motorCount                                             CONSTANT)
@@ -474,6 +475,7 @@ public:
     bool supportsThrottleModeCenterZero(void) const;
     bool supportsRadio(void) const;
     bool supportsJSButton(void) const;
+    bool supportsCalibratePressure(void) const;
 
     void setFlying(bool flying);
     void setGuidedMode(bool guidedMode);

src/uas/UAS.cc

@@ -528,6 +528,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
     int airspeedCal = 0;
     int radioCal = 0;
     int accelCal = 0;
+    int pressureCal = 0;
     int escCal = 0;
 
     switch (calType) {
@@ -552,6 +553,9 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
     case StartCalibrationLevel:
         accelCal = 2;
         break;
+    case StartCalibrationPressure:
+        pressureCal = 1;
+        break;
     case StartCalibrationEsc:
         escCal = 1;
         break;
@@ -576,7 +580,7 @@ void UAS::startCalibration(UASInterface::StartCalibrationType calType)
                                        0,                                // 0=first transmission of command
                                        gyroCal,                          // gyro cal
                                        magCal,                           // mag cal
-                                       0,                                // ground pressure
+                                       pressureCal,                      // ground pressure
                                        radioCal,                         // radio cal
                                        accelCal,                         // accel cal
                                        airspeedCal,                      // PX4: airspeed cal, ArduPilot: compass mot

src/uas/UASInterface.h

@@ -111,6 +111,7 @@ public:
         StartCalibrationAirspeed,
         StartCalibrationAccel,
         StartCalibrationLevel,
+        StartCalibrationPressure,
         StartCalibrationEsc,
         StartCalibrationCopyTrims,
         StartCalibrationUavcanEsc,