From e0476c67e320e56a4e997472e8a2b63547272405 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 15 Mar 2015 15:45:58 -0700 Subject: [PATCH] Disable calibration buttons while calibrating Also changed to pull specific small parameters set after cal instead of full refresh. This way, the red/green indicators update much more quickly. --- src/AutoPilotPlugins/PX4/SensorsComponent.qml | 19 ++++- .../PX4/SensorsComponentController.cc | 85 ++++++++++++------- .../PX4/SensorsComponentController.h | 17 +++- 3 files changed, 85 insertions(+), 36 deletions(-) diff --git a/src/AutoPilotPlugins/PX4/SensorsComponent.qml b/src/AutoPilotPlugins/PX4/SensorsComponent.qml index 38bdb27be..a1eb3174a 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponent.qml +++ b/src/AutoPilotPlugins/PX4/SensorsComponent.qml @@ -79,6 +79,14 @@ Rectangle { onLoaded: { controller.statusLog = statusTextArea controller.progressBar = progressBar + controller.compassButton = compassButton + controller.gyroButton = gyroButton + controller.accelButton = accelButton + controller.airspeedButton = airspeedButton + controller.compass0RotationCombo = compass0RotationCombo + controller.compass1RotationCombo = compass1RotationCombo + controller.compass2RotationCombo = compass2RotationCombo + controller.boardRotationCombo = boardRotationCombo } } @@ -97,12 +105,12 @@ Rectangle { spacing: 20 - QGCLabel { text: "Calibrate:"; anchors.baseline: firstButton.baseline } + QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline } IndicatorButton { property Fact fact: Fact { name: "CAL_MAG0_ID" } - id: firstButton + id: compassButton width: parent.buttonWidth text: "Compass" indicatorGreen: fact.value != 0 @@ -112,6 +120,7 @@ Rectangle { IndicatorButton { property Fact fact: Fact { name: "CAL_GYRO0_ID" } + id: gyroButton width: parent.buttonWidth text: "Gyroscope" indicatorGreen: fact.value != 0 @@ -121,6 +130,7 @@ Rectangle { IndicatorButton { property Fact fact: Fact { name: "CAL_ACC0_ID" } + id: accelButton width: parent.buttonWidth text: "Accelerometer" indicatorGreen: fact.value != 0 @@ -130,6 +140,7 @@ Rectangle { IndicatorButton { property Fact fact: Fact { name: "SENS_DPRES_OFF" } + id: airspeedButton width: parent.buttonWidth text: "Airspeed" visible: controller.fixedWing @@ -280,6 +291,7 @@ Rectangle { QGCLabel { text: "Autpilot Orientation" } FactComboBox { + id: boardRotationCombo width: rotationColumnWidth; model: rotations fact: Fact { name: "SENS_BOARD_ROT" } @@ -295,6 +307,7 @@ Rectangle { id: compass0ComponentCombo FactComboBox { + id: compass0RotationCombo width: rotationColumnWidth model: rotations fact: Fact { name: "CAL_MAG0_ROT" } @@ -313,6 +326,7 @@ Rectangle { id: compass1ComponentCombo FactComboBox { + id: compass1RotationCombo width: rotationColumnWidth model: rotations fact: Fact { name: "CAL_MAG1_ROT" } @@ -331,6 +345,7 @@ Rectangle { id: compass2ComponentCombo FactComboBox { + id: compass1RotationCombo width: rotationColumnWidth model: rotations fact: Fact { name: "CAL_MAG2_ROT" } diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc index 6b44d110e..509e760ce 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.cc +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.cc @@ -51,6 +51,7 @@ SensorsComponentController::SensorsComponentController(AutoPilotPlugin* autopilo _accelCalRightSideInProgress(false), _accelCalNoseDownSideInProgress(false), _accelCalTailDownSideInProgress(false), + _textLoggingStarted(false), _autopilot(autopilot) { Q_ASSERT(autopilot); @@ -69,9 +70,37 @@ void SensorsComponentController::_appendStatusLog(const QString& text) Q_ARG(QVariant, varText)); } -void SensorsComponentController::calibrateGyro(void) +void SensorsComponentController::_startCalibration(void) { _beginTextLogging(); + _hideAllCalAreas(); + + _compassButton->setEnabled(false); + _gyroButton->setEnabled(false); + _accelButton->setEnabled(false); + _airspeedButton->setEnabled(false); +} + +void SensorsComponentController::_stopCalibration(bool failed) +{ + _compassButton->setEnabled(true); + _gyroButton->setEnabled(true); + _accelButton->setEnabled(true); + _airspeedButton->setEnabled(true); + + _progressBar->setProperty("value", failed ? 0 : 1); + _updateAndEmitGyroCalInProgress(false); + _refreshParams(); + + if (failed) { + QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed."); + _hideAllCalAreas(); + } +} + +void SensorsComponentController::calibrateGyro(void) +{ + _startCalibration(); UASInterface* uas = _autopilot->uas(); Q_ASSERT(uas); @@ -80,8 +109,7 @@ void SensorsComponentController::calibrateGyro(void) void SensorsComponentController::calibrateCompass(void) { - _hideAllCalAreas(); - _beginTextLogging(); + _startCalibration(); UASInterface* uas = _autopilot->uas(); Q_ASSERT(uas); @@ -90,7 +118,7 @@ void SensorsComponentController::calibrateCompass(void) void SensorsComponentController::calibrateAccel(void) { - _beginTextLogging(); + _startCalibration(); UASInterface* uas = _autopilot->uas(); Q_ASSERT(uas); @@ -99,8 +127,7 @@ void SensorsComponentController::calibrateAccel(void) void SensorsComponentController::calibrateAirspeed(void) { - _hideAllCalAreas(); - _beginTextLogging(); + _startCalibration(); UASInterface* uas = _autopilot->uas(); Q_ASSERT(uas); @@ -109,9 +136,12 @@ void SensorsComponentController::calibrateAirspeed(void) void SensorsComponentController::_beginTextLogging(void) { - UASInterface* uas = _autopilot->uas(); - Q_ASSERT(uas); - connect(uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage); + if (!_textLoggingStarted) { + _textLoggingStarted = true; + UASInterface* uas = _autopilot->uas(); + Q_ASSERT(uas); + connect(uas, &UASInterface::textMessageReceived, this, &SensorsComponentController::_handleUASTextMessage); + } } void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, int severity, QString text) @@ -129,7 +159,7 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in } QStringList ignorePrefixList; - ignorePrefixList << "[cmd]" << "[mavlink pm]" << "[ekf check]" << "[pm]" << "[inav]"; + ignorePrefixList << "[cmd]" << "[mavlink pm]" << "[ekf check]" << "[pm]" << "[inav]" << "IN AIR MODE" << "LANDED MODE"; foreach (QString ignorePrefix, ignorePrefixList) { if (text.startsWith(ignorePrefix)) { return; @@ -150,11 +180,9 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in _appendStatusLog(text); if (text == "gyro calibration: started") { - _hideAllCalAreas(); _updateAndEmitShowGyroCalArea(true); _updateAndEmitGyroCalInProgress(true); } else if (text == "accel calibration: started") { - _hideAllCalAreas(); _accelCalDownSideDone = false; _accelCalUpsideDownSideDone = false; _accelCalLeftSideDone = false; @@ -227,34 +255,27 @@ void SensorsComponentController::_handleUASTextMessage(int uasId, int compId, in _accelCalTailDownSideInProgress = false; emit accelCalSidesDoneChanged(); emit accelCalSidesInProgressChanged(); - _refreshParams(); + _stopCalibration(false /* success */); } else if (text == "gyro calibration: done") { - _progressBar->setProperty("value", 1); - _updateAndEmitGyroCalInProgress(false); - _refreshParams(); + _stopCalibration(false /* success */); } else if (text == "mag calibration: done" || text == "dpress calibration: done") { - _progressBar->setProperty("value", 1); - _refreshParams(); + _stopCalibration(false /* success */); } else if (text.endsWith(" calibration: failed")) { - QGCMessageBox::warning("Calibration", "Calibration failed. Calibration log will be displayed."); - _hideAllCalAreas(); - _progressBar->setProperty("value", 0); - _updateAndEmitGyroCalInProgress(false); - _refreshParams(); + _stopCalibration(true /* failed */); } } void SensorsComponentController::_refreshParams(void) { -#if 0 - // FIXME: Not sure if firmware issue yet - _autopilot->refreshParametersPrefix("CAL_"); - _autopilot->refreshParametersPrefix("SENS_"); -#else - // Sending too many parameter requests like above doesn't seem to work. So for now, - // ask for everything back - _autopilot->refreshAllParameters(); -#endif + _autopilot->refreshParameter("CAL_MAG0_ID"); + _autopilot->refreshParameter("CAL_GYRO0_ID"); + _autopilot->refreshParameter("CAL_ACC0_ID"); + _autopilot->refreshParameter("SENS_DPRES_OFF"); + + _autopilot->refreshParameter("CAL_MAG0_ROT"); + _autopilot->refreshParameter("CAL_MAG1_ROT"); + _autopilot->refreshParameter("CAL_MAG2_ROT"); + _autopilot->refreshParameter("SENS_BOARD_ROT"); } bool SensorsComponentController::fixedWing(void) diff --git a/src/AutoPilotPlugins/PX4/SensorsComponentController.h b/src/AutoPilotPlugins/PX4/SensorsComponentController.h index 44d096a33..560dd2f55 100644 --- a/src/AutoPilotPlugins/PX4/SensorsComponentController.h +++ b/src/AutoPilotPlugins/PX4/SensorsComponentController.h @@ -46,6 +46,11 @@ public: Q_PROPERTY(QQuickItem* statusLog MEMBER _statusLog) Q_PROPERTY(QQuickItem* progressBar MEMBER _progressBar) + Q_PROPERTY(QQuickItem* compassButton MEMBER _compassButton) + Q_PROPERTY(QQuickItem* gyroButton MEMBER _gyroButton) + Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton) + Q_PROPERTY(QQuickItem* airspeedButton MEMBER _airspeedButton) + Q_PROPERTY(bool showGyroCalArea MEMBER _showGyroCalArea NOTIFY showGyroCalAreaChanged) Q_PROPERTY(bool showAccelCalArea MEMBER _showAccelCalArea NOTIFY showAccelCalAreaChanged) @@ -83,6 +88,8 @@ private slots: void _handleUASTextMessage(int uasId, int compId, int severity, QString text); private: + void _startCalibration(void); + void _stopCalibration(bool failed); void _beginTextLogging(void); void _appendStatusLog(const QString& text); void _refreshParams(void); @@ -93,8 +100,12 @@ private: void _updateAndEmitShowGyroCalArea(bool show); void _updateAndEmitShowAccelCalArea(bool show); - QQuickItem* _statusLog; - QQuickItem* _progressBar; + QQuickItem* _statusLog; + QQuickItem* _progressBar; + QQuickItem* _compassButton; + QQuickItem* _gyroButton; + QQuickItem* _accelButton; + QQuickItem* _airspeedButton; bool _showGyroCalArea; bool _showAccelCalArea; @@ -115,6 +126,8 @@ private: bool _accelCalNoseDownSideInProgress; bool _accelCalTailDownSideInProgress; + bool _textLoggingStarted; + AutoPilotPlugin* _autopilot; }; -- 2.22.0