From 465999bf2a0eb29368ecebc490e67485cb32bcc9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 27 Dec 2016 07:20:26 -0800 Subject: [PATCH] Ardupilot onboard compass cal support --- .../APM/APMSensorsComponent.qml | 341 ++++++++--------- .../APM/APMSensorsComponentController.cc | 346 ++++++++++++++---- .../APM/APMSensorsComponentController.h | 65 +++- .../APM/APMSensorsComponentSummary.qml | 61 ++- src/FirmwarePlugin/APM/APMSensorParams.qml | 87 +++++ .../APM/QGroundControl.ArduPilot.qmldir | 3 + src/comm/MockLink.cc | 8 +- 7 files changed, 594 insertions(+), 317 deletions(-) create mode 100644 src/FirmwarePlugin/APM/APMSensorParams.qml create mode 100644 src/FirmwarePlugin/APM/QGroundControl.ArduPilot.qmldir diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index a0dcf688a..a0b553f52 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -20,6 +20,7 @@ import QGroundControl.Palette 1.0 import QGroundControl.Controls 1.0 import QGroundControl.ScreenTools 1.0 import QGroundControl.Controllers 1.0 +import QGroundControl.ArduPilot 1.0 SetupPage { id: sensorsPage @@ -29,8 +30,8 @@ SetupPage { id: sensorsPageComponent RowLayout { - width: 1000//availableWidth - height: 1000//availableHeight + width: availableWidth + height: availableHeight spacing: ScreenTools.defaultFontPixelWidth / 2 // Help text which is shown both in the status text area prior to pressing a cal button and in the @@ -62,49 +63,12 @@ SetupPage { readonly property int rotationColumnWidth: 250 - property Fact compass1Id: controller.getParameterFact(-1, "COMPASS_DEV_ID") - property Fact compass2Id: controller.getParameterFact(-1, "COMPASS_DEV_ID2") - property Fact compass3Id: controller.getParameterFact(-1, "COMPASS_DEV_ID3") - property Fact compass1ExternalFact: controller.getParameterFact(-1, "COMPASS_EXTERNAL") - property Fact compass1Rot: controller.getParameterFact(-1, "COMPASS_ORIENT") - - property Fact boardRot: controller.getParameterFact(-1, "AHRS_ORIENTATION") - - property bool accelCalNeeded: controller.accelSetupNeeded - property bool compassCalNeeded: controller.compassSetupNeeded - - - // The following parameters are not available in olders firmwares + property Fact noFact: Fact { } - property bool compass2ExternalParamAvailable: controller.parameterExists(-1, "COMPASS_EXTERN2") - property bool compass3ExternalParamAvailable: controller.parameterExists(-1, "COMPASS_EXTERN3") - property bool compass2RotParamAvailable: controller.parameterExists(-1, "COMPASS_ORIENT2") - property bool compass3RotParamAvailable: controller.parameterExists(-1, "COMPASS_ORIENT3") - property bool compass1UseParamAvailable: controller.parameterExists(-1, "COMPASS_USE") - property bool compass2UseParamAvailable: controller.parameterExists(-1, "COMPASS_USE2") - property bool compass3UseParamAvailable: controller.parameterExists(-1, "COMPASS_USE3") + property bool accelCalNeeded: controller.accelSetupNeeded + property bool compassCalNeeded: controller.compassSetupNeeded - property Fact noFact: Fact { } - property Fact compass2ExternalFact: compass2ExternalParamAvailable ? controller.getParameterFact(-1, "COMPASS_EXTERN2") : noFact - property Fact compass3ExternalFact: compass3ExternalParamAvailable ? controller.getParameterFact(-1, "COMPASS_EXTERN3") : noFact - property Fact compass2Rot: compass2RotParamAvailable ? controller.getParameterFact(-1, "COMPASS_ORIENT2") : noFact - property Fact compass3Rot: compass3RotParamAvailable ? controller.getParameterFact(-1, "COMPASS_ORIENT3") : noFact - property Fact compass1UseFact: compass1UseParamAvailable ? controller.getParameterFact(-1, "COMPASS_USE") : noFact - property Fact compass2UseFact: compass2UseParamAvailable ? controller.getParameterFact(-1, "COMPASS_USE2") : noFact - property Fact compass3UseFact: compass3UseParamAvailable ? controller.getParameterFact(-1, "COMPASS_USE3") : noFact - - // We track these values by binding through a separate property so we can handle missing params - property bool compass1External: compass1ExternalFact.value - property bool compass2External: compass2ExternalParamAvailable ? compass2ExternalFact.value : false // false: Simulate internal so we don't show rotation combos - property bool compass3External: compass3ExternalParamAvailable ? compass3ExternalFact.value : false // false: Simulate internal so we don't show rotation combos - property bool compass1Use: compass1UseParamAvailable ? compass1UseFact.value : true - property bool compass2Use: compass2UseParamAvailable ? compass2UseFact.value : true - property bool compass3Use: compass3UseParamAvailable ? compass3UseFact.value : true - - // Id > = signals compass available, rot < 0 signals internal compass - property bool showCompass1: compass1Id.value > 0 - property bool showCompass2: compass2Id.value > 0 - property bool showCompass3: compass3Id.value > 0 + property Fact boardRot: controller.getParameterFact(-1, "AHRS_ORIENTATION") readonly property int _calTypeCompass: 1 ///< Calibrate compass readonly property int _calTypeAccel: 2 ///< Calibrate accel @@ -114,13 +78,6 @@ SetupPage { property string _orientationDialogHelp: orientationHelpSet property int _orientationDialogCalType - function validCompassOffsets(compassParamPrefix) { - var ofsX = controller.getParameterFact(-1, compassParamPrefix + "X") - var ofsY = controller.getParameterFact(-1, compassParamPrefix + "Y") - var ofsZ = controller.getParameterFact(-1, compassParamPrefix + "Z") - return Math.sqrt(ofsX.value^2 + ofsY.value^2 + ofsZ.value^2) < 600 - } - function showOrientationsDialog(calType) { var dialogTitle var buttons = StandardButton.Ok @@ -149,6 +106,11 @@ SetupPage { showDialog(orientationsDialogComponent, dialogTitle, qgcView.showDialogDefaultWidth, buttons) } + APMSensorParams { + id: sensorParams + factPanelController: controller + } + APMSensorsComponentController { id: controller factPanel: sensorsPage.viewPanel @@ -163,6 +125,8 @@ SetupPage { setOrientationsButton: setOrientationsButton orientationCalAreaHelpText: orientationCalAreaHelpText + property var rgCompassCalFitness: [ controller.compass1CalFitness, controller.compass2CalFitness, controller.compass3CalFitness ] + onResetStatusTextArea: statusLog.text = statusTextAreaDefaultText onWaitingForCancelChanged: { @@ -174,43 +138,16 @@ SetupPage { } onCalibrationComplete: { - if (_orientationDialogCalType == _calTypeAccel) { - _postCalibrationDialogText = qsTr("Accelerometer calibration complete.") - _postCalibrationDialogParams = [ "INS_ACCSCAL_X", "INS_ACCSCAL_Y", "INS_ACCSCAL_Z", - "INS_ACC2SCAL_X", "INS_ACC2SCAL_Y", "INS_ACC2SCAL_Z", - "INS_ACC3SCAL_X", "INS_ACC3SCAL_Y", "INS_ACC3SCAL_Z", - "INS_GYROFFS_X", "INS_GYROFFS_Y", "INS_GYROFFS_Z", - "INS_GYR2OFFS_X", "INS_GYR2OFFS_Y", "INS_GYR2OFFS_Z", - "INS_GYR3OFFS_X", "INS_GYR3OFFS_Y", "INS_GYR3OFFS_Z" ] - showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok) - } else if (_orientationDialogCalType == _calTypeCompass) { - _postCalibrationDialogText = qsTr("Compass calibration complete. ") - _postCalibrationDialogParams = []; - if (compass1Id.value > 0) { - if (!validCompassOffsets("COMPASS_OFS_")) { - _postCalibrationDialogText += _badCompassCalText.replace("%1", 1) - } - _postCalibrationDialogParams.push("COMPASS_OFS_X") - _postCalibrationDialogParams.push("COMPASS_OFS_Y") - _postCalibrationDialogParams.push("COMPASS_OFS_Z") - } - if (compass2Id.value > 0) { - if (!validCompassOffsets("COMPASS_OFS_")) { - _postCalibrationDialogText += _badCompassCalText.replace("%1", 2) - } - _postCalibrationDialogParams.push("COMPASS_OFS2_X") - _postCalibrationDialogParams.push("COMPASS_OFS2_Y") - _postCalibrationDialogParams.push("COMPASS_OFS2_Z") - } - if (compass3Id.value > 0) { - if (!validCompassOffsets("COMPASS_OFS_")) { - _postCalibrationDialogText += _badCompassCalText.replace("%1", 3) - } - _postCalibrationDialogParams.push("COMPASS_OFS3_X") - _postCalibrationDialogParams.push("COMPASS_OFS3_Y") - _postCalibrationDialogParams.push("COMPASS_OFS3_Z") - } - showDialog(postCalibrationDialogComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok) + switch (calType) { + case APMSensorsComponentController.CalTypeAccel: + showMessage(qsTr("Calibration complete"), qsTr("Accelerometer calibration complete."), StandardButton.Ok) + break + case APMSensorsComponentController.CalTypeOffboardCompass: + showMessage(qsTr("Calibration complete"), qsTr("Compass calibration complete."), StandardButton.Ok) + break + case APMSensorsComponentController.CalTypeOnboardCompass: + showDialog(postOnboardCompassCalibrationComponent, qsTr("Calibration complete"), qgcView.showDialogDefaultWidth, StandardButton.Ok) + break } } } @@ -218,7 +155,6 @@ SetupPage { Component.onCompleted: { var usingUDP = controller.usingUDPLink() if (usingUDP) { - console.log("onUsingUDPLink") showMessage("Sensor Calibration", "Performing sensor calibration over a WiFi connection is known to be unreliable. You should disconnect and perform calibration using a direct USB connection instead.", StandardButton.Ok) } } @@ -226,43 +162,144 @@ SetupPage { QGCPalette { id: qgcPal; colorGroupEnabled: true } Component { - id: postCalibrationDialogComponent + id: singleCompassOnboardResultsComponent - QGCViewDialog { - QGCLabel { - id: textLabel + Column { + anchors.left: parent.left + anchors.right: parent.right + spacing: Math.round(ScreenTools.defaultFontPixelHeight / 2) + visible: sensorParams.rgCompassAvailable[index] + + property real greenMaxThreshold: 8 * (sensorParams.rgCompassExternal[index] ? 1 : 2) + property real yellowMaxThreshold: 15 * (sensorParams.rgCompassExternal[index] ? 1 : 2) + property real fitnessRange: 25 * (sensorParams.rgCompassExternal[index] ? 1 : 2) + + Item { anchors.left: parent.left anchors.right: parent.right - wrapMode: Text.WordWrap - text: _postCalibrationDialogText + height: ScreenTools.defaultFontPixelHeight + + Row { + id: fitnessRow + anchors.fill: parent + + Rectangle { + width: parent.width * (greenMaxThreshold / fitnessRange) + height: parent.height + color: "green" + } + Rectangle { + width: parent.width * ((yellowMaxThreshold - greenMaxThreshold) / fitnessRange) + height: parent.height + color: "yellow" + } + Rectangle { + width: parent.width * ((fitnessRange - yellowMaxThreshold) / fitnessRange) + height: parent.height + color: "red" + } + } + + Rectangle { + height: fitnessRow.height * 0.66 + width: height + anchors.verticalCenter: fitnessRow.verticalCenter + x: (fitnessRow.width * (Math.min(Math.max(controller.rgCompassCalFitness[index], 0.0), fitnessRange) / fitnessRange)) - (width / 2) + radius: height / 2 + color: "white" + border.color: "black" + } } - QGCCheckBox { - id: showValues - anchors.topMargin: ScreenTools.defaultFontPixelHeight - anchors.top: textLabel.bottom - text: qsTr("Show values") + Column { + anchors.leftMargin: ScreenTools.defaultFontPixelWidth * 2 + anchors.left: parent.left + anchors.right: parent.right + spacing: Math.round(ScreenTools.defaultFontPixelHeight / 4) + + QGCLabel { + text: "Compass " + index + " " + + (sensorParams.rgCompassPrimary[index] ? "(primary" : "(secondary") + + (sensorParams.rgCompassExternalParamAvailable[index] ? + (sensorParams.rgCompassExternal[index] ? ", external" : ", internal" ) : + "") + + ")" + } + + FactCheckBox { + text: "Use Compass" + fact: sensorParams.rgCompassUseFact[index] + visible: sensorParams.rgCompassUseParamAvailable[index] && !sensorParams.rgCompassPrimary[index] + } } + } + } - QGCFlickable { - anchors.topMargin: ScreenTools.defaultFontPixelHeight - anchors.top: showValues.bottom - anchors.bottom: parent.bottom - contentHeight: valueColumn.height - flickableDirection: Flickable.VerticalFlick - visible: showValues.checked + Component { + id: postOnboardCompassCalibrationComponent - Column { - id: valueColumn + QGCViewDialog { + Column { + anchors.margins: ScreenTools.defaultFontPixelWidth + anchors.left: parent.left + anchors.right: parent.right + spacing: ScreenTools.defaultFontPixelHeight + + Repeater { + model: 3 + delegate: singleCompassOnboardResultsComponent + } - Repeater { - model: _postCalibrationDialogParams + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: qsTr("Shown in the indicator bars is the quality of the calibration for each compass.\n\n") + + qsTr("- Green indicates a well functioning compass.\n") + + qsTr("- Yellow indicates a questionable compass or calibration.\n") + + qsTr("- Red indicates a compass which should not be used.\n\n") + + qsTr("YOU MUST REBOOT YOUR VEHICLE AFTER EACH CALIBRATION.") + } + } + } + } - QGCLabel { - text: fact.name +": " + fact.valueString + Component { + id: singleCompassSettingsComponent - property Fact fact: controller.getParameterFact(-1, modelData) - } + Column { + spacing: Math.round(ScreenTools.defaultFontPixelHeight / 2) + visible: sensorParams.rgCompassAvailable[index] + + QGCLabel { + text: "Compass " + index + " " + + (sensorParams.rgCompassPrimary[index] ? "(primary" : "(secondary") + + (sensorParams.rgCompassExternalParamAvailable[index] ? + (sensorParams.rgCompassExternal[index] ? ", external" : ", internal" ) : + "") + + ")" + } + + Column { + anchors.margins: ScreenTools.defaultFontPixelWidth * 2 + anchors.left: parent.left + spacing: Math.round(ScreenTools.defaultFontPixelHeight / 4) + + FactCheckBox { + text: "Use Compass" + fact: sensorParams.rgCompassUseFact[index] + visible: sensorParams.rgCompassUseParamAvailable[index] && !sensorParams.rgCompassPrimary[index] + } + + Column { + visible: sensorParams.rgCompassExternal[index] && sensorParams.rgCompassRotParamAvailable[index] + + QGCLabel { text: qsTr("Orientation:") } + + FactComboBox { + width: rotationColumnWidth + indexModel: false + fact: sensorParams.rgCompassRotFact[index] } } } @@ -304,9 +341,7 @@ SetupPage { } Column { - QGCLabel { - text: qsTr("Autopilot Orientation:") - } + QGCLabel { text: qsTr("Autopilot Orientation:") } FactComboBox { width: rotationColumnWidth @@ -315,73 +350,9 @@ SetupPage { } } - Column { - visible: _orientationsDialogShowCompass && showCompass1 - - FactCheckBox { - text: "Use Compass 1" - fact: compass1UseFact - } - - Column { - visible: showCompass1Rot - - QGCLabel { - text: qsTr("Compass 1 Orientation:") - } - - FactComboBox { - width: rotationColumnWidth - indexModel: false - fact: compass1Rot - } - } - } - - Column { - visible: _orientationsDialogShowCompass && showCompass2 - - FactCheckBox { - text: "Use Compass 2" - fact: compass2UseFact - } - - Column { - visible: showCompass1Rot - - QGCLabel { - text: qsTr("Compass 2 Orientation:") - } - - FactComboBox { - width: rotationColumnWidth - indexModel: false - fact: compass2Rot - } - } - } - - Column { - visible: _orientationsDialogShowCompass && showCompass3 - - FactCheckBox { - text: "Use Compass 3" - fact: compass3UseFact - } - - Column { - visible: showCompass3Rot - - QGCLabel { - text: qsTr("Compass 3 Orientation:") - } - - FactComboBox { - width: rotationColumnWidth - indexModel: false - fact: compass3Rot - } - } + Repeater { + model: _orientationsDialogShowCompass ? 3 : 0 + delegate: singleCompassSettingsComponent } } // Column } // QGCFlickable @@ -474,6 +445,7 @@ SetupPage { } // QGCViewDialog } // Component - levelHorizonDialogComponent + /// Left button column Column { spacing: ScreenTools.defaultFontPixelHeight / 2 Layout.alignment: Qt.AlignLeft | Qt.AlignTop @@ -551,6 +523,7 @@ SetupPage { } } // Column - Buttons + /// Right column - cal area Column { anchors.top: parent.top anchors.bottom: parent.bottom diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index b68c7f90a..6db800532 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -19,47 +19,48 @@ #include QGC_LOGGING_CATEGORY(APMSensorsComponentControllerLog, "APMSensorsComponentControllerLog") - -APMSensorsComponentController::APMSensorsComponentController(void) : - _statusLog(NULL), - _progressBar(NULL), - _compassButton(NULL), - _accelButton(NULL), - _compassMotButton(NULL), - _levelButton(NULL), - _nextButton(NULL), - _cancelButton(NULL), - _setOrientationsButton(NULL), - _showOrientationCalArea(false), - _magCalInProgress(false), - _accelCalInProgress(false), - _compassMotCalInProgress(false), - _levelInProgress(false), - _orientationCalDownSideDone(false), - _orientationCalUpsideDownSideDone(false), - _orientationCalLeftSideDone(false), - _orientationCalRightSideDone(false), - _orientationCalNoseDownSideDone(false), - _orientationCalTailDownSideDone(false), - _orientationCalDownSideVisible(false), - _orientationCalUpsideDownSideVisible(false), - _orientationCalLeftSideVisible(false), - _orientationCalRightSideVisible(false), - _orientationCalNoseDownSideVisible(false), - _orientationCalTailDownSideVisible(false), - _orientationCalDownSideInProgress(false), - _orientationCalUpsideDownSideInProgress(false), - _orientationCalLeftSideInProgress(false), - _orientationCalRightSideInProgress(false), - _orientationCalNoseDownSideInProgress(false), - _orientationCalTailDownSideInProgress(false), - _orientationCalDownSideRotate(false), - _orientationCalUpsideDownSideRotate(false), - _orientationCalLeftSideRotate(false), - _orientationCalRightSideRotate(false), - _orientationCalNoseDownSideRotate(false), - _orientationCalTailDownSideRotate(false), - _waitingForCancel(false) +QGC_LOGGING_CATEGORY(APMSensorsComponentControllerVerboseLog, "APMSensorsComponentControllerVerboseLog") + +const char* APMSensorsComponentController::_compassCalFitnessParam = "COMPASS_CAL_FIT"; + +APMSensorsComponentController::APMSensorsComponentController(void) + : _statusLog(NULL) + , _progressBar(NULL) + , _compassButton(NULL) + , _accelButton(NULL) + , _compassMotButton(NULL) + , _levelButton(NULL) + , _nextButton(NULL) + , _cancelButton(NULL) + , _setOrientationsButton(NULL) + , _showOrientationCalArea(false) + , _calTypeInProgress(CalTypeNone) + , _orientationCalDownSideDone(false) + , _orientationCalUpsideDownSideDone(false) + , _orientationCalLeftSideDone(false) + , _orientationCalRightSideDone(false) + , _orientationCalNoseDownSideDone(false) + , _orientationCalTailDownSideDone(false) + , _orientationCalDownSideVisible(false) + , _orientationCalUpsideDownSideVisible(false) + , _orientationCalLeftSideVisible(false) + , _orientationCalRightSideVisible(false) + , _orientationCalNoseDownSideVisible(false) + , _orientationCalTailDownSideVisible(false) + , _orientationCalDownSideInProgress(false) + , _orientationCalUpsideDownSideInProgress(false) + , _orientationCalLeftSideInProgress(false) + , _orientationCalRightSideInProgress(false) + , _orientationCalNoseDownSideInProgress(false) + , _orientationCalTailDownSideInProgress(false) + , _orientationCalDownSideRotate(false) + , _orientationCalUpsideDownSideRotate(false) + , _orientationCalLeftSideRotate(false) + , _orientationCalRightSideRotate(false) + , _orientationCalNoseDownSideRotate(false) + , _orientationCalTailDownSideRotate(false) + , _waitingForCancel(false) + , _restoreCompassCalFitness(false) { _compassCal.setVehicle(_vehicle); connect(&_compassCal, &APMCompassCal::vehicleTextMessage, this, &APMSensorsComponentController::_handleUASTextMessage); @@ -72,6 +73,11 @@ APMSensorsComponentController::APMSensorsComponentController(void) : connect(qgcApp()->toolbox()->mavlinkProtocol(), &MAVLinkProtocol::messageReceived, this, &APMSensorsComponentController::_mavlinkMessageReceived); } +APMSensorsComponentController::~APMSensorsComponentController() +{ + _restorePreviousCompassCalFitness(); +} + /// Appends the specified text to the status log area in the ui void APMSensorsComponentController::_appendStatusLog(const QString& text) { @@ -96,10 +102,10 @@ void APMSensorsComponentController::_startLogCalibration(void) _compassMotButton->setEnabled(false); _levelButton->setEnabled(false); _setOrientationsButton->setEnabled(false); - if (_accelCalInProgress || _compassMotCalInProgress) { + if (_calTypeInProgress == CalTypeAccel || _calTypeInProgress == CalTypeCompassMot) { _nextButton->setEnabled(true); } - _cancelButton->setEnabled(false); + _cancelButton->setEnabled(_calTypeInProgress == CalTypeOnboardCompass); } void APMSensorsComponentController::_startVisualCalibration(void) @@ -156,11 +162,15 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll _nextButton->setEnabled(false); _cancelButton->setEnabled(false); + if (_calTypeInProgress == CalTypeOnboardCompass) { + _restorePreviousCompassCalFitness(); + } + if (code == StopCalibrationSuccess) { _resetInternalState(); _progressBar->setProperty("value", 1); - if (_vehicle->parameterManager()->parameterExists(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))) { - _vehicle->parameterManager()->getParameter(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))->setRawValue(0); + if (parameterExists(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))) { + getParameterFact(FactSystem::defaultComponentId, QStringLiteral("COMPASS_LEARN"))->setRawValue(0); } } else { _progressBar->setProperty("value", 0); @@ -175,11 +185,11 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll case StopCalibrationSuccess: _orientationCalAreaHelpText->setProperty("text", "Calibration complete"); emit resetStatusTextArea(); - emit calibrationComplete(); + emit calibrationComplete(_calTypeInProgress); break; case StopCalibrationSuccessShowLog: - emit calibrationComplete(); + emit calibrationComplete(_calTypeInProgress); break; case StopCalibrationCancelled: @@ -194,21 +204,95 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll break; } - _magCalInProgress = false; - _accelCalInProgress = false; - _compassMotCalInProgress = false; - _levelInProgress = false; + _calTypeInProgress = CalTypeNone; +} + +void APMSensorsComponentController::_mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle) +{ + Q_UNUSED(component); + Q_UNUSED(noReponseFromVehicle); + + if (_vehicle->id() != vehicleId) { + return; + } + + if (command == MAV_CMD_DO_CANCEL_MAG_CAL) { + disconnect(_vehicle, &Vehicle::mavCommandResult, this, &APMSensorsComponentController::_mavCommandResult); + if (result == MAV_RESULT_ACCEPTED) { + // Onboard mag cal is supported + _calTypeInProgress = CalTypeOnboardCompass; + _rgCompassCalProgress[0] = 0; + _rgCompassCalProgress[1] = 0; + _rgCompassCalProgress[2] = 0; + _rgCompassCalComplete[0] = false; + _rgCompassCalComplete[1] = false; + _rgCompassCalComplete[2] = false; + + _startLogCalibration(); + uint8_t compassBits = 0; + if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID")->rawValue().toInt() > 0) { + compassBits |= 1 << 0; + qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 1"; + } else { + _rgCompassCalComplete[0] = true; + _rgCompassCalSucceeded[0] = true; + _rgCompassCalFitness[0] = 0; + } + if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID2")->rawValue().toInt() > 0) { + compassBits |= 1 << 1; + qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 2"; + } else { + _rgCompassCalComplete[1] = true; + _rgCompassCalSucceeded[1] = MAG_CAL_SUCCESS; + _rgCompassCalFitness[1] = 0; + } + if (getParameterFact(FactSystem::defaultComponentId, "COMPASS_DEV_ID3")->rawValue().toInt() > 0) { + compassBits |= 1 << 2; + qCDebug(APMSensorsComponentControllerLog) << "Performing onboard compass cal for compass 3"; + } else { + _rgCompassCalComplete[2] = true; + _rgCompassCalSucceeded[2] = MAG_CAL_SUCCESS; + _rgCompassCalFitness[2] = 0; + } + + // We bump up the fitness value so calibration will always succeed + Fact* compassCalFitness = getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam); + _restoreCompassCalFitness = true; + _previousCompassCalFitness = compassCalFitness->rawValue().toFloat(); + getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam)->setRawValue(100.0); + + _appendStatusLog(tr("Rotate the vehicle randomly around all axes until the progress bar fills all the way to the right .")); + _vehicle->sendMavCommand(_vehicle->defaultComponentId(), + MAV_CMD_DO_START_MAG_CAL, + true, // showError + compassBits, // which compass(es) to calibrate + 0, // no retry on failure + 1, // save values after complete + 0, // no delayed start + 0); // no auto-reboot + + } else { + // Onboard mag cal is not supported + _compassCal.startCalibration(); + } + } else if (command == MAV_CMD_DO_START_MAG_CAL && result != MAV_RESULT_ACCEPTED) { + _restorePreviousCompassCalFitness(); + } } void APMSensorsComponentController::calibrateCompass(void) { - _startLogCalibration(); - _compassCal.startCalibration(); + // First we need to determine if the vehicle support onboard compass cal. There isn't an easy way to + // do this. A hack is to send the mag cancel command and see if it is accepted. + connect(_vehicle, &Vehicle::mavCommandResult, this, &APMSensorsComponentController::_mavCommandResult); + _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, false /* showError */); + + // Now we wait for the result to come back } void APMSensorsComponentController::calibrateAccel(void) { - _accelCalInProgress = true; + _calTypeInProgress = CalTypeAccel; _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); _uas->startCalibration(UASInterface::StartCalibrationAccel); @@ -216,7 +300,7 @@ void APMSensorsComponentController::calibrateAccel(void) void APMSensorsComponentController::calibrateMotorInterference(void) { - _compassMotCalInProgress = true; + _calTypeInProgress = CalTypeCompassMot; _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); _appendStatusLog(tr("Raise the throttle slowly to between 50% ~ 75% (the props will spin!) for 5 ~ 10 seconds.")); @@ -227,7 +311,7 @@ void APMSensorsComponentController::calibrateMotorInterference(void) void APMSensorsComponentController::levelHorizon(void) { - _levelInProgress = true; + _calTypeInProgress = CalTypeLevelHorizon; _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); _appendStatusLog(tr("Hold the vehicle in its level flight position.")); @@ -252,8 +336,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, QString percent = text.split("<").last().split(">").first(); bool ok; int p = percent.toInt(&ok); - if (ok) { - Q_ASSERT(_progressBar); + if (ok && _progressBar) { _progressBar->setProperty("value", (float)(p / 100.0)); } return; @@ -317,7 +400,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, _orientationCalAreaHelpText->setProperty("text", "Place your vehicle into one of the Incomplete orientations shown below and hold it still"); if (text == "accel") { - _accelCalInProgress = true; + _calTypeInProgress = CalTypeAccel; _orientationCalDownSideVisible = true; _orientationCalUpsideDownSideVisible = true; _orientationCalLeftSideVisible = true; @@ -325,7 +408,7 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, _orientationCalTailDownSideVisible = true; _orientationCalNoseDownSideVisible = true; } else if (text == "mag") { - _magCalInProgress = true; + _calTypeInProgress = CalTypeOffboardCompass; _orientationCalDownSideVisible = true; _orientationCalUpsideDownSideVisible = true; _orientationCalLeftSideVisible = true; @@ -349,37 +432,37 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, if (side == QLatin1Literal("down")) { _orientationCalDownSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalDownSideRotate = true; } } else if (side == QLatin1Literal("up")) { _orientationCalUpsideDownSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalUpsideDownSideRotate = true; } } else if (side == QLatin1Literal("left")) { _orientationCalLeftSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalLeftSideRotate = true; } } else if (side == QLatin1Literal("right")) { _orientationCalRightSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalRightSideRotate = true; } } else if (side == QLatin1Literal("front")) { _orientationCalNoseDownSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalNoseDownSideRotate = true; } } else if (side == QLatin1Literal("back")) { _orientationCalTailDownSideInProgress = true; - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalTailDownSideRotate = true; } } - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { _orientationCalAreaHelpText->setProperty("text", "Rotate the vehicle continuously as shown in the diagram until marked as Completed"); } else { _orientationCalAreaHelpText->setProperty("text", "Hold still in the current orientation"); @@ -472,17 +555,23 @@ void APMSensorsComponentController::_hideAllCalAreas(void) void APMSensorsComponentController::cancelCalibration(void) { - _waitingForCancel = true; - emit waitingForCancelChanged(); _cancelButton->setEnabled(false); - if (_magCalInProgress) { + if (_calTypeInProgress == CalTypeOffboardCompass) { + _waitingForCancel = true; + emit waitingForCancelChanged(); _compassCal.cancelCalibration(); + } else if (_calTypeInProgress == CalTypeOnboardCompass) { + _vehicle->sendMavCommand(_vehicle->defaultComponentId(), MAV_CMD_DO_CANCEL_MAG_CAL, true /* showError */); + _stopCalibration(StopCalibrationCancelled); } else { + _waitingForCancel = true; + emit waitingForCancelChanged(); // The firmware doesn't always allow us to cancel calibration. The best we can do is wait // for it to timeout. _uas->stopCalibration(); } + } void APMSensorsComponentController::nextClicked(void) @@ -500,7 +589,7 @@ void APMSensorsComponentController::nextClicked(void) _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg); - if (_compassMotCalInProgress) { + if (_calTypeInProgress == CalTypeCompassMot) { _stopCalibration(StopCalibrationSuccess); } } @@ -520,15 +609,9 @@ bool APMSensorsComponentController::usingUDPLink(void) return _vehicle->priorityLink()->getLinkConfiguration()->type() == LinkConfiguration::TypeUdp; } -void APMSensorsComponentController::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) +void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message) { - Q_UNUSED(link); - - if (message.sysid != _vehicle->id()) { - return; - } - - if (message.msgid == MAVLINK_MSG_ID_COMMAND_ACK && _levelInProgress) { + if (_calTypeInProgress == CalTypeLevelHorizon) { mavlink_command_ack_t commandAck; mavlink_msg_command_ack_decode(&message, &commandAck); @@ -546,3 +629,108 @@ void APMSensorsComponentController::_mavlinkMessageReceived(LinkInterface* link, } } } + +void APMSensorsComponentController::_handleMagCalProgress(mavlink_message_t& message) +{ + if (_calTypeInProgress == CalTypeOnboardCompass) { + mavlink_mag_cal_progress_t magCalProgress; + mavlink_msg_mag_cal_progress_decode(&message, &magCalProgress); + + qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalProgress id:mask:pct" + << magCalProgress.compass_id << magCalProgress.cal_mask << magCalProgress.completion_pct; + + // How many compasses are we calibrating? + int compassCalCount = 0; + for (int i=0; i<3; i++) { + if (magCalProgress.cal_mask & (1 << i)) { + compassCalCount++; + } + } + + if (magCalProgress.compass_id < 3) { + // Each compass gets a portion of the overall progress + _rgCompassCalProgress[magCalProgress.compass_id] = magCalProgress.completion_pct / compassCalCount; + } + + if (_progressBar) { + _progressBar->setProperty("value", (float)(_rgCompassCalProgress[0] + _rgCompassCalProgress[1] + _rgCompassCalProgress[2]) / 100.0); + } + } +} + +void APMSensorsComponentController::_handleMagCalReport(mavlink_message_t& message) +{ + if (_calTypeInProgress == CalTypeOnboardCompass) { + mavlink_mag_cal_report_t magCalReport; + mavlink_msg_mag_cal_report_decode(&message, &magCalReport); + + qCDebug(APMSensorsComponentControllerVerboseLog) << "_handleMagCalReport id:mask:status:fitness" + << magCalReport.compass_id << magCalReport.cal_mask << magCalReport.cal_status << magCalReport.fitness; + + bool additionalCompassCompleted = false; + if (magCalReport.compass_id < 3 && !_rgCompassCalComplete[magCalReport.compass_id]) { + if (magCalReport.cal_status == MAG_CAL_SUCCESS) { + _appendStatusLog(tr("Compass %1 calibration complete").arg(magCalReport.compass_id)); + } else { + _appendStatusLog(tr("Compass %1 calibration below quality threshold").arg(magCalReport.compass_id)); + } + _rgCompassCalComplete[magCalReport.compass_id] = true; + _rgCompassCalSucceeded[magCalReport.compass_id] = magCalReport.cal_status == MAG_CAL_SUCCESS; + _rgCompassCalFitness[magCalReport.compass_id] = magCalReport.fitness; + additionalCompassCompleted = true; + } + + if (_rgCompassCalComplete[0] && _rgCompassCalComplete[1] &&_rgCompassCalComplete[2]) { + for (int i=0; i<3; i++) { + qCDebug(APMSensorsComponentControllerLog) << QString("Onboard compass call report #%1: succeed:fitness %2:%3").arg(i).arg(_rgCompassCalSucceeded[i]).arg(_rgCompassCalFitness[i]); + } + emit compass1CalFitnessChanged(_rgCompassCalFitness[0]); + emit compass2CalFitnessChanged(_rgCompassCalFitness[1]); + emit compass3CalFitnessChanged(_rgCompassCalFitness[2]); + emit compass1CalSucceededChanged(_rgCompassCalSucceeded[0]); + emit compass2CalSucceededChanged(_rgCompassCalSucceeded[1]); + emit compass3CalSucceededChanged(_rgCompassCalSucceeded[2]); + if (_rgCompassCalSucceeded[0] && _rgCompassCalSucceeded[1] && _rgCompassCalSucceeded[2]) { + _appendStatusLog(tr("All compasses calibrated successfully")); + _appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW FOR NEW SETTINGS TO TAKE AFFECT")); + _stopCalibration(StopCalibrationSuccessShowLog); + } else { + _appendStatusLog(tr("Compass calibration failed")); + _appendStatusLog(tr("YOU MUST REBOOT YOUR VEHICLE NOW AND RETRY COMPASS CALIBRATION PRIOR TO FLIGHT")); + _stopCalibration(StopCalibrationFailed); + } + } else if (additionalCompassCompleted) { + _appendStatusLog(tr("Continue rotating...")); + } + + } +} + +void APMSensorsComponentController::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) +{ + Q_UNUSED(link); + + if (message.sysid != _vehicle->id()) { + return; + } + + switch (message.msgid) { + case MAVLINK_MSG_ID_COMMAND_ACK: + _handleCommandAck(message); + break; + case MAVLINK_MSG_ID_MAG_CAL_PROGRESS: + _handleMagCalProgress(message); + break; + case MAVLINK_MSG_ID_MAG_CAL_REPORT: + _handleMagCalReport(message); + break; + } +} + +void APMSensorsComponentController::_restorePreviousCompassCalFitness(void) +{ + if (_restoreCompassCalFitness) { + _restoreCompassCalFitness = false; + getParameterFact(FactSystem::defaultComponentId, _compassCalFitnessParam)->setRawValue(_previousCompassCalFitness); + } +} diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index ca7e7a350..16129f6c9 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h @@ -20,6 +20,7 @@ #include "APMCompassCal.h" Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog) +Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerVerboseLog) /// Sensors Component MVC Controller for SensorsComponent.qml. class APMSensorsComponentController : public FactPanelController @@ -28,6 +29,7 @@ class APMSensorsComponentController : public FactPanelController public: APMSensorsComponentController(void); + ~APMSensorsComponentController(); Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) @@ -75,7 +77,15 @@ public: Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged) Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged) - + + Q_PROPERTY(bool compass1CalSucceeded READ compass1CalSucceeded NOTIFY compass1CalSucceededChanged) + Q_PROPERTY(bool compass2CalSucceeded READ compass2CalSucceeded NOTIFY compass2CalSucceededChanged) + Q_PROPERTY(bool compass3CalSucceeded READ compass3CalSucceeded NOTIFY compass3CalSucceededChanged) + + Q_PROPERTY(double compass1CalFitness READ compass1CalFitness NOTIFY compass1CalFitnessChanged) + Q_PROPERTY(double compass2CalFitness READ compass2CalFitness NOTIFY compass2CalFitnessChanged) + Q_PROPERTY(double compass3CalFitness READ compass3CalFitness NOTIFY compass3CalFitnessChanged) + Q_INVOKABLE void calibrateCompass(void); Q_INVOKABLE void calibrateAccel(void); Q_INVOKABLE void calibrateMotorInterference(void); @@ -86,7 +96,25 @@ public: bool compassSetupNeeded(void) const; bool accelSetupNeeded(void) const; - + + typedef enum { + CalTypeAccel, + CalTypeOnboardCompass, + CalTypeOffboardCompass, + CalTypeLevelHorizon, + CalTypeCompassMot, + CalTypeNone + } CalType_t; + Q_ENUM(CalType_t) + + bool compass1CalSucceeded(void) const { return _rgCompassCalSucceeded[0]; } + bool compass2CalSucceeded(void) const { return _rgCompassCalSucceeded[1]; } + bool compass3CalSucceeded(void) const { return _rgCompassCalSucceeded[2]; } + + double compass1CalFitness(void) const { return _rgCompassCalFitness[0]; } + double compass2CalFitness(void) const { return _rgCompassCalFitness[1]; } + double compass3CalFitness(void) const { return _rgCompassCalFitness[2]; } + signals: void showGyroCalAreaChanged(void); void showOrientationCalAreaChanged(void); @@ -97,11 +125,18 @@ signals: void resetStatusTextArea(void); void waitingForCancelChanged(void); void setupNeededChanged(void); - void calibrationComplete(void); - + void calibrationComplete(CalType_t calType); + void compass1CalSucceededChanged(bool compass1CalSucceeded); + void compass2CalSucceededChanged(bool compass2CalSucceeded); + void compass3CalSucceededChanged(bool compass3CalSucceeded); + void compass1CalFitnessChanged(double compass1CalFitness); + void compass2CalFitnessChanged(double compass2CalFitness); + void compass3CalFitnessChanged(double compass3CalFitness); + private slots: void _handleUASTextMessage(int uasId, int compId, int severity, QString text); void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); + void _mavCommandResult(int vehicleId, int component, int command, int result, bool noReponseFromVehicle); private: void _startLogCalibration(void); @@ -110,7 +145,11 @@ private: void _refreshParams(void); void _hideAllCalAreas(void); void _resetInternalState(void); - + void _handleCommandAck(mavlink_message_t& message); + void _handleMagCalProgress(mavlink_message_t& message); + void _handleMagCalReport(mavlink_message_t& message); + void _restorePreviousCompassCalFitness(void); + enum StopCalibrationCode { StopCalibrationSuccess, StopCalibrationSuccessShowLog, @@ -137,11 +176,13 @@ private: bool _showOrientationCalArea; - bool _magCalInProgress; - bool _accelCalInProgress; - bool _compassMotCalInProgress; - bool _levelInProgress; - + CalType_t _calTypeInProgress; + + uint8_t _rgCompassCalProgress[3]; + bool _rgCompassCalComplete[3]; + bool _rgCompassCalSucceeded[3]; + float _rgCompassCalFitness[3]; + bool _orientationCalDownSideDone; bool _orientationCalUpsideDownSideDone; bool _orientationCalLeftSideDone; @@ -171,6 +212,10 @@ private: bool _orientationCalTailDownSideRotate; bool _waitingForCancel; + + bool _restoreCompassCalFitness; + float _previousCompassCalFitness; + static const char* _compassCalFitnessParam; static const int _supportedFirmwareCalVersion = 2; }; diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml index 37d0738ff..85bd33730 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentSummary.qml @@ -7,6 +7,7 @@ import QGroundControl.FactControls 1.0 import QGroundControl.Controls 1.0 import QGroundControl.Palette 1.0 import QGroundControl.Controllers 1.0 +import QGroundControl.ArduPilot 1.0 /* IMPORTANT NOTE: Any changes made here must also be made to SensorsComponentSummary.qml @@ -18,55 +19,35 @@ FactPanel { color: qgcPal.windowShadeDark QGCPalette { id: qgcPal; colorGroupEnabled: enabled } - APMSensorsComponentController { id: controller; factPanel: panel } - - property Fact compass1IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID") - property Fact compass2IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID2") - property Fact compass3IdFact: controller.getParameterFact(-1, "COMPASS_DEV_ID3") - - property Fact compass1OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS_X") - property Fact compass1OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS_Y") - property Fact compass1OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS_Z") - property Fact compass2OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS2_X") - property Fact compass2OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS2_Y") - property Fact compass2OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS2_Z") - property Fact compass3OfsXFact: controller.getParameterFact(-1, "COMPASS_OFS3_X") - property Fact compass3OfsYFact: controller.getParameterFact(-1, "COMPASS_OFS3_Y") - property Fact compass3OfsZFact: controller.getParameterFact(-1, "COMPASS_OFS3_Z") - - property bool compass1Available: compass1IdFact.value !== 0 - property bool compass2Available: compass2IdFact.value !== 0 - property bool compass3Available: compass3IdFact.value !== 0 - property bool compass1Calibrated: compass1Available ? compass1OfsXFact.value != 0.0 && compass1OfsYFact.value != 0.0 &&compass1OfsZFact.value != 0.0 : false - property bool compass2Calibrated: compass2Available ? compass2OfsXFact.value != 0.0 && compass2OfsYFact.value != 0.0 &&compass2OfsZFact.value != 0.0 : false - property bool compass3Calibrated: compass3Available ? compass3OfsXFact.value != 0.0 && compass3OfsYFact.value != 0.0 &&compass3OfsZFact.value != 0.0 : false + APMSensorsComponentController { id: controller; factPanel: panel } - property bool compassCalNeeded: controller.compassSetupNeeded + APMSensorParams { + id: sensorParams + factPanelController: controller + } Column { anchors.fill: parent - VehicleSummaryRow { - labelText: qsTr("Compass 1:") - visible: compass1Available - valueText: compass1Calibrated ? qsTr("Ready") : qsTr("Setup required") - } - - VehicleSummaryRow { - labelText: qsTr("Compass 2:") - visible: compass2Available - valueText: compass2Calibrated ? qsTr("Ready") : qsTr("Setup required") - } - - VehicleSummaryRow { - labelText: qsTr("Compass 3:") - visible: compass3Available - valueText: compass3Calibrated ? qsTr("Ready") : qsTr("Setup required") + Repeater { + model: 3 + + VehicleSummaryRow { + labelText: qsTr("Compass ") + (index + 1) + ":" + valueText: sensorParams.rgCompassAvailable[index] ? + (sensorParams.rgCompassCalibrated[index] ? + (sensorParams.rgCompassPrimary[index] ? "Primary" : "Secondary") + + (sensorParams.rgCompassExternalParamAvailable[index] ? + (sensorParams.rgCompassExternal[index] ? ", External" : ", Internal" ) : + "") : + qsTr("Setup required")) : + qsTr("Not installed") + } } VehicleSummaryRow { - labelText: qsTr("Accelerometer:") + labelText: qsTr("Accelerometer(s):") valueText: controller.accelSetupNeeded ? qsTr("Setup required") : qsTr("Ready") } } diff --git a/src/FirmwarePlugin/APM/APMSensorParams.qml b/src/FirmwarePlugin/APM/APMSensorParams.qml new file mode 100644 index 000000000..217ba564d --- /dev/null +++ b/src/FirmwarePlugin/APM/APMSensorParams.qml @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * (c) 2009-2016 QGROUNDCONTROL PROJECT + * + * QGroundControl is licensed according to the terms in the file + * COPYING.md in the root of the source code directory. + * + ****************************************************************************/ + +import QtQuick 2.2 + +import QGroundControl.FactSystem 1.0 + +/// This is used to handle the various differences between firmware versions and missing parameters in each in a standard way. +Item { + property var factPanelController ///< Must be specified by consumer of control + + property Fact _noFact: Fact { } + + property Fact compassPrimaryFact: factPanelController.getParameterFact(-1, "COMPASS_PRIMARY") + property bool compass1Primary: compassPrimaryFact.rawValue == 0 + property bool compass2Primary: compassPrimaryFact.rawValue == 1 + property bool compass3Primary: compassPrimaryFact.rawValue == 2 + property var rgCompassPrimary: [ compass1Primary, compass2Primary, compass3Primary ] + + property Fact compass1Id: factPanelController.getParameterFact(-1, "COMPASS_DEV_ID") + property Fact compass2Id: factPanelController.getParameterFact(-1, "COMPASS_DEV_ID2") + property Fact compass3Id: factPanelController.getParameterFact(-1, "COMPASS_DEV_ID3") + + property bool compass1Available: compass1Id.value > 0 + property bool compass2Available: compass2Id.value > 0 + property bool compass3Available: compass3Id.value > 0 + property var rgCompassAvailable: [ compass1Available, compass2Available, compass3Available ] + + property bool compass1RotParamAvailable: factPanelController.parameterExists(-1, "COMPASS_ORIENT") + property bool compass2RotParamAvailable: factPanelController.parameterExists(-1, "COMPASS_ORIENT2") + property bool compass3RotParamAvailable: factPanelController.parameterExists(-1, "COMPASS_ORIENT3") + property var rgCompassRotParamAvailable: [ compass1RotParamAvailable, compass2RotParamAvailable, compass3RotParamAvailable ] + + property Fact compass1RotFact: compass2RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT") : _noFact + property Fact compass2RotFact: compass2RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT2") : _noFact + property Fact compass3RotFact: compass3RotParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_ORIENT3") : _noFact + property var rgCompassRotFact: [ compass1RotFact, compass2RotFact, compass3RotFact ] + + property bool compass1UseParamAvailable: factPanelController.parameterExists(-1, "COMPASS_USE") + property bool compass2UseParamAvailable: factPanelController.parameterExists(-1, "COMPASS_USE2") + property bool compass3UseParamAvailable: factPanelController.parameterExists(-1, "COMPASS_USE3") + property var rgCompassUseParamAvailable: [ compass1UseParamAvailable, compass2UseParamAvailable, compass3UseParamAvailable ] + + property Fact compass1UseFact: compass1UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE") : _noFact + property Fact compass2UseFact: compass2UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE2") : _noFact + property Fact compass3UseFact: compass3UseParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_USE3") : _noFact + property var rgCompassUseFact: [ compass1UseFact, compass2UseFact, compass3UseFact ] + + property bool compass1Use: compass1UseParamAvailable ? compass1UseFact.value : true + property bool compass2Use: compass2UseParamAvailable ? compass2UseFact.value : true + property bool compass3Use: compass3UseParamAvailable ? compass3UseFact.value : true + + property bool compass1ExternalParamAvailable: factPanelController.parameterExists(-1, "COMPASS_EXTERNAL") + property bool compass2ExternalParamAvailable: factPanelController.parameterExists(-1, "COMPASS_EXTERN2") + property bool compass3ExternalParamAvailable: factPanelController.parameterExists(-1, "COMPASS_EXTERN3") + property var rgCompassExternalParamAvailable: [ compass1ExternalParamAvailable, compass2ExternalParamAvailable, compass3ExternalParamAvailable ] + + property Fact compass1ExternalFact: compass1ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERNAL") : _noFact + property Fact compass2ExternalFact: compass2ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERN2") : _noFact + property Fact compass3ExternalFact: compass3ExternalParamAvailable ? factPanelController.getParameterFact(-1, "COMPASS_EXTERN3") : _noFact + + property bool compass1External: !!compass1ExternalFact.rawValue + property bool compass2External: !!compass2ExternalFact.rawValue + property bool compass3External: !!compass3ExternalFact.rawValue + property var rgCompassExternal: [ compass1External, compass2External, compass3External ] + + property Fact compass1OfsXFact: factPanelController.getParameterFact(-1, "COMPASS_OFS_X") + property Fact compass1OfsYFact: factPanelController.getParameterFact(-1, "COMPASS_OFS_Y") + property Fact compass1OfsZFact: factPanelController.getParameterFact(-1, "COMPASS_OFS_Z") + property Fact compass2OfsXFact: factPanelController.getParameterFact(-1, "COMPASS_OFS2_X") + property Fact compass2OfsYFact: factPanelController.getParameterFact(-1, "COMPASS_OFS2_Y") + property Fact compass2OfsZFact: factPanelController.getParameterFact(-1, "COMPASS_OFS2_Z") + property Fact compass3OfsXFact: factPanelController.getParameterFact(-1, "COMPASS_OFS3_X") + property Fact compass3OfsYFact: factPanelController.getParameterFact(-1, "COMPASS_OFS3_Y") + property Fact compass3OfsZFact: factPanelController.getParameterFact(-1, "COMPASS_OFS3_Z") + + property bool compass1Calibrated: compass1Available ? compass1OfsXFact.value != 0.0 && compass1OfsYFact.value != 0.0 &&compass1OfsZFact.value != 0.0 : false + property bool compass2Calibrated: compass2Available ? compass2OfsXFact.value != 0.0 && compass2OfsYFact.value != 0.0 &&compass2OfsZFact.value != 0.0 : false + property bool compass3Calibrated: compass3Available ? compass3OfsXFact.value != 0.0 && compass3OfsYFact.value != 0.0 &&compass3OfsZFact.value != 0.0 : false + property var rgCompassCalibrated: [ compass1Calibrated, compass2Calibrated, compass3Calibrated ] +} diff --git a/src/FirmwarePlugin/APM/QGroundControl.ArduPilot.qmldir b/src/FirmwarePlugin/APM/QGroundControl.ArduPilot.qmldir new file mode 100644 index 000000000..54a0a5f36 --- /dev/null +++ b/src/FirmwarePlugin/APM/QGroundControl.ArduPilot.qmldir @@ -0,0 +1,3 @@ +Module QGroundControl.ArduPilot + +APMSensorParams 1.0 APMSensorParams.qml diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index 6b946f30f..24def6a8a 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -188,14 +188,14 @@ void MockLink::_loadParams(void) if (_firmwareType == MAV_AUTOPILOT_ARDUPILOTMEGA) { if (_vehicleType == MAV_TYPE_FIXED_WING) { - paramFile.setFileName(":/unittest/APMArduPlaneMockLink.params"); + paramFile.setFileName(":/MockLink/APMArduPlaneMockLink.params"); } else if (_vehicleType == MAV_TYPE_SUBMARINE ) { - paramFile.setFileName(":/unittest/APMArduSubMockLink.params"); + paramFile.setFileName(":/MockLink/APMArduSubMockLink.params"); } else { - paramFile.setFileName(":/unittest/APMArduCopterMockLink.params"); + paramFile.setFileName(":/MockLink/APMArduCopterMockLink.params"); } } else { - paramFile.setFileName(":/unittest/PX4MockLink.params"); + paramFile.setFileName(":/MockLink/PX4MockLink.params"); } -- 2.22.0