diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index 51b5b4eaddda4c6ac2dd550bca4904502b58b03a..1f328647edbdc01adb929a6255b9524e244c4351 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -532,6 +532,10 @@ SetupPage { id: motorInterferenceButton width: parent.buttonWidth text: qsTr("CompassMot") + visible: _activeVehicle ? _activeVehicle.supportsMotorInterference : false + + property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle + onClicked: showDialog(compassMotDialogComponent, qsTr("CompassMot - Compass Motor Interference Calibration"), qgcView.showDialogFullWidth, StandardButton.Cancel | StandardButton.Ok) } diff --git a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc index 74f0678f6828edd65607755507249ace18420ee0..5b0dd15a049a20feae7cdb1c16f1e73f506e0365 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc @@ -101,3 +101,8 @@ 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 435795825fc2f6280ac641c4d3585f9665b06fe5..d44bacf43f5bb019047b6714d1e36b9acc7cdb3b 100644 --- a/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h +++ b/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h @@ -81,6 +81,8 @@ public: bool supportsCalibratePressure(void); + bool supportsMotorInterference(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; diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc index 2974b73feddc09c46457d0cebd5a08d0ec3972ee..5df28367dd5ac895caf761ce4dbd157ee6939649 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.cc +++ b/src/FirmwarePlugin/FirmwarePlugin.cc @@ -139,6 +139,11 @@ bool FirmwarePlugin::supportsCalibratePressure(void) return false; } +bool FirmwarePlugin::supportsMotorInterference(void) +{ + return true; +} + bool FirmwarePlugin::supportsJSButton(void) { return false; diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h index 9cf38be3bcbd5abf7e4e0e29e547e87491f65ca4..d9aa618034cfdaf461119d0f4be3d552b630b5b3 100644 --- a/src/FirmwarePlugin/FirmwarePlugin.h +++ b/src/FirmwarePlugin/FirmwarePlugin.h @@ -167,6 +167,10 @@ public: /// 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); + /// 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. diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 0794337dcc1bbbe9aa328462cf48b71b0fdffa97..9abd02ae7a87910cd92748dfa42eeb39271e24fe 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1777,6 +1777,11 @@ bool Vehicle::supportsCalibratePressure(void) const return _firmwarePlugin->supportsCalibratePressure(); } +bool Vehicle::supportsMotorInterference(void) const +{ + return _firmwarePlugin->supportsMotorInterference(); +} + void Vehicle::_setCoordinateValid(bool coordinateValid) { if (coordinateValid != _coordinateValid) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 89516ba87cc159560e1e9af5c417ab7b3c52133d..c5e0d27d57a05b086abf4eb1ad4d712b28eef9ca 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -282,7 +282,8 @@ 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 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) Q_PROPERTY(int motorCount READ motorCount CONSTANT) @@ -506,6 +507,7 @@ public: bool supportsRadio(void) const; bool supportsJSButton(void) const; bool supportsCalibratePressure(void) const; + bool supportsMotorInterference(void) const; void setFlying(bool flying); void setGuidedMode(bool guidedMode);