Merge pull request #1503 from DonLakeFlyer/SensorCal

Sensor cal rework

qgroundcontrol.qrc

@@ -64,6 +64,10 @@
         <file alias="VehicleNoseDown.png">src/AutoPilotPlugins/PX4/Images/VehicleNoseDown.png</file>
         <file alias="VehicleTailDown.png">src/AutoPilotPlugins/PX4/Images/VehicleTailDown.png</file>
 
+        <file alias="VehicleDownRotate.png">src/AutoPilotPlugins/PX4/Images/VehicleDownRotate.png</file>
+        <file alias="VehicleLeftRotate.png">src/AutoPilotPlugins/PX4/Images/VehicleLeftRotate.png</file>
+        <file alias="VehicleNoseDownRotate.png">src/AutoPilotPlugins/PX4/Images/VehicleNoseDownRotate.png</file>
+
         <!-- Airframe component resourecs -->
         <file alias="AirframeComponent.qml">src/AutoPilotPlugins/PX4/AirframeComponent.qml</file>
         <file alias="AirframeStandardPlane.png">src/AutoPilotPlugins/PX4/Images/AirframeStandardPlane.png</file>

src/AutoPilotPlugins/PX4/FlightModesComponentController.cc

@@ -153,11 +153,11 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start)
             _rgRCReversed[i] = floatReversed == -1.0f;
         }
         
-        _uas->startRadioControlCalibration();
+        _uas->startCalibration(UASInterface::StartCalibrationRadio);
         connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
     } else {
         disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
-        _uas->endRadioControlCalibration();
+        _uas->stopCalibration();
         _initRcValues();
         emit switchLiveRangeChanged();
     }

src/AutoPilotPlugins/PX4/SensorsComponentController.h

@@ -50,18 +50,11 @@ public:
     Q_PROPERTY(QQuickItem* gyroButton MEMBER _gyroButton)
     Q_PROPERTY(QQuickItem* accelButton MEMBER _accelButton)
     Q_PROPERTY(QQuickItem* airspeedButton MEMBER _airspeedButton)
+    Q_PROPERTY(QQuickItem* cancelButton MEMBER _cancelButton)
+    Q_PROPERTY(QQuickItem* orientationCalAreaHelpText MEMBER _orientationCalAreaHelpText)
     
-    Q_PROPERTY(bool showCompass0 MEMBER _showCompass0 CONSTANT)
-    Q_PROPERTY(bool showCompass1 MEMBER _showCompass1 CONSTANT)
-    Q_PROPERTY(bool showCompass2 MEMBER _showCompass2 CONSTANT)
-    
-    Q_PROPERTY(bool showGyroCalArea MEMBER _showGyroCalArea NOTIFY showGyroCalAreaChanged)
     Q_PROPERTY(bool showOrientationCalArea MEMBER _showOrientationCalArea NOTIFY showOrientationCalAreaChanged)
     
-    Q_PROPERTY(bool gyroCalInProgress MEMBER _gyroCalInProgress NOTIFY gyroCalInProgressChanged)
-    
-    Q_PROPERTY(QString calInProgressText MEMBER _calInProgressText NOTIFY calInProgressTextChanged)
-    
     Q_PROPERTY(bool orientationCalDownSideDone MEMBER _orientationCalDownSideDone NOTIFY orientationCalSidesDoneChanged)
     Q_PROPERTY(bool orientationCalUpsideDownSideDone MEMBER _orientationCalUpsideDownSideDone NOTIFY orientationCalSidesDoneChanged)
     Q_PROPERTY(bool orientationCalLeftSideDone MEMBER _orientationCalLeftSideDone NOTIFY orientationCalSidesDoneChanged)
@@ -69,6 +62,13 @@ public:
     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 orientationCalDownSideInProgress MEMBER _orientationCalDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
     Q_PROPERTY(bool orientationCalUpsideDownSideInProgress MEMBER _orientationCalUpsideDownSideInProgress NOTIFY orientationCalSidesInProgressChanged)
     Q_PROPERTY(bool orientationCalLeftSideInProgress MEMBER _orientationCalLeftSideInProgress NOTIFY orientationCalSidesInProgressChanged)
@@ -76,37 +76,48 @@ public:
     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 orientationCalLeftSideRotate MEMBER _orientationCalLeftSideRotate NOTIFY orientationCalSidesRotateChanged)
+    Q_PROPERTY(bool orientationCalNoseDownSideRotate MEMBER _orientationCalNoseDownSideRotate NOTIFY orientationCalSidesRotateChanged)
+    
+    Q_PROPERTY(bool waitingForCancel MEMBER _waitingForCancel NOTIFY waitingForCancelChanged)
+    
     Q_INVOKABLE void calibrateCompass(void);
     Q_INVOKABLE void calibrateGyro(void);
     Q_INVOKABLE void calibrateAccel(void);
     Q_INVOKABLE void calibrateAirspeed(void);
+    Q_INVOKABLE void cancelCalibration(void);
     
     bool fixedWing(void);
     
 signals:
     void showGyroCalAreaChanged(void);
     void showOrientationCalAreaChanged(void);
-    void gyroCalInProgressChanged(void);
     void orientationCalSidesDoneChanged(void);
+    void orientationCalSidesVisibleChanged(void);
     void orientationCalSidesInProgressChanged(void);
-    void calInProgressTextChanged(const QString& newText);
+    void orientationCalSidesRotateChanged(void);
+    void resetStatusTextArea(void);
+    void waitingForCancelChanged(void);
+    void setCompassRotations(void);
     
 private slots:
     void _handleUASTextMessage(int uasId, int compId, int severity, QString text);
     
 private:
-    void _startCalibration(void);
-    void _stopCalibration(bool failed);
-    void _beginTextLogging(void);
+    void _startLogCalibration(void);
+    void _startVisualCalibration(void);
     void _appendStatusLog(const QString& text);
     void _refreshParams(void);
     void _hideAllCalAreas(void);
     
-    void _updateAndEmitGyroCalInProgress(bool inProgress);
-    
-    void _updateAndEmitCalInProgressText(const QString& text);
+    enum StopCalibrationCode {
+        StopCalibrationSuccess,
+        StopCalibrationFailed,
+        StopCalibrationCancelled
+    };
+    void _stopCalibration(StopCalibrationCode code);
     
-    void _updateAndEmitShowGyroCalArea(bool show);
     void _updateAndEmitShowOrientationCalArea(bool show);
 
     QQuickItem* _statusLog;
@@ -115,6 +126,8 @@ private:
     QQuickItem* _gyroButton;
     QQuickItem* _accelButton;
     QQuickItem* _airspeedButton;
+    QQuickItem* _cancelButton;
+    QQuickItem* _orientationCalAreaHelpText;
     
     bool _showGyroCalArea;
     bool _showOrientationCalArea;
@@ -127,8 +140,6 @@ private:
     bool _magCalInProgress;
     bool _accelCalInProgress;
     
-    QString _calInProgressText;
-    
     bool _orientationCalDownSideDone;
     bool _orientationCalUpsideDownSideDone;
     bool _orientationCalLeftSideDone;
@@ -136,6 +147,13 @@ private:
     bool _orientationCalNoseDownSideDone;
     bool _orientationCalTailDownSideDone;
     
+    bool _orientationCalDownSideVisible;
+    bool _orientationCalUpsideDownSideVisible;
+    bool _orientationCalLeftSideVisible;
+    bool _orientationCalRightSideVisible;
+    bool _orientationCalNoseDownSideVisible;
+    bool _orientationCalTailDownSideVisible;
+    
     bool _orientationCalDownSideInProgress;
     bool _orientationCalUpsideDownSideInProgress;
     bool _orientationCalLeftSideInProgress;
@@ -143,9 +161,15 @@ private:
     bool _orientationCalNoseDownSideInProgress;
     bool _orientationCalTailDownSideInProgress;
     
-    bool _textLoggingStarted;
+    bool _orientationCalDownSideRotate;
+    bool _orientationCalLeftSideRotate;
+    bool _orientationCalNoseDownSideRotate;
+    
+    bool _unknownFirmwareVersion;
+    bool _waitingForCancel;
     
     AutoPilotPlugin*    _autopilot;
+    UASInterface*       _uas;
 };
 
 #endif

src/qgcunittest/MockUAS.h

@@ -105,6 +105,8 @@ public:
     virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; };
     virtual int getAutopilotType() { return MAV_AUTOPILOT_PX4; };
     virtual QGCUASFileManager* getFileManager() {Q_ASSERT(false); return NULL; }
+    virtual void startCalibration(StartCalibrationType calType) { Q_UNUSED(calType); return; };
+    virtual void stopCalibration() { return; };
 
     /** @brief Send a message over this link (to this or to all UAS on this link) */
     virtual void sendMessage(LinkInterface* link, mavlink_message_t message){ Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); }
@@ -152,11 +154,6 @@ public slots:
     virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw)
         { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); };
     virtual void setLocalPositionOffset(float x, float y, float z, float yaw) { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); };
-    virtual void startRadioControlCalibration(int param) { Q_UNUSED(param); return; };
-    virtual void endRadioControlCalibration() { return; };
-    virtual void startMagnetometerCalibration() { Q_ASSERT(false); };
-    virtual void startGyroscopeCalibration() { Q_ASSERT(false); };
-    virtual void startPressureCalibration() { Q_ASSERT(false); };
     virtual void setBatterySpecs(const QString& specs) { Q_UNUSED(specs); Q_ASSERT(false); };
     virtual QString getBatterySpecs() { Q_ASSERT(false); return _bogusString; };
     virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)

src/uas/UAS.cc

@@ -1439,19 +1439,67 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
     Q_UNUSED(yaw);
 }
 
-void UAS::startRadioControlCalibration(int param)
+void UAS::startCalibration(UASInterface::StartCalibrationType calType)
 {
+    int gyroCal = 0;
+    int magCal = 0;
+    int airspeedCal = 0;
+    int radioCal = 0;
+    int accelCal = 0;
+    
+    switch (calType) {
+        case StartCalibrationGyro:
+            gyroCal = 1;
+            break;
+        case StartCalibrationMag:
+            magCal = 1;
+            break;
+        case StartCalibrationAirspeed:
+            airspeedCal = 1;
+            break;
+        case StartCalibrationRadio:
+            radioCal = 1;
+            break;
+        case StartCalibrationAccel:
+            accelCal = 1;
+            break;
+    }
+    
     mavlink_message_t msg;
-    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(),
+                                  mavlink->getComponentId(),
+                                  &msg,
+                                  uasId,
+                                  0,                                // target component
+                                  MAV_CMD_PREFLIGHT_CALIBRATION,    // command id
+                                  0,                                // 0=first transmission of command
+                                  gyroCal,                          // gyro cal
+                                  magCal,                           // mag cal
+                                  0,                                // ground pressure
+                                  radioCal,                         // radio cal
+                                  accelCal,                         // accel cal
+                                  airspeedCal,                      // airspeed cal
+                                  0);                               // unused
     sendMessage(msg);
 }
 
-void UAS::endRadioControlCalibration()
+void UAS::stopCalibration(void)
 {
     mavlink_message_t msg;
-    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0, 0);
+    mavlink_msg_command_long_pack(mavlink->getSystemId(),
+                                  mavlink->getComponentId(),
+                                  &msg,
+                                  uasId,
+                                  0,                                // target component
+                                  MAV_CMD_PREFLIGHT_CALIBRATION,    // command id
+                                  0,                                // 0=first transmission of command
+                                  0,                                // gyro cal
+                                  0,                                // mag cal
+                                  0,                                // ground pressure
+                                  0,                                // radio cal
+                                  0,                                // accel cal
+                                  0,                                // airspeed cal
+                                  0);                               // unused
     sendMessage(msg);
 }
 
@@ -1469,30 +1517,6 @@ void UAS::stopDataRecording()
     sendMessage(msg);
 }
 
-void UAS::startMagnetometerCalibration()
-{
-    mavlink_message_t msg;
-    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0, 0, 0, 0);
-    sendMessage(msg);
-}
-
-void UAS::startGyroscopeCalibration()
-{
-    mavlink_message_t msg;
-    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0, 0, 0, 0);
-    sendMessage(msg);
-}
-
-void UAS::startPressureCalibration()
-{
-    mavlink_message_t msg;
-    // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
-    mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0, 0, 0, 0);
-    sendMessage(msg);
-}
-
 /**
 * Check if time is smaller than 40 years, assuming no system without Unix
 * timestamp runs longer than 40 years continuously without reboot. In worst case

src/uas/UAS.h

@@ -863,11 +863,8 @@ public slots:
     /** @brief Add an offset in body frame to the setpoint */
     void setLocalPositionOffset(float x, float y, float z, float yaw);
 
-    void startRadioControlCalibration(int param=1);
-    void endRadioControlCalibration();
-    void startMagnetometerCalibration();
-    void startGyroscopeCalibration();
-    void startPressureCalibration();
+    void startCalibration(StartCalibrationType calType);
+    void stopCalibration(void);
 
     void startDataRecording();
     void stopDataRecording();

src/uas/UASInterface.h

@@ -243,6 +243,20 @@ public:
     static const unsigned int WAYPOINT_RADIUS_DEFAULT_FIXED_WING = 25;
     static const unsigned int WAYPOINT_RADIUS_DEFAULT_ROTARY_WING = 5;
     
+    enum StartCalibrationType {
+        StartCalibrationRadio,
+        StartCalibrationGyro,
+        StartCalibrationMag,
+        StartCalibrationAirspeed,
+        StartCalibrationAccel
+    };
+    
+    /// Starts the specified calibration
+    virtual void startCalibration(StartCalibrationType calType) = 0;
+    
+    /// Ends any current calibration
+    virtual void stopCalibration(void) = 0;
+
 public slots:
 
     /** @brief Set a new name for the system */
@@ -344,12 +358,6 @@ public slots:
     virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0;
     virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0;
 
-    virtual void startRadioControlCalibration(int param=1) = 0;
-    virtual void endRadioControlCalibration() = 0;
-    virtual void startMagnetometerCalibration() = 0;
-    virtual void startGyroscopeCalibration() = 0;
-    virtual void startPressureCalibration() = 0;
-
     /** @brief Return if this a rotary wing */
     virtual bool isRotaryWing() = 0;
     /** @brief Return if this is a fixed wing */

src/ui/px4_configuration/PX4RCCalibration.cc

@@ -892,7 +892,7 @@ void PX4RCCalibration::_writeCalibration(void)
 {
     if (!_mav) return;
     
-    _mav->endRadioControlCalibration();
+    _mav->stopCalibration();
     
     _validateCalibration();
     
@@ -986,7 +986,7 @@ void PX4RCCalibration::_startCalibration(void)
     _resetInternalCalibrationValues();
     
     // Let the mav known we are starting calibration. This should turn off motors and so forth.
-    _mav->startRadioControlCalibration();
+    _mav->startCalibration(UASInterface::StartCalibrationRadio);
     
     _ui->rcCalNext->setText(tr("Next"));
     _ui->rcCalCancel->setEnabled(true);
@@ -1001,7 +1001,7 @@ void PX4RCCalibration::_stopCalibration(void)
     _currentStep = -1;
     
     if (_mav) {
-        _mav->endRadioControlCalibration();
+        _mav->stopCalibration();
         _setInternalCalibrationValuesFromParameters();
     } else {
         _resetInternalCalibrationValues();