Merge pull request #1363 from DonLakeFlyer/SensorCal

Disable calibration buttons while calibrating

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" }

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)

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;
 };