diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml index 4148cd0..82a1ad6 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponent.qml +++ b/src/AutoPilotPlugins/APM/APMSensorsComponent.qml @@ -569,6 +569,26 @@ SetupPage { } // QGCViewDialog } // Component - calibratePressureDialogComponent + Component { + id: calibrateGyroDialogComponent + + QGCViewDialog { + id: calibrateGyroDialog + + function accept() { + controller.calibrateGyro() + calibrateGyroDialog.hideDialog() + } + + QGCLabel { + anchors.left: parent.left + anchors.right: parent.right + wrapMode: Text.WordWrap + text: qsTr("For Gyroscope calibration you will need to place your vehicle on a surface and leave it still.\n\nClick Ok to start calibration.") + } + } + } + QGCFlickable { id: buttonFlickable anchors.left: parent.left @@ -623,10 +643,17 @@ SetupPage { QGCButton { width: _buttonWidth + text: qsTr("Gyro") + visible: activeVehicle && (activeVehicle.multiRotor | activeVehicle.rover) + onClicked: mainWindow.showComponentDialog(calibrateGyroDialogComponent, qsTr("Calibrate Gyro"), mainWindow.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) + } + + QGCButton { + width: _buttonWidth text: _calibratePressureText onClicked: mainWindow.showComponentDialog(calibratePressureDialogComponent, _calibratePressureText, mainWindow.showDialogDefaultWidth, StandardButton.Cancel | StandardButton.Ok) - readonly property string _calibratePressureText: activeVehicle.fixedWing ? qsTr("Cal Baro/Airspeed") : qsTr("Calibrate Pressure") + readonly property string _calibratePressureText: activeVehicle.fixedWing ? qsTr("Baro/Airspeed") : qsTr("Pressure") } QGCButton { diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc index 758d300..f2dbe92 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.cc @@ -294,7 +294,7 @@ void APMSensorsComponentController::calibrateAccel(void) _calTypeInProgress = CalTypeAccel; _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAccel); + _vehicle->startCalibration(Vehicle::CalibrationAccel); } void APMSensorsComponentController::calibrateMotorInterference(void) @@ -305,7 +305,7 @@ void APMSensorsComponentController::calibrateMotorInterference(void) _appendStatusLog(tr("Raise the throttle slowly to between 50% ~ 75% (the props will spin!) for 5 ~ 10 seconds.")); _appendStatusLog(tr("Quickly bring the throttle back down to zero")); _appendStatusLog(tr("Press the Next button to complete the calibration")); - _uas->startCalibration(UASInterface::StartCalibrationCompassMot); + _vehicle->startCalibration(Vehicle::CalibrationAPMCompassMot); } void APMSensorsComponentController::levelHorizon(void) @@ -314,7 +314,7 @@ void APMSensorsComponentController::levelHorizon(void) _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); _appendStatusLog(tr("Hold the vehicle in its level flight position.")); - _uas->startCalibration(UASInterface::StartCalibrationLevel); + _vehicle->startCalibration(Vehicle::CalibrationLevel); } void APMSensorsComponentController::calibratePressure(void) @@ -323,7 +323,16 @@ void APMSensorsComponentController::calibratePressure(void) _vehicle->setConnectionLostEnabled(false); _startLogCalibration(); _appendStatusLog(tr("Requesting pressure calibration...")); - _uas->startCalibration(UASInterface::StartCalibrationPressure); + _vehicle->startCalibration(Vehicle::CalibrationAPMPressureAirspeed); +} + +void APMSensorsComponentController::calibrateGyro(void) +{ + _calTypeInProgress = CalTypeGyro; + _vehicle->setConnectionLostEnabled(false); + _startLogCalibration(); + _appendStatusLog(tr("Requesting gyro calibration...")); + _vehicle->startCalibration(Vehicle::CalibrationGyro); } void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) @@ -444,203 +453,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId, _stopCalibration(StopCalibrationFailed); return; } - -#if 0 - - if (text.contains(QLatin1Literal("progress <"))) { - QString percent = text.split("<").last().split(">").first(); - bool ok; - int p = percent.toInt(&ok); - if (ok && _progressBar) { - _progressBar->setProperty("value", (float)(p / 100.0)); - } - return; - } - - QString anyKey(QStringLiteral("and press any")); - if (text.contains(anyKey)) { - text = text.left(text.indexOf(anyKey)) + QStringLiteral("and click Next to continue."); - _nextButton->setEnabled(true); - } - - _appendStatusLog(text); - qCDebug(APMSensorsComponentControllerLog) << text << severity; - - if (text.contains(QLatin1String("Calibration successful"))) { - _stopCalibration(StopCalibrationSuccess); - return; - } - - if (text.contains(QLatin1String("FAILED"))) { - _stopCalibration(StopCalibrationFailed); - return; - } - - // All calibration messages start with [cal] - QString calPrefix(QStringLiteral("[cal] ")); - if (!text.startsWith(calPrefix)) { - return; - } - text = text.right(text.length() - calPrefix.length()); - - QString calStartPrefix(QStringLiteral("calibration started: ")); - if (text.startsWith(calStartPrefix)) { - text = text.right(text.length() - calStartPrefix.length()); - - _startVisualCalibration(); - - if (text == QLatin1Literal("accel") || text == QLatin1Literal("mag") || text == QLatin1Literal("gyro")) { - // Reset all progress indication - _orientationCalDownSideDone = false; - _orientationCalUpsideDownSideDone = false; - _orientationCalLeftSideDone = false; - _orientationCalRightSideDone = false; - _orientationCalTailDownSideDone = false; - _orientationCalNoseDownSideDone = false; - _orientationCalDownSideInProgress = false; - _orientationCalUpsideDownSideInProgress = false; - _orientationCalLeftSideInProgress = false; - _orientationCalRightSideInProgress = false; - _orientationCalNoseDownSideInProgress = false; - _orientationCalTailDownSideInProgress = false; - - // Reset all visibility - _orientationCalDownSideVisible = false; - _orientationCalUpsideDownSideVisible = false; - _orientationCalLeftSideVisible = false; - _orientationCalRightSideVisible = false; - _orientationCalTailDownSideVisible = false; - _orientationCalNoseDownSideVisible = false; - - _orientationCalAreaHelpText->setProperty("text", "Place your vehicle into one of the Incomplete orientations shown below and hold it still"); - - if (text == "accel") { - _calTypeInProgress = CalTypeAccel; - _orientationCalDownSideVisible = true; - _orientationCalUpsideDownSideVisible = true; - _orientationCalLeftSideVisible = true; - _orientationCalRightSideVisible = true; - _orientationCalTailDownSideVisible = true; - _orientationCalNoseDownSideVisible = true; - } else if (text == "mag") { - _calTypeInProgress = CalTypeOffboardCompass; - _orientationCalDownSideVisible = true; - _orientationCalUpsideDownSideVisible = true; - _orientationCalLeftSideVisible = true; - _orientationCalRightSideVisible = true; - _orientationCalTailDownSideVisible = true; - _orientationCalNoseDownSideVisible = true; - } else { - Q_ASSERT(false); - } - emit orientationCalSidesDoneChanged(); - emit orientationCalSidesVisibleChanged(); - emit orientationCalSidesInProgressChanged(); - _updateAndEmitShowOrientationCalArea(true); - } - return; - } - - if (text.endsWith(QLatin1Literal("orientation detected"))) { - QString side = text.section(" ", 0, 0); - qDebug() << "Side started" << side; - - if (side == QLatin1Literal("down")) { - _orientationCalDownSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalDownSideRotate = true; - } - } else if (side == QLatin1Literal("up")) { - _orientationCalUpsideDownSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalUpsideDownSideRotate = true; - } - } else if (side == QLatin1Literal("left")) { - _orientationCalLeftSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalLeftSideRotate = true; - } - } else if (side == QLatin1Literal("right")) { - _orientationCalRightSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalRightSideRotate = true; - } - } else if (side == QLatin1Literal("front")) { - _orientationCalNoseDownSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalNoseDownSideRotate = true; - } - } else if (side == QLatin1Literal("back")) { - _orientationCalTailDownSideInProgress = true; - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalTailDownSideRotate = true; - } - } - - if (_calTypeInProgress == CalTypeOffboardCompass) { - _orientationCalAreaHelpText->setProperty("text", tr("Rotate the vehicle continuously as shown in the diagram until marked as Completed")); - } else { - _orientationCalAreaHelpText->setProperty("text", tr("Hold still in the current orientation")); - } - - emit orientationCalSidesInProgressChanged(); - emit orientationCalSidesRotateChanged(); - return; - } - - if (text.endsWith(QLatin1Literal("side done, rotate to a different side"))) { - QString side = text.section(" ", 0, 0); - qDebug() << "Side finished" << side; - - if (side == QLatin1Literal("down")) { - _orientationCalDownSideInProgress = false; - _orientationCalDownSideDone = true; - _orientationCalDownSideRotate = false; - } else if (side == QLatin1Literal("up")) { - _orientationCalUpsideDownSideInProgress = false; - _orientationCalUpsideDownSideDone = true; - _orientationCalUpsideDownSideRotate = false; - } else if (side == QLatin1Literal("left")) { - _orientationCalLeftSideInProgress = false; - _orientationCalLeftSideDone = true; - _orientationCalLeftSideRotate = false; - } else if (side == QLatin1Literal("right")) { - _orientationCalRightSideInProgress = false; - _orientationCalRightSideDone = true; - _orientationCalRightSideRotate = false; - } else if (side == QLatin1Literal("front")) { - _orientationCalNoseDownSideInProgress = false; - _orientationCalNoseDownSideDone = true; - _orientationCalNoseDownSideRotate = false; - } else if (side == QLatin1Literal("back")) { - _orientationCalTailDownSideInProgress = false; - _orientationCalTailDownSideDone = true; - _orientationCalTailDownSideRotate = false; - } - - _orientationCalAreaHelpText->setProperty("text", tr("Place you vehicle into one of the orientations shown below and hold it still")); - - emit orientationCalSidesInProgressChanged(); - emit orientationCalSidesDoneChanged(); - emit orientationCalSidesRotateChanged(); - return; - } - - if (text.startsWith(QLatin1Literal("calibration done:"))) { - _stopCalibration(StopCalibrationSuccess); - return; - } - - if (text.startsWith(QLatin1Literal("calibration cancelled"))) { - _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed); - return; - } - - if (text.startsWith(QLatin1Literal("calibration failed"))) { - _stopCalibration(StopCalibrationFailed); - return; - } -#endif } void APMSensorsComponentController::_refreshParams(void) @@ -685,7 +497,7 @@ void APMSensorsComponentController::cancelCalibration(void) 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(); + _vehicle->stopCalibration(); } } @@ -728,36 +540,18 @@ bool APMSensorsComponentController::usingUDPLink(void) void APMSensorsComponentController::_handleCommandAck(mavlink_message_t& message) { - if (_calTypeInProgress == CalTypeLevelHorizon) { - 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("Level horizon complete")); - _stopCalibration(StopCalibrationSuccessShowLog); - break; - default: - _appendStatusLog(tr("Level horizon failed")); - _stopCalibration(StopCalibrationFailed); - break; - } - } - } - - if (_calTypeInProgress == CalTypePressure) { + if (_calTypeInProgress == CalTypeLevelHorizon || _calTypeInProgress == CalTypeGyro || _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")); + _appendStatusLog(tr("Successfully completed")); _stopCalibration(StopCalibrationSuccessShowLog); break; default: - _appendStatusLog(tr("Pressure calibration fail")); + _appendStatusLog(tr("Failed")); _stopCalibration(StopCalibrationFailed); break; } diff --git a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h index 238d403..b06f733 100644 --- a/src/AutoPilotPlugins/APM/APMSensorsComponentController.h +++ b/src/AutoPilotPlugins/APM/APMSensorsComponentController.h @@ -7,9 +7,7 @@ * ****************************************************************************/ - -#ifndef APMSensorsComponentController_H -#define APMSensorsComponentController_H +#pragma once #include @@ -31,70 +29,72 @@ public: APMSensorsComponentController(void); ~APMSensorsComponentController(); - Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) - Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) + Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) + Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) - Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) - Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton) - Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText) + Q_PROPERTY(QQuickItem* nextButton MEMBER _nextButton) + Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton) + Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText) - Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged) - Q_PROPERTY(bool accelSetupNeeded READ accelSetupNeeded NOTIFY setupNeededChanged) + Q_PROPERTY(bool compassSetupNeeded READ compassSetupNeeded NOTIFY setupNeededChanged) + Q_PROPERTY(bool accelSetupNeeded READ accelSetupNeeded NOTIFY setupNeededChanged) - Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged) + Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged) - Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalRightSideDone MEMBER _orientationCalRightSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalNoseDownSideDone MEMBER _orientationCalNoseDownSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalTailDownSideDone MEMBER _orientationCalTailDownSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalRightSideDone MEMBER _orientationCalRightSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalNoseDownSideDone MEMBER _orientationCalNoseDownSideDone NOTIFY orientationCalSidesDoneChanged) + Q_PROPERTY(bool orientationCalTailDownSideDone MEMBER _orientationCalTailDownSideDone NOTIFY orientationCalSidesDoneChanged) - Q_PROPERTY(bool orientationCalDownSideVisible MEMBER _orientationCalDownSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalUpsideDownSideVisible MEMBER _orientationCalUpsideDownSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalLeftSideVisible MEMBER _orientationCalLeftSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalRightSideVisible MEMBER _orientationCalRightSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalNoseDownSideVisible MEMBER _orientationCalNoseDownSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalTailDownSideVisible MEMBER _orientationCalTailDownSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalDownSideVisible MEMBER _orientationCalDownSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalUpsideDownSideVisible MEMBER _orientationCalUpsideDownSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalLeftSideVisible MEMBER _orientationCalLeftSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalRightSideVisible MEMBER _orientationCalRightSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalNoseDownSideVisible MEMBER _orientationCalNoseDownSideVisible NOTIFY orientationCalSidesVisibleChanged) + Q_PROPERTY(bool orientationCalTailDownSideVisible MEMBER _orientationCalTailDownSideVisible NOTIFY orientationCalSidesVisibleChanged) - Q_PROPERTY(bool orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalRightSideInProgress MEMBER _orientationCalRightSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalNoseDownSideInProgress MEMBER _orientationCalNoseDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalRightSideInProgress MEMBER _orientationCalRightSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalNoseDownSideInProgress MEMBER _orientationCalNoseDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) + Q_PROPERTY(bool orientationCalTailDownSideInProgress MEMBER _orientationCalTailDownSideInProgress NOTIFY orientationCalSidesInProgressChanged) - Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool orientationCalUpsideDownSideRotate MEMBER _orientationCalUpsideDownSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool orientationCalRightSideRotate MEMBER _orientationCalRightSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalDownSideRotate MEMBER _orientationCalDownSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalUpsideDownSideRotate MEMBER _orientationCalUpsideDownSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalRightSideRotate MEMBER _orientationCalRightSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged) + Q_PROPERTY(bool orientationCalTailDownSideRotate MEMBER _orientationCalTailDownSideRotate NOTIFY orientationCalSidesRotateChanged) - Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged) + 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(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_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); - 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); + Q_INVOKABLE void calibrateCompass (void); + Q_INVOKABLE void calibrateAccel (void); + Q_INVOKABLE void calibrateGyro (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); - bool compassSetupNeeded(void) const; - bool accelSetupNeeded(void) const; + bool compassSetupNeeded (void) const; + bool accelSetupNeeded (void) const; typedef enum { CalTypeAccel, + CalTypeGyro, CalTypeOnboardCompass, CalTypeOffboardCompass, CalTypeLevelHorizon, @@ -113,40 +113,40 @@ public: double compass3CalFitness(void) const { return _rgCompassCalFitness[2]; } signals: - void showGyroCalAreaChanged(void); - void showOrientationCalAreaChanged(void); - void orientationCalSidesDoneChanged(void); - void orientationCalSidesVisibleChanged(void); - void orientationCalSidesInProgressChanged(void); - void orientationCalSidesRotateChanged(void); - void resetStatusTextArea(void); - void waitingForCancelChanged(void); - void setupNeededChanged(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); - void setAllCalButtonsEnabled(bool enabled); + void showGyroCalAreaChanged (void); + void showOrientationCalAreaChanged (void); + void orientationCalSidesDoneChanged (void); + void orientationCalSidesVisibleChanged (void); + void orientationCalSidesInProgressChanged (void); + void orientationCalSidesRotateChanged (void); + void resetStatusTextArea (void); + void waitingForCancelChanged (void); + void setupNeededChanged (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); + void setAllCalButtonsEnabled (bool enabled); private slots: - void _handleUASTextMessage(int uasId, int compId, int severity, QString text); + 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); + void _mavCommandResult (int vehicleId, int component, int command, int result, bool noReponseFromVehicle); private: - void _startLogCalibration(void); - void _startVisualCalibration(void); - void _appendStatusLog(const QString& text); - 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); + void _startLogCalibration (void); + void _startVisualCalibration (void); + void _appendStatusLog (const QString& text); + 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, @@ -212,5 +212,3 @@ private: static const int _supportedFirmwareCalVersion = 2; }; - -#endif diff --git a/src/AutoPilotPlugins/Common/RadioComponentController.cc b/src/AutoPilotPlugins/Common/RadioComponentController.cc index eebd44e..3f08e26 100644 --- a/src/AutoPilotPlugins/Common/RadioComponentController.cc +++ b/src/AutoPilotPlugins/Common/RadioComponentController.cc @@ -701,7 +701,7 @@ void RadioComponentController::_writeCalibration(void) if (!_uas) return; if (_px4Vehicle()) { - _uas->stopCalibration(); + _vehicle->stopCalibration(); } if (!_px4Vehicle() && (_vehicle->vehicleType() == MAV_TYPE_HELICOPTER || _vehicle->multiRotor()) && _rgChannelInfo[_rgFunctionChannelMapping[rcCalFunctionThrottle]].reversed) { @@ -798,7 +798,7 @@ void RadioComponentController::_startCalibration(void) // Let the mav known we are starting calibration. This should turn off motors and so forth. if (_px4Vehicle()) { - _uas->startCalibration(UASInterface::StartCalibrationRadio); + _vehicle->startCalibration(Vehicle::CalibrationRadio); } _nextButton->setProperty("text", tr("Next")); @@ -815,7 +815,7 @@ void RadioComponentController::_stopCalibration(void) if (_uas) { if (_px4Vehicle()) { - _uas->stopCalibration(); + _vehicle->stopCalibration(); } _setInternalCalibrationValuesFromParameters(); } else { @@ -1024,7 +1024,7 @@ void RadioComponentController::_signalAllAttitudeValueChanges(void) void RadioComponentController::copyTrims(void) { - _uas->startCalibration(UASInterface::StartCalibrationCopyTrims); + _vehicle->startCalibration(Vehicle::CalibrationCopyTrims); } bool RadioComponentController::_px4Vehicle(void) const diff --git a/src/AutoPilotPlugins/PX4/PowerComponentController.cc b/src/AutoPilotPlugins/PX4/PowerComponentController.cc index 896ac97..9b45c0f 100644 --- a/src/AutoPilotPlugins/PX4/PowerComponentController.cc +++ b/src/AutoPilotPlugins/PX4/PowerComponentController.cc @@ -7,10 +7,6 @@ * ****************************************************************************/ - -/// @file -/// @author Don Gagne - #include "PowerComponentController.h" #include "QGCMAVLink.h" #include "UAS.h" @@ -27,7 +23,7 @@ void PowerComponentController::calibrateEsc(void) { _warningMessages.clear(); connect(_vehicle, &Vehicle::textMessageReceived, this, &PowerComponentController::_handleUASTextMessage); - _uas->startCalibration(UASInterface::StartCalibrationEsc); + _vehicle->startCalibration(Vehicle::CalibrationEsc); } void PowerComponentController::busConfigureActuators(void) diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index d8124fb..c592f9d 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -192,31 +192,31 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St void SensorsComponentController::calibrateGyro(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationGyro); + _vehicle->startCalibration(Vehicle::CalibrationGyro); } void SensorsComponentController::calibrateCompass(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationMag); + _vehicle->startCalibration(Vehicle::CalibrationMag); } void SensorsComponentController::calibrateAccel(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAccel); + _vehicle->startCalibration(Vehicle::CalibrationAccel); } void SensorsComponentController::calibrateLevel(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationLevel); + _vehicle->startCalibration(Vehicle::CalibrationLevel); } void SensorsComponentController::calibrateAirspeed(void) { _startLogCalibration(); - _uas->startCalibration(UASInterface::StartCalibrationAirspeed); + _vehicle->startCalibration(Vehicle::CalibrationPX4Airspeed); } void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) @@ -485,5 +485,5 @@ void SensorsComponentController::cancelCalibration(void) _waitingForCancel = true; emit waitingForCancelChanged(); _cancelButton->setEnabled(false); - _uas->stopCalibration(); + _vehicle->stopCalibration(); } diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index f5be62e..1a98291 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -1325,8 +1325,8 @@ void Vehicle::_handleHighLatency(mavlink_message_t& message) const double altitude; } coordinate { highLatency.latitude / (double)1E7, - highLatency.longitude / (double)1E7, - static_cast(highLatency.altitude_amsl) + highLatency.longitude / (double)1E7, + static_cast(highLatency.altitude_amsl) }; _coordinate.setLatitude(coordinate.latitude); @@ -3584,6 +3584,87 @@ void Vehicle::rebootVehicle() sendMessageOnLinkThreadSafe(priorityLink(), msg); } +void Vehicle::startCalibration(Vehicle::CalibrationType calType) +{ + float param1 = 0; + float param2 = 0; + float param3 = 0; + float param4 = 0; + float param5 = 0; + float param6 = 0; + float param7 = 0; + + switch (calType) { + case CalibrationGyro: + param1 = 1; + break; + case CalibrationMag: + param2 = 1; + break; + case CalibrationRadio: + param4 = 1; + break; + case CalibrationCopyTrims: + param4 = 2; + break; + case CalibrationAccel: + param5 = 1; + break; + case CalibrationLevel: + param5 = 2; + break; + case CalibrationEsc: + param7 = 1; + break; + case CalibrationPX4Airspeed: + param6 = 1; + break; + case CalibrationPX4Pressure: + param3 = 1; + break; + case CalibrationAPMCompassMot: + param6 = 1; + break; + case CalibrationAPMPressureAirspeed: + param3 = 1; + break; + case CalibrationAPMPreFlight: + param3 = 1; // GroundPressure/Airspeed + if (multiRotor() || rover()) { + // Gyro cal for ArduCopter only + param1 = 1; + } + } + + // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn + // causes the retry logic to break down. + mavlink_message_t msg; + mavlink_msg_command_long_pack_chan(_mavlink->getSystemId(), + _mavlink->getComponentId(), + priorityLink()->mavlinkChannel(), + &msg, + id(), + defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + 0, // 0=first transmission of command + param1, param2, param3, param4, param5, param6, param7); + sendMessageOnLinkThreadSafe(priorityLink(), msg); +} + +void Vehicle::stopCalibration(void) +{ + sendMavCommand(defaultComponentId(), // target component + MAV_CMD_PREFLIGHT_CALIBRATION, // command id + true, // showError + 0, // gyro cal + 0, // mag cal + 0, // ground pressure + 0, // radio cal + 0, // accel cal + 0, // airspeed cal + 0); // unused +} + void Vehicle::setSoloFirmware(bool soloFirmware) { if (soloFirmware != _soloFirmware) { diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index f879085..8cb64c9 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -645,7 +645,7 @@ public: Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) - Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) + Q_PROPERTY(bool iARDURsROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) Q_PROPERTY(CheckList checkListState READ checkListState WRITE setCheckListState NOTIFY checkListStateChanged) Q_PROPERTY(bool readyToFlyAvailable READ readyToFlyAvailable NOTIFY readyToFlyAvailableChanged) ///< true: readyToFly signalling is available on this vehicle Q_PROPERTY(bool readyToFly READ readyToFly NOTIFY readyToFlyChanged) @@ -985,6 +985,24 @@ public: /// @return the maximum version unsigned maxProtoVersion () const { return _maxProtoVersion; } + enum CalibrationType { + CalibrationRadio, + CalibrationGyro, + CalibrationMag, + CalibrationAccel, + CalibrationLevel, + CalibrationEsc, + CalibrationCopyTrims, + CalibrationAPMCompassMot, + CalibrationAPMPressureAirspeed, + CalibrationAPMPreFlight, + CalibrationPX4Airspeed, + CalibrationPX4Pressure, + }; + + void startCalibration (CalibrationType calType); + void stopCalibration (void); + Fact* roll () { return &_rollFact; } Fact* pitch () { return &_pitchFact; } Fact* heading () { return &_headingFact; } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f793493..ea6e4da 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -348,95 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message) #pragma warning(pop, 0) #endif -void UAS::startCalibration(UASInterface::StartCalibrationType calType) -{ - if (!_vehicle) { - return; - } - - int gyroCal = 0; - int magCal = 0; - int airspeedCal = 0; - int radioCal = 0; - int accelCal = 0; - int pressureCal = 0; - int escCal = 0; - - switch (calType) { - case StartCalibrationGyro: - gyroCal = 1; - break; - case StartCalibrationMag: - magCal = 1; - break; - case StartCalibrationAirspeed: - airspeedCal = 1; - break; - case StartCalibrationRadio: - radioCal = 1; - break; - case StartCalibrationCopyTrims: - radioCal = 2; - break; - case StartCalibrationAccel: - accelCal = 1; - break; - case StartCalibrationLevel: - accelCal = 2; - break; - case StartCalibrationPressure: - pressureCal = 1; - break; - case StartCalibrationEsc: - escCal = 1; - break; - case StartCalibrationUavcanEsc: - escCal = 2; - break; - case StartCalibrationCompassMot: - airspeedCal = 1; // ArduPilot, bit of a hack - break; - } - - // We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn - // causes the retry logic to break down. - mavlink_message_t msg; - mavlink_msg_command_long_pack_chan(mavlink->getSystemId(), - mavlink->getComponentId(), - _vehicle->priorityLink()->mavlinkChannel(), - &msg, - uasId, - _vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - 0, // 0=first transmission of command - gyroCal, // gyro cal - magCal, // mag cal - pressureCal, // ground pressure - radioCal, // radio cal - accelCal, // accel cal - airspeedCal, // PX4: airspeed cal, ArduPilot: compass mot - escCal); // esc cal - _vehicle->sendMessageOnLinkThreadSafe(_vehicle->priorityLink(), msg); -} - -void UAS::stopCalibration(void) -{ - if (!_vehicle) { - return; - } - - _vehicle->sendMavCommand(_vehicle->defaultComponentId(), // target component - MAV_CMD_PREFLIGHT_CALIBRATION, // command id - true, // showError - 0, // gyro cal - 0, // mag cal - 0, // ground pressure - 0, // radio cal - 0, // accel cal - 0, // airspeed cal - 0); // unused -} - void UAS::startBusConfig(UASInterface::StartBusConfigType calType) { if (!_vehicle) { diff --git a/src/uas/UAS.h b/src/uas/UAS.h index b6ff6a5..7493594 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -180,9 +180,6 @@ public slots: /** @brief Receive a message from one of the communication links. */ virtual void receiveMessage(mavlink_message_t message); - void startCalibration(StartCalibrationType calType); - void stopCalibration(void); - void startBusConfig(StartBusConfigType calType); void stopBusConfig(void); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 0d31064..a7d9e48 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -39,31 +39,11 @@ public: /** @brief The time interval the robot is switched on **/ virtual quint64 getUptime() const = 0; - enum StartCalibrationType { - StartCalibrationRadio, - StartCalibrationGyro, - StartCalibrationMag, - StartCalibrationAirspeed, - StartCalibrationAccel, - StartCalibrationLevel, - StartCalibrationPressure, - StartCalibrationEsc, - StartCalibrationCopyTrims, - StartCalibrationUavcanEsc, - StartCalibrationCompassMot, - }; - enum StartBusConfigType { StartBusConfigActuators, EndBusConfigActuators, }; - /// Starts the specified calibration - virtual void startCalibration(StartCalibrationType calType) = 0; - - /// Ends any current calibration - virtual void stopCalibration(void) = 0; - /// Starts the specified bus configuration virtual void startBusConfig(StartBusConfigType calType) = 0;