New start/stop calibration apis

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/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() { Q_ASSERT(false); 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

@@ -242,6 +242,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:
 
@@ -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();