Merge pull request #9010 from DonLakeFlyer/APMGyroCal

ArduPilot: Sensors - Add separate gyro cal support for ArduCopter based vehicles

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
@@ -621,12 +641,19 @@ 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 {

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

src/AutoPilotPlugins/PX4/PowerComponentController.cc

@@ -7,10 +7,6 @@
  *
  ****************************************************************************/
 
-
-/// @file
-///     @author Don Gagne <don@thegagnes.com>
-
 #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)

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

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<double>(highLatency.altitude_amsl)
+                highLatency.longitude  / (double)1E7,
+                static_cast<double>(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) {

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

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) {

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

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;