APM Stack Compass Calibration

qgroundcontrol.pro

@@ -560,6 +560,7 @@ HEADERS+= \
     src/AutoPilotPlugins/APM/APMAirframeComponent.h \
     src/AutoPilotPlugins/APM/APMAirframeComponentController.h \
     src/AutoPilotPlugins/APM/APMAirframeComponentAirframes.h \
+    src/AutoPilotPlugins/APM/APMCompassCal.h \
     src/AutoPilotPlugins/APM/APMComponent.h \
     src/AutoPilotPlugins/APM/APMFlightModesComponent.h \
     src/AutoPilotPlugins/APM/APMFlightModesComponentController.h \
@@ -613,6 +614,7 @@ SOURCES += \
     src/AutoPilotPlugins/APM/APMAutoPilotPlugin.cc \
     src/AutoPilotPlugins/APM/APMAirframeComponent.cc \
     src/AutoPilotPlugins/APM/APMAirframeComponentController.cc \
+    src/AutoPilotPlugins/APM/APMCompassCal.cc \
     src/AutoPilotPlugins/APM/APMComponent.cc \
     src/AutoPilotPlugins/APM/APMFlightModesComponent.cc \
     src/AutoPilotPlugins/APM/APMFlightModesComponentController.cc \

src/AutoPilotPlugins/APM/APMCompassCal.h

+/*=====================================================================
+ 
+ QGroundControl Open Source Ground Control Station
+ 
+ (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
+ 
+ This file is part of the QGROUNDCONTROL project
+ 
+ QGROUNDCONTROL is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+ 
+ QGROUNDCONTROL is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ GNU General Public License for more details.
+ 
+ You should have received a copy of the GNU General Public License
+ along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.
+ 
+ ======================================================================*/
+
+#ifndef APMCompassCal_H
+#define APMCompassCal_H
+
+#include <QObject>
+#include <QThread>
+#include <QVector3D>
+
+#include "QGCLoggingCategory.h"
+#include "QGCMAVLink.h"
+#include "Vehicle.h"
+
+Q_DECLARE_LOGGING_CATEGORY(APMCompassCalLog)
+
+class CalWorkerThread : public QThread
+{
+    Q_OBJECT
+
+public:
+    CalWorkerThread(Vehicle* vehicle, QObject* parent = NULL);
+
+    // Cancel currently in progress calibration
+    void cancel(void) { _cancel = true; }
+
+    // Overrides from QThread
+    void run(void) Q_DECL_FINAL;
+
+    static const unsigned max_mags = 3;
+
+    bool                    rgCompassAvailable[max_mags];
+    QMutex                  lastScaledImuMutex;
+    mavlink_raw_imu_t       lastRawImu;
+    mavlink_scaled_imu_t    rgLastScaledImu[max_mags];
+
+signals:
+    void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
+
+private:
+    void _emitVehicleTextMessage(const QString& message);
+
+    // The routines below are based on the PX4 flight stack compass cal routines. Hence
+    // the PX4 Flight Stack coding style to maintain some level of code movement.
+
+    static const float mag_sphere_radius;
+    static const unsigned int calibration_sides;			///< The total number of sides
+    static const unsigned int calibration_total_points;     ///< The total points per magnetometer
+    static const unsigned int calibraton_duration_seconds;  ///< The total duration the routine is allowed to take
+
+    // The order of these cannot change since the calibration calculations depend on them in this order
+    enum detect_orientation_return {
+        DETECT_ORIENTATION_TAIL_DOWN,
+        DETECT_ORIENTATION_NOSE_DOWN,
+        DETECT_ORIENTATION_LEFT,
+        DETECT_ORIENTATION_RIGHT,
+        DETECT_ORIENTATION_UPSIDE_DOWN,
+        DETECT_ORIENTATION_RIGHTSIDE_UP,
+        DETECT_ORIENTATION_ERROR
+    };
+    static const unsigned detect_orientation_side_count = 6;
+
+    // Data passed to calibration worker routine
+    typedef struct  {
+        unsigned        done_count;
+        unsigned int	calibration_points_perside;
+        unsigned int	calibration_interval_perside_seconds;
+        uint64_t        calibration_interval_perside_useconds;
+        unsigned int	calibration_counter_total[max_mags];
+        bool            side_data_collected[detect_orientation_side_count];
+        float*          x[max_mags];
+        float*          y[max_mags];
+        float*          z[max_mags];
+    } mag_worker_data_t;
+
+    enum calibrate_return {
+        calibrate_return_ok,
+        calibrate_return_error,
+        calibrate_return_cancelled
+    };
+
+    /**
+     * Least-squares fit of a sphere to a set of points.
+     *
+     * Fits a sphere to a set of points on the sphere surface.
+     *
+     * @param x point coordinates on the X axis
+     * @param y point coordinates on the Y axis
+     * @param z point coordinates on the Z axis
+     * @param size number of points
+     * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
+     * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
+     * @param sphere_x coordinate of the sphere center on the X axis
+     * @param sphere_y coordinate of the sphere center on the Y axis
+     * @param sphere_z coordinate of the sphere center on the Z axis
+     * @param sphere_radius sphere radius
+     *
+     * @return 0 on success, 1 on failure
+     */
+    int sphere_fit_least_squares(const float x[], const float y[], const float z[],
+                     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z,
+                     float *sphere_radius);
+
+    /// Wait for vehicle to become still and detect it's orientation
+    ///	@return Returns detect_orientation_return according to orientation of still vehicle
+    enum detect_orientation_return detect_orientation(void);
+
+    /// Returns the human readable string representation of the orientation
+    ///	@param orientation Orientation to return string for, "error" if buffer is too small
+    const char* detect_orientation_str(enum detect_orientation_return orientation);
+
+    /// Perform calibration sequence which require a rest orientation detection prior to calibration.
+    ///	@return OK: Calibration succeeded, ERROR: Calibration failed
+    calibrate_return calibrate_from_orientation(
+                            bool	side_data_collected[detect_orientation_side_count],	///< Sides for which data still needs calibration
+                            void*	worker_data);                                       ///< Opaque data passed to worker routine
+
+    bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count);
+
+    calibrate_return calibrate(void);
+    calibrate_return mag_calibration_worker(detect_orientation_return orientation, void* data);
+    unsigned progress_percentage(mag_worker_data_t* worker_data);
+
+    Vehicle*    _vehicle;
+    bool        _cancel;
+};
+
+// Used to calibrate APM Stack compass by simulating PX4 Flight Stack firmware compass cal
+// on the ground station side of things.
+class APMCompassCal : public QObject
+{
+    Q_OBJECT
+    
+public:
+    APMCompassCal(void);
+    ~APMCompassCal();
+
+    void setVehicle(Vehicle* vehicle);
+    void startCalibration(void);
+    void cancelCalibration(void);
+    
+signals:
+    void vehicleTextMessage(int vehicleId, int compId, int severity, QString text);
+
+private slots:
+    void _handleMavlinkRawImu(mavlink_message_t message);
+    void _handleMavlinkScaledImu2(mavlink_message_t message);
+    void _handleMavlinkScaledImu3(mavlink_message_t message);
+
+private:
+    void _setSensorTransmissionSpeed(bool fast);
+    void _stopCalibration(void);
+    void _emitVehicleTextMessage(const QString& message);
+
+    Vehicle*            _vehicle;
+    CalWorkerThread*    _calWorkerThread;
+};
+
+#endif
+

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

 /*=====================================================================
- 
+
  QGroundControl Open Source Ground Control Station
  
  (c) 2009, 2015 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>
@@ -40,7 +40,6 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _nextButton(NULL),
     _cancelButton(NULL),
     _showOrientationCalArea(false),
-    _gyroCalInProgress(false),
     _magCalInProgress(false),
     _accelCalInProgress(false),
     _orientationCalDownSideDone(false),
@@ -69,6 +68,9 @@ APMSensorsComponentController::APMSensorsComponentController(void) :
     _orientationCalTailDownSideRotate(false),
     _waitingForCancel(false)
 {
+    _compassCal.setVehicle(_vehicle);
+    connect(&_compassCal, &APMCompassCal::vehicleTextMessage, this, &APMSensorsComponentController::_handleUASTextMessage);
+
     APMAutoPilotPlugin * apmPlugin = qobject_cast<APMAutoPilotPlugin*>(_vehicle->autopilotPlugin());
 
     _sensorsComponent = apmPlugin->sensorsComponent();
@@ -96,7 +98,9 @@ void APMSensorsComponentController::_startLogCalibration(void)
     
     _compassButton->setEnabled(false);
     _accelButton->setEnabled(false);
-    _nextButton->setEnabled(true);
+    if (_accelCalInProgress) {
+        _nextButton->setEnabled(true);
+    }
     _cancelButton->setEnabled(false);
 }
 
@@ -145,7 +149,7 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _accelButton->setEnabled(true);
     _nextButton->setEnabled(false);
     _cancelButton->setEnabled(false);
-    
+
     if (code == StopCalibrationSuccess) {
         _resetInternalState();
         _progressBar->setProperty("value", 1);
@@ -159,40 +163,37 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     _refreshParams();
     
     switch (code) {
-        case StopCalibrationSuccess:
-            _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
-            emit resetStatusTextArea();
-            if (_magCalInProgress) {
-                emit setCompassRotations();
-            }
-            break;
-            
-        case StopCalibrationCancelled:
-            emit resetStatusTextArea();
-            _hideAllCalAreas();
-            break;
-            
-        default:
-            // Assume failed
-            _hideAllCalAreas();
-            qgcApp()->showMessage("Calibration failed. Calibration log will be displayed.");
-            break;
+    case StopCalibrationSuccess:
+        _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
+        emit resetStatusTextArea();
+        break;
+
+    case StopCalibrationCancelled:
+        emit resetStatusTextArea();
+        _hideAllCalAreas();
+        break;
+
+    default:
+        // Assume failed
+        _hideAllCalAreas();
+        qgcApp()->showMessage("Calibration failed. Calibration log will be displayed.");
+        break;
     }
     
     _magCalInProgress = false;
     _accelCalInProgress = false;
-    _gyroCalInProgress = false;
 }
 
 void APMSensorsComponentController::calibrateCompass(void)
 {
     _startLogCalibration();
-    _uas->startCalibration(UASInterface::StartCalibrationMag);
+    _compassCal.startCalibration();
 }
 
 void APMSensorsComponentController::calibrateAccel(void)
 {
     _startLogCalibration();
+    _accelCalInProgress = true;
     _uas->startCalibration(UASInterface::StartCalibrationAccel);
 }
 
@@ -211,9 +212,21 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
         return;
     }
 
+    if (text.contains("progress <")) {
+        QString percent = text.split("<").last().split(">").first();
+        bool ok;
+        int p = percent.toInt(&ok);
+        if (ok) {
+            Q_ASSERT(_progressBar);
+            _progressBar->setProperty("value", (float)(p / 100.0));
+        }
+        return;
+    }
+
     QString anyKey("and press any");
     if (text.contains(anyKey)) {
         text = text.left(text.indexOf(anyKey)) + "and click Next to continue.";
+        _nextButton->setEnabled(true);
     }
 
     _appendStatusLog(text);
@@ -229,7 +242,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
         return;
     }
 
-/*
     // All calibration messages start with [cal]
     QString calPrefix("[cal] ");
     if (!text.startsWith(calPrefix)) {
@@ -243,7 +255,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
         
         _startVisualCalibration();
         
-        text = parts[1];
         if (text == "accel" || text == "mag" || text == "gyro") {
             // Reset all progress indication
             _orientationCalDownSideDone = false;
@@ -285,9 +296,6 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
                 _orientationCalRightSideVisible = true;
                 _orientationCalTailDownSideVisible = true;
                 _orientationCalNoseDownSideVisible = true;
-            } else if (text == "gyro") {
-                _gyroCalInProgress = true;
-                _orientationCalDownSideVisible = true;
             } else {
                 Q_ASSERT(false);
             }
@@ -381,12 +389,21 @@ void APMSensorsComponentController::_handleUASTextMessage(int uasId, int compId,
         return;
     }
     
+    QString calCompletePrefix("calibration done:");
+    if (text.startsWith(calCompletePrefix)) {
+        _stopCalibration(StopCalibrationSuccess);
+        return;
+    }
+
     if (text.startsWith("calibration cancelled")) {
         _stopCalibration(_waitingForCancel ? StopCalibrationCancelled : StopCalibrationFailed);
         return;
     }
 
-    */
+    if (text.startsWith("calibration failed")) {
+        _stopCalibration(StopCalibrationFailed);
+        return;
+    }
 }
 
 void APMSensorsComponentController::_refreshParams(void)
@@ -407,17 +424,17 @@ void APMSensorsComponentController::_refreshParams(void)
 bool APMSensorsComponentController::fixedWing(void)
 {
     switch (_vehicle->vehicleType()) {
-        case MAV_TYPE_FIXED_WING:
-        case MAV_TYPE_VTOL_DUOROTOR:
-        case MAV_TYPE_VTOL_QUADROTOR:
-        case MAV_TYPE_VTOL_TILTROTOR:
-        case MAV_TYPE_VTOL_RESERVED2:
-        case MAV_TYPE_VTOL_RESERVED3:
-        case MAV_TYPE_VTOL_RESERVED4:
-        case MAV_TYPE_VTOL_RESERVED5:
-            return true;
-        default:
-            return false;
+    case MAV_TYPE_FIXED_WING:
+    case MAV_TYPE_VTOL_DUOROTOR:
+    case MAV_TYPE_VTOL_QUADROTOR:
+    case MAV_TYPE_VTOL_TILTROTOR:
+    case MAV_TYPE_VTOL_RESERVED2:
+    case MAV_TYPE_VTOL_RESERVED3:
+    case MAV_TYPE_VTOL_RESERVED4:
+    case MAV_TYPE_VTOL_RESERVED5:
+        return true;
+    default:
+        return false;
     }
 }
 
@@ -434,12 +451,17 @@ void APMSensorsComponentController::_hideAllCalAreas(void)
 
 void APMSensorsComponentController::cancelCalibration(void)
 {
-    // The firmware doesn't allow us to cancel calibration. The best we can do is wait
-    // for it to timeout.
     _waitingForCancel = true;
     emit waitingForCancelChanged();
     _cancelButton->setEnabled(false);
-    _uas->stopCalibration();
+
+    if (_magCalInProgress) {
+        _compassCal.cancelCalibration();
+    } else {
+        // The firmware doesn't always allow us to cancel calibration. The best we can do is wait
+        // for it to timeout.
+        _uas->stopCalibration();
+    }
 }
 
 void APMSensorsComponentController::nextClicked(void)

src/AutoPilotPlugins/APM/APMSensorsComponentController.h

@@ -30,6 +30,7 @@
 #include "FactPanelController.h"
 #include "QGCLoggingCategory.h"
 #include "APMSensorsComponent.h"
+#include "APMCompassCal.h"
 
 Q_DECLARE_LOGGING_CATEGORY(APMSensorsComponentControllerLog)
 
@@ -106,7 +107,6 @@ signals:
     void orientationCalSidesRotateChanged(void);
     void resetStatusTextArea(void);
     void waitingForCancelChanged(void);
-    void setCompassRotations(void);
     void setupNeededChanged(void);
     
 private slots:
@@ -129,6 +129,7 @@ private:
     
     void _updateAndEmitShowOrientationCalArea(bool show);
 
+    APMCompassCal           _compassCal;
     APMSensorsComponent*    _sensorsComponent;
 
     QQuickItem* _statusLog;
@@ -139,10 +140,8 @@ private:
     QQuickItem* _cancelButton;
     QQuickItem* _orientationCalAreaHelpText;
     
-    bool _showGyroCalArea;
     bool _showOrientationCalArea;
     
-    bool _gyroCalInProgress;
     bool _magCalInProgress;
     bool _accelCalInProgress;
     

src/Vehicle/Vehicle.cc

@@ -223,6 +223,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
     case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
         _handleRCChannelsRaw(message);
         break;
+    case MAVLINK_MSG_ID_RAW_IMU:
+        emit mavlinkRawImu(message);
+        break;
+    case MAVLINK_MSG_ID_SCALED_IMU:
+        emit mavlinkScaledImu1(message);
+        break;
+    case MAVLINK_MSG_ID_SCALED_IMU2:
+        emit mavlinkScaledImu2(message);
+        break;
+    case MAVLINK_MSG_ID_SCALED_IMU3:
+        emit mavlinkScaledImu3(message);
+        break;
     }
 
     emit mavlinkMessageReceived(message);
@@ -1035,7 +1047,7 @@ bool Vehicle::missingParameters(void)
     return _autopilotPlugin->missingParameters();
 }
 
-void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
+void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple)
 {
     mavlink_message_t               msg;
     mavlink_request_data_stream_t   dataStream;
@@ -1048,8 +1060,12 @@ void Vehicle::requestDataStream(MAV_DATA_STREAM stream, uint16_t rate)
 
     mavlink_msg_request_data_stream_encode(_mavlink->getSystemId(), _mavlink->getComponentId(), &msg, &dataStream);
 
-    // We use sendMessageMultiple since we really want these to make it to the vehicle
-    sendMessageMultiple(msg);
+    if (sendMultiple) {
+        // We use sendMessageMultiple since we really want these to make it to the vehicle
+        sendMessageMultiple(msg);
+    } else {
+        sendMessage(msg);
+    }
 }
 
 void Vehicle::_sendMessageMultipleNext(void)

src/Vehicle/Vehicle.h

@@ -204,7 +204,8 @@ public:
     /// Requests the specified data stream from the vehicle
     ///     @param stream Stream which is being requested
     ///     @param rate Rate at which to send stream in Hz
-    void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate);
+    ///     @param sendMultiple Send multiple time to guarantee Vehicle reception
+    void requestDataStream(MAV_DATA_STREAM stream, uint16_t rate, bool sendMultiple = true);
 
     bool missingParameters(void);
 
@@ -321,6 +322,11 @@ signals:
     /// Remote control RSSI changed  (0% - 100%)
     void remoteControlRSSIChanged(uint8_t rssi);
 
+    void mavlinkRawImu(mavlink_message_t message);
+    void mavlinkScaledImu1(mavlink_message_t message);
+    void mavlinkScaledImu2(mavlink_message_t message);
+    void mavlinkScaledImu3(mavlink_message_t message);
+
 private slots:
     void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message);
     void _linkInactiveOrDeleted(LinkInterface* link);