Commit 7911dc29 authored by Don Gagne's avatar Don Gagne Committed by GitHub

Merge pull request #4594 from bluerobotics/calibrate-pressure

Add calibrate pressure button in sensors page, enabled only for ArduSub
parents f7f5ddb2 42783643
......@@ -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
......
......@@ -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)
......
......@@ -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;
......
......@@ -96,3 +96,8 @@ bool ArduSubFirmwarePlugin::supportsJSButton(void)
{
return true;
}
bool ArduSubFirmwarePlugin::supportsCalibratePressure(void)
{
return true;
}
......@@ -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;
......
......@@ -134,6 +134,11 @@ bool FirmwarePlugin::supportsRadio(void)
return true;
}
bool FirmwarePlugin::supportsCalibratePressure(void)
{
return false;
}
bool FirmwarePlugin::supportsJSButton(void)
{
return false;
......
......@@ -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.
......
......@@ -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) {
......
......@@ -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);
......
......@@ -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
......
......@@ -111,6 +111,7 @@ public:
StartCalibrationAirspeed,
StartCalibrationAccel,
StartCalibrationLevel,
StartCalibrationPressure,
StartCalibrationEsc,
StartCalibrationCopyTrims,
StartCalibrationUavcanEsc,
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment