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.cc

+/*=====================================================================
+
+ 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/>.
+ 
+ ======================================================================*/
+
+#include "APMCompassCal.h"
+#include "AutoPilotPlugin.h"
+
+QGC_LOGGING_CATEGORY(APMCompassCalLog, "APMCompassCalLog")
+
+const float CalWorkerThread::mag_sphere_radius = 0.2f;
+const unsigned int CalWorkerThread::calibration_sides = 6;
+const unsigned int CalWorkerThread::calibration_total_points = 240;
+const unsigned int CalWorkerThread::calibraton_duration_seconds = CalWorkerThread::calibration_sides * 10;
+
+CalWorkerThread::CalWorkerThread(Vehicle* vehicle, QObject* parent)
+    : QThread(parent)
+    , _vehicle(vehicle)
+    , _cancel(false)
+{
+
+}
+
+void CalWorkerThread::run(void)
+{
+    if (calibrate() == calibrate_return_ok) {
+        _emitVehicleTextMessage(QStringLiteral("[cal] progress <100>"));
+        _emitVehicleTextMessage(QStringLiteral("[cal] calibration done: mag"));
+    }
+}
+
+void CalWorkerThread::_emitVehicleTextMessage(const QString& message)
+{
+    emit vehicleTextMessage(_vehicle->id(), 0, MAV_SEVERITY_INFO, message);
+    qDebug() << message;
+}
+
+unsigned CalWorkerThread::progress_percentage(mag_worker_data_t* worker_data)
+{
+    return 100 * ((float)worker_data->done_count) / calibration_sides;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::calibrate(void)
+{
+    calibrate_return result = calibrate_return_ok;
+
+    mag_worker_data_t worker_data;
+
+    worker_data.done_count = 0;
+    worker_data.calibration_points_perside = calibration_total_points / calibration_sides;
+    worker_data.calibration_interval_perside_seconds = calibraton_duration_seconds / calibration_sides;
+    worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
+
+    // Collect data for all sides
+    worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] =  false;
+    worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] =          false;
+    worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] =     false;
+    worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] =     false;
+    worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] =   false;
+    worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] =         false;
+
+    for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+        // Initialize to no memory allocated
+        worker_data.x[cur_mag] = NULL;
+        worker_data.y[cur_mag] = NULL;
+        worker_data.z[cur_mag] = NULL;
+        worker_data.calibration_counter_total[cur_mag] = 0;
+    }
+
+    const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
+
+    for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+        if (rgCompassAvailable[cur_mag]) {
+            worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+            worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+            worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+            if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
+                _emitVehicleTextMessage("[cal] ERROR: out of memory");
+                result = calibrate_return_error;
+            }
+        }
+    }
+
+    if (result == calibrate_return_ok) {
+        result = calibrate_from_orientation(
+                    worker_data.side_data_collected,    // Sides to calibrate
+                    &worker_data);                      // Opaque data for calibration worked
+    }
+
+    // Calculate calibration values for each mag
+
+    float sphere_x[max_mags];
+    float sphere_y[max_mags];
+    float sphere_z[max_mags];
+    float sphere_radius[max_mags];
+
+    // Sphere fit the data to get calibration values
+    if (result == calibrate_return_ok) {
+        for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+            if (rgCompassAvailable[cur_mag]) {
+                sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+                                         worker_data.calibration_counter_total[cur_mag],
+                                         100, 0.0f,
+                                         &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
+                                         &sphere_radius[cur_mag]);
+
+                if (qIsNaN(sphere_x[cur_mag]) || qIsNaN(sphere_y[cur_mag]) || qIsNaN(sphere_z[cur_mag])) {
+                    _emitVehicleTextMessage(QString("[cal] ERROR: NaN in sphere fit for mag %1").arg(cur_mag));
+                    result = calibrate_return_error;
+                }
+            }
+        }
+    }
+
+    // Data points are no longer needed
+    for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+        free(worker_data.x[cur_mag]);
+        free(worker_data.y[cur_mag]);
+        free(worker_data.z[cur_mag]);
+    }
+
+    static const char* rgCompassOffsetParam[3][3] = {
+        { "COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z" },
+        { "COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z" },
+        { "COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z" },
+    };
+
+    AutoPilotPlugin* plugin = _vehicle->autopilotPlugin();
+    if (result == calibrate_return_ok) {
+        for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
+            if (rgCompassAvailable[cur_mag]) {
+                _emitVehicleTextMessage(QString("[cal] mag #%1 off: x:%2 y:%3 z:%4").arg(cur_mag).arg(-sphere_x[cur_mag]).arg(-sphere_y[cur_mag]).arg(-sphere_z[cur_mag]));
+
+                const char* offsetParam = rgCompassOffsetParam[cur_mag][0];
+                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_x[cur_mag]);
+                offsetParam = rgCompassOffsetParam[cur_mag][1];
+                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_y[cur_mag]);
+                offsetParam = rgCompassOffsetParam[cur_mag][2];
+                plugin->getParameterFact(-1, offsetParam)->setRawValue(-sphere_z[cur_mag]);
+            }
+        }
+    }
+
+    return result;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::mag_calibration_worker(detect_orientation_return orientation, void* data)
+{
+    calibrate_return result = calibrate_return_ok;
+
+    unsigned int calibration_counter_side;
+
+    mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
+
+    _emitVehicleTextMessage("[cal] Rotate vehicle around the detected orientation");
+    _emitVehicleTextMessage(QString("[cal] Continue rotation for %1 seconds").arg(worker_data->calibration_interval_perside_seconds));
+
+    uint64_t calibration_deadline = QGC::groundTimeUsecs() + worker_data->calibration_interval_perside_useconds;
+
+    unsigned int loop_interval_usecs = (worker_data->calibration_interval_perside_seconds * 1000000) / worker_data->calibration_points_perside;
+
+    calibration_counter_side = 0;
+
+    while (QGC::groundTimeUsecs() < calibration_deadline && calibration_counter_side < worker_data->calibration_points_perside) {
+        if (_cancel) {
+            result = calibrate_return_cancelled;
+            break;
+        }
+
+        int prev_count[max_mags];
+        bool rejected = false;
+
+        for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
+            prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+
+            if (!rgCompassAvailable[cur_mag]) {
+                continue;
+            }
+
+            lastScaledImuMutex.lock();
+            mavlink_scaled_imu_t copyLastScaledImu = rgLastScaledImu[cur_mag];
+            lastScaledImuMutex.unlock();
+
+#if 0
+            // Check if this measurement is good to go in
+            rejected = rejected || reject_sample(copyLastScaledImu.xmag, copyLastScaledImu.ymag, copyLastScaledImu.zmag,
+                                                 worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
+                                                 worker_data->calibration_counter_total[cur_mag],
+                                                 calibration_sides * worker_data->calibration_points_perside);
+#endif
+
+            worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.xmag;
+            worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.ymag;
+            worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = copyLastScaledImu.zmag;
+            worker_data->calibration_counter_total[cur_mag]++;
+        }
+
+        // Keep calibration of all mags in lockstep
+        if (rejected) {
+            qCDebug(APMCompassCalLog) << QStringLiteral("Point rejected");
+
+            // Reset counts, since one of the mags rejected the measurement
+            for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+                worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
+            }
+        } else {
+            calibration_counter_side++;
+
+            // Progress indicator for side
+            _emitVehicleTextMessage(QString("[cal] %1 side calibration: progress <%2>").arg(detect_orientation_str(orientation)).arg(progress_percentage(worker_data) +
+                                                                                                                                     (unsigned)((100 / calibration_sides) * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))));
+        }
+
+        usleep(loop_interval_usecs);
+    }
+
+    if (result == calibrate_return_ok) {
+        _emitVehicleTextMessage(QString("[cal] %1 side done, rotate to a different side").arg(detect_orientation_str(orientation)));
+
+        worker_data->done_count++;
+        _emitVehicleTextMessage(QString("[cal] progress <%1>").arg(progress_percentage(worker_data)));
+    }
+
+    return result;
+}
+
+CalWorkerThread::calibrate_return CalWorkerThread::calibrate_from_orientation(
+        bool	side_data_collected[detect_orientation_side_count],
+        void*	worker_data)
+{
+    calibrate_return result = calibrate_return_ok;
+
+    unsigned orientation_failures = 0;
+
+    // Rotate through all requested orientations
+    while (true) {
+        if (_cancel) {
+            result = calibrate_return_cancelled;
+            break;
+        }
+
+        unsigned int side_complete_count = 0;
+
+        // Update the number of completed sides
+        for (unsigned i = 0; i < detect_orientation_side_count; i++) {
+            if (side_data_collected[i]) {
+                side_complete_count++;
+            }
+        }
+
+        if (side_complete_count == detect_orientation_side_count) {
+            // We have completed all sides, move on
+            break;
+        }
+
+        /* inform user which orientations are still needed */
+        char pendingStr[256];
+        pendingStr[0] = 0;
+
+        for (unsigned int cur_orientation=0; cur_orientation<detect_orientation_side_count; cur_orientation++) {
+            if (!side_data_collected[cur_orientation]) {
+                strcat(pendingStr, " ");
+                strcat(pendingStr, detect_orientation_str((enum detect_orientation_return)cur_orientation));
+            }
+        }
+        _emitVehicleTextMessage(QString("[cal] pending:%1").arg(pendingStr));
+
+        _emitVehicleTextMessage(QStringLiteral("[cal] hold vehicle still on a pending side"));
+        enum detect_orientation_return orient = detect_orientation();
+
+        if (orient == DETECT_ORIENTATION_ERROR) {
+            orientation_failures++;
+            _emitVehicleTextMessage(QStringLiteral("[cal] detected motion, hold still..."));
+            continue;
+        }
+
+        /* inform user about already handled side */
+        if (side_data_collected[orient]) {
+            orientation_failures++;
+            _emitVehicleTextMessage(QString("%1 side already completed").arg(detect_orientation_str(orient)));
+            _emitVehicleTextMessage(QStringLiteral("rotate to a pending side"));
+            continue;
+        }
+
+        _emitVehicleTextMessage(QString("[cal] %1 orientation detected").arg(detect_orientation_str(orient)));
+        orientation_failures = 0;
+
+        // Call worker routine
+        result = mag_calibration_worker(orient, worker_data);
+        if (result != calibrate_return_ok ) {
+            break;
+        }
+
+        _emitVehicleTextMessage(QString("[cal] %1 side done, rotate to a different side").arg(detect_orientation_str(orient)));
+
+        // Note that this side is complete
+        side_data_collected[orient] = true;
+        usleep(200000);
+    }
+
+    return result;
+}
+
+enum CalWorkerThread::detect_orientation_return CalWorkerThread::detect_orientation(void)
+{
+    bool    stillDetected = false;
+    quint64 stillDetectTime;
+
+    int16_t lastX = 0;
+    int16_t lastY = 0;
+    int16_t lastZ = 0;
+
+    while (true) {
+        lastScaledImuMutex.lock();
+        mavlink_raw_imu_t   copyLastRawImu = lastRawImu;
+        lastScaledImuMutex.unlock();
+
+        int16_t xDelta = abs(lastX - copyLastRawImu.xacc);
+        int16_t yDelta = abs(lastY - copyLastRawImu.yacc);
+        int16_t zDelta = abs(lastZ - copyLastRawImu.zacc);
+
+        lastX = copyLastRawImu.xacc;
+        lastY = copyLastRawImu.yacc;
+        lastZ = copyLastRawImu.zacc;
+
+        if (xDelta < 100 && yDelta < 100 && zDelta < 100) {
+            if (stillDetected) {
+                if (QGC::groundTimeMilliseconds() - stillDetectTime > 1000) {
+                    break;
+                }
+            } else {
+                stillDetectTime = QGC::groundTimeMilliseconds();
+                stillDetected = true;
+            }
+        } else {
+            stillDetected = false;
+        }
+
+        if (_cancel) {
+            break;
+        }
+
+        // FIXME: No timeout for never detect still
+
+        usleep(1000);
+    }
+
+    static const uint16_t rawImuOneG = 800;
+    static const uint16_t rawImuNoGThreshold = 200;
+
+    if (lastX > rawImuOneG && abs(lastY) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_TAIL_DOWN;        // [ g, 0, 0 ]
+    }
+
+    if (lastX < -rawImuOneG && abs(lastY) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_NOSE_DOWN;        // [ -g, 0, 0 ]
+    }
+
+    if (lastY > rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_LEFT;        // [ 0, g, 0 ]
+    }
+
+    if (lastY < -rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastZ) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_RIGHT;        // [ 0, -g, 0 ]
+    }
+
+    if (lastZ > rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastY) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_UPSIDE_DOWN;        // [ 0, 0, g ]
+    }
+
+    if (lastZ < -rawImuOneG && abs(lastX) < rawImuNoGThreshold && abs(lastY) < rawImuNoGThreshold) {
+        return DETECT_ORIENTATION_RIGHTSIDE_UP;        // [ 0, 0, -g ]
+    }
+
+    _emitVehicleTextMessage(QStringLiteral("[cal] ERROR: invalid orientation"));
+
+    return DETECT_ORIENTATION_ERROR;	// Can't detect orientation
+}
+
+const char* CalWorkerThread::detect_orientation_str(enum detect_orientation_return orientation)
+{
+    static const char* rgOrientationStrs[] = {
+        "back",		// tail down
+        "front",	// nose down
+        "left",
+        "right",
+        "up",		// upside-down
+        "down",		// right-side up
+        "error"
+    };
+
+    return rgOrientationStrs[orientation];
+}
+
+#if 0
+bool CalWorkerThread::reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
+{
+    float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
+
+    for (size_t i = 0; i < count; i++) {
+        float dx = sx - x[i];
+        float dy = sy - y[i];
+        float dz = sz - z[i];
+        float dist = sqrtf(dx * dx + dy * dy + dz * dz);
+
+        qDebug() << dist << min_sample_dist;
+
+        if (dist < min_sample_dist) {
+            return true;
+        }
+    }
+
+    return false;
+}
+#endif
+
+int CalWorkerThread::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)
+{
+
+    float x_sumplain = 0.0f;
+    float x_sumsq = 0.0f;
+    float x_sumcube = 0.0f;
+
+    float y_sumplain = 0.0f;
+    float y_sumsq = 0.0f;
+    float y_sumcube = 0.0f;
+
+    float z_sumplain = 0.0f;
+    float z_sumsq = 0.0f;
+    float z_sumcube = 0.0f;
+
+    float xy_sum = 0.0f;
+    float xz_sum = 0.0f;
+    float yz_sum = 0.0f;
+
+    float x2y_sum = 0.0f;
+    float x2z_sum = 0.0f;
+    float y2x_sum = 0.0f;
+    float y2z_sum = 0.0f;
+    float z2x_sum = 0.0f;
+    float z2y_sum = 0.0f;
+
+    for (unsigned int i = 0; i < size; i++) {
+
+        float x2 = x[i] * x[i];
+        float y2 = y[i] * y[i];
+        float z2 = z[i] * z[i];
+
+        x_sumplain += x[i];
+        x_sumsq += x2;
+        x_sumcube += x2 * x[i];
+
+        y_sumplain += y[i];
+        y_sumsq += y2;
+        y_sumcube += y2 * y[i];
+
+        z_sumplain += z[i];
+        z_sumsq += z2;
+        z_sumcube += z2 * z[i];
+
+        xy_sum += x[i] * y[i];
+        xz_sum += x[i] * z[i];
+        yz_sum += y[i] * z[i];
+
+        x2y_sum += x2 * y[i];
+        x2z_sum += x2 * z[i];
+
+        y2x_sum += y2 * x[i];
+        y2z_sum += y2 * z[i];
+
+        z2x_sum += z2 * x[i];
+        z2y_sum += z2 * y[i];
+    }
+
+    //
+    //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
+    //
+    //    P is a structure that has been computed with the data earlier.
+    //    P.npoints is the number of elements; the length of X,Y,Z are identical.
+    //    P's members are logically named.
+    //
+    //    X[n] is the x component of point n
+    //    Y[n] is the y component of point n
+    //    Z[n] is the z component of point n
+    //
+    //    A is the x coordiante of the sphere
+    //    B is the y coordiante of the sphere
+    //    C is the z coordiante of the sphere
+    //    Rsq is the radius squared of the sphere.
+    //
+    //This method should converge; maybe 5-100 iterations or more.
+    //
+    float x_sum = x_sumplain / size;        //sum( X[n] )
+    float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
+    float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
+    float y_sum = y_sumplain / size;        //sum( Y[n] )
+    float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
+    float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
+    float z_sum = z_sumplain / size;        //sum( Z[n] )
+    float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
+    float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )
+
+    float XY = xy_sum / size;        //sum( X[n] * Y[n] )
+    float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
+    float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
+    float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
+    float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
+    float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
+    float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
+    float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
+    float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
+
+    //Reduction of multiplications
+    float F0 = x_sum2 + y_sum2 + z_sum2;
+    float F1 =  0.5f * F0;
+    float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
+    float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
+    float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
+
+    //Set initial conditions:
+    float A = x_sum;
+    float B = y_sum;
+    float C = z_sum;
+
+    //First iteration computation:
+    float A2 = A * A;
+    float B2 = B * B;
+    float C2 = C * C;
+    float QS = A2 + B2 + C2;
+    float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
+
+    //Set initial conditions:
+    float Rsq = F0 + QB + QS;
+
+    //First iteration computation:
+    float Q0 = 0.5f * (QS - Rsq);
+    float Q1 = F1 + Q0;
+    float Q2 = 8.0f * (QS - Rsq + QB + F0);
+    float aA, aB, aC, nA, nB, nC, dA, dB, dC;
+
+    //Iterate N times, ignore stop condition.
+    unsigned int n = 0;
+
+#define FLT_EPSILON 1.1920929e-07F  /* 1E-5 */
+
+    while (n < max_iterations) {
+        n++;
+
+        //Compute denominator:
+        aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
+        aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
+        aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
+        aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+        aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+        aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
+
+        //Compute next iteration
+        nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
+        nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
+        nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
+
+        //Check for stop condition
+        dA = (nA - A);
+        dB = (nB - B);
+        dC = (nC - C);
+
+        if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
+
+        //Compute next iteration's values
+        A = nA;
+        B = nB;
+        C = nC;
+        A2 = A * A;
+        B2 = B * B;
+        C2 = C * C;
+        QS = A2 + B2 + C2;
+        QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
+        Rsq = F0 + QB + QS;
+        Q0 = 0.5f * (QS - Rsq);
+        Q1 = F1 + Q0;
+        Q2 = 8.0f * (QS - Rsq + QB + F0);
+    }
+
+    *sphere_x = A;
+    *sphere_y = B;
+    *sphere_z = C;
+    *sphere_radius = sqrtf(Rsq);
+
+    return 0;
+}
+
+APMCompassCal::APMCompassCal(void)
+    : _vehicle(NULL)
+    , _calWorkerThread(NULL)
+{
+
+}
+
+APMCompassCal::~APMCompassCal()
+{
+
+}
+
+void APMCompassCal::setVehicle(Vehicle* vehicle)
+{
+    if (!vehicle) {
+        qWarning() << "vehicle == NULL";
+    }
+
+    _vehicle = vehicle;
+}
+
+void APMCompassCal::startCalibration(void)
+{
+    _setSensorTransmissionSpeed(true /* fast */);
+    connect (_vehicle, &Vehicle::mavlinkRawImu,     this, &APMCompassCal::_handleMavlinkRawImu);
+    connect (_vehicle, &Vehicle::mavlinkScaledImu2, this, &APMCompassCal::_handleMavlinkScaledImu2);
+    connect (_vehicle, &Vehicle::mavlinkScaledImu3, this, &APMCompassCal::_handleMavlinkScaledImu3);
+
+    // Simulate a start message
+    _emitVehicleTextMessage("[cal] calibration started: mag");
+
+    _calWorkerThread = new CalWorkerThread(_vehicle, this);
+    connect(_calWorkerThread, &CalWorkerThread::vehicleTextMessage, this, &APMCompassCal::vehicleTextMessage);
+
+    static const char* rgCompassOffsetParam[3][4] = {
+        { "COMPASS_OFS_X", "COMPASS_OFS_Y", "COMPASS_OFS_Z", "COMPASS_DEV_ID" },
+        { "COMPASS_OFS2_X", "COMPASS_OFS2_Y", "COMPASS_OFS2_Z", "COMPASS_DEV_ID2" },
+        { "COMPASS_OFS3_X", "COMPASS_OFS3_Y", "COMPASS_OFS3_Z", "COMPASS_DEV_ID3" },
+    };
+
+    // Clear the offset parameters so we get raw data
+    AutoPilotPlugin* plugin = _vehicle->autopilotPlugin();
+    for (int i=0; i<3; i++) {
+        _calWorkerThread->rgCompassAvailable[i] = true;
+
+        const char* deviceIdParam = rgCompassOffsetParam[i][3];
+        if (plugin->parameterExists(-1, deviceIdParam)) {
+            if (plugin->getParameterFact(-1, deviceIdParam)->rawValue().toInt() > 0) {
+                for (int j=0; j<3; j++) {
+                    const char* offsetParam = rgCompassOffsetParam[i][j];
+                    if (plugin->parameterExists(-1, offsetParam)) {
+                        plugin->getParameterFact(-1, offsetParam)->setRawValue(0.0);
+                    } else {
+                        _calWorkerThread->rgCompassAvailable[i] = false;
+                    }
+                }
+            } else {
+                _calWorkerThread->rgCompassAvailable[i] = false;
+            }
+        } else {
+            _calWorkerThread->rgCompassAvailable[i] = false;
+        }
+
+        qCDebug(APMCompassCalLog) << QStringLiteral("Compass %1 available: %2").arg(i).arg(_calWorkerThread->rgCompassAvailable[i]);
+    }
+
+    _calWorkerThread->start();
+}
+
+void APMCompassCal::cancelCalibration(void)
+{
+    _stopCalibration();
+
+    // Simulate a cancelled message
+    _emitVehicleTextMessage("[cal] calibration cancelled");
+}
+
+void APMCompassCal::_handleMavlinkRawImu(mavlink_message_t message)
+{
+    _calWorkerThread->lastScaledImuMutex.lock();
+    mavlink_msg_raw_imu_decode(&message, &_calWorkerThread->lastRawImu);
+    _calWorkerThread->rgLastScaledImu[0].xacc = _calWorkerThread->lastRawImu.xacc;
+    _calWorkerThread->rgLastScaledImu[0].yacc = _calWorkerThread->lastRawImu.yacc;
+    _calWorkerThread->rgLastScaledImu[0].zacc = _calWorkerThread->lastRawImu.zacc;
+    _calWorkerThread->rgLastScaledImu[0].xgyro = _calWorkerThread->lastRawImu.xgyro;
+    _calWorkerThread->rgLastScaledImu[0].ygyro = _calWorkerThread->lastRawImu.ygyro;
+    _calWorkerThread->rgLastScaledImu[0].zgyro = _calWorkerThread->lastRawImu.zgyro;
+    _calWorkerThread->rgLastScaledImu[0].xmag = _calWorkerThread->lastRawImu.xmag;
+    _calWorkerThread->rgLastScaledImu[0].ymag = _calWorkerThread->lastRawImu.ymag;
+    _calWorkerThread->rgLastScaledImu[0].zmag = _calWorkerThread->lastRawImu.zmag;
+    _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_handleMavlinkScaledImu2(mavlink_message_t message)
+{
+    _calWorkerThread->lastScaledImuMutex.lock();
+    mavlink_msg_scaled_imu2_decode(&message, (mavlink_scaled_imu2_t*)&_calWorkerThread->rgLastScaledImu[1]);
+    qDebug() << "IMU2" << _calWorkerThread->rgLastScaledImu[1].xmag << _calWorkerThread->rgLastScaledImu[1].ymag << _calWorkerThread->rgLastScaledImu[1].zmag;
+    _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_handleMavlinkScaledImu3(mavlink_message_t message)
+{
+    _calWorkerThread->lastScaledImuMutex.lock();
+    mavlink_msg_scaled_imu3_decode(&message, (mavlink_scaled_imu3_t*)&_calWorkerThread->rgLastScaledImu[2]);
+    _calWorkerThread->lastScaledImuMutex.unlock();
+}
+
+void APMCompassCal::_setSensorTransmissionSpeed(bool fast)
+{
+    _vehicle->requestDataStream(MAV_DATA_STREAM_RAW_SENSORS, fast ? 10 : 2);
+}
+
+void APMCompassCal::_stopCalibration(void)
+{
+    _calWorkerThread->cancel();
+    disconnect (_vehicle, &Vehicle::mavlinkRawImu,      this, &APMCompassCal::_handleMavlinkRawImu);
+    disconnect (_vehicle, &Vehicle::mavlinkScaledImu2,  this, &APMCompassCal::_handleMavlinkScaledImu2);
+    disconnect (_vehicle, &Vehicle::mavlinkScaledImu3,  this, &APMCompassCal::_handleMavlinkScaledImu3);
+    _setSensorTransmissionSpeed(false /* fast */);
+}
+
+void APMCompassCal::_emitVehicleTextMessage(const QString& message)
+{
+    emit vehicleTextMessage(_vehicle->id(), 0, MAV_SEVERITY_INFO, message);
+}

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/APMSensorsComponent.qml

@@ -40,6 +40,7 @@ QGCView {
     // Help text which is shown both in the status text area prior to pressing a cal button and in the
     // pre-calibration dialog.
 
+    readonly property string orientationHelp:       "If the orientation is in the direction of flight, select None."
     readonly property string boardRotationText:     "If the autopilot is mounted in flight direction, leave the default value (None)"
     readonly property string compassRotationText:   "If the compass or GPS module is mounted in flight direction, leave the default value (None)"
 
@@ -61,64 +62,29 @@ QGCView {
     readonly property int mainTextH1PointSize: ScreenTools.mediumFontPixelSize // Seems to be unused
 
     readonly property int rotationColumnWidth: 250
-    readonly property var rotations: [
-        "None",
-        "Yaw 45",
-        "Yaw 90",
-        "Yaw 135",
-        "Yaw 180",
-        "Yaw 225",
-        "Yaw 270",
-        "Yaw 315",
-        "Roll 180",
-        "Roll 180. Yaw 45",
-        "Roll 180. Yaw 90",
-        "Roll 180. Yaw 135",
-        "Pitch 180",
-        "Roll 180. Yaw 225",
-        "Roll 180, Yaw 270",
-        "Roll 180, Yaw 315",
-        "Roll 90",
-        "Roll 90, Yaw 45",
-        "Roll 90, Yaw 90",
-        "Roll 90, Yaw 135",
-        "Roll 270",
-        "Roll 270, Yaw 45",
-        "Roll 270, Yaw 90",
-        "Roll 270, Yaw 136",
-        "Pitch 90",
-        "Pitch 270",
-        "Pitch 180, Yaw 90",
-        "Pitch 180, Yaw 270",
-        "Roll 90, Pitch 90",
-        "Roll 180, Pitch 90",
-        "Roll 270, Pitch 90",
-        "Roll 90, Pitch 180",
-        "Roll 270, Pitch 180",
-        "Roll 90, Pitch 270",
-        "Roll 180, Pitch 270",
-        "Roll 270, Pitch 270",
-        "Roll 90, Pitch 180, Yaw 90",
-        "Roll 90, Yaw 270",
-        "Yaw 293, Pitch 68, Roll 90",
-    ]
-
-    property Fact cal_mag0_id:      controller.getParameterFact(-1, "COMPASS_DEV_ID")
-    property Fact cal_mag1_id:      controller.getParameterFact(-1, "COMPASS_DEV_ID2")
-    property Fact cal_mag2_id:      controller.getParameterFact(-1, "COMPASS_DEV_ID3")
-    property Fact cal_mag0_rot:     controller.getParameterFact(-1, "COMPASS_ORIENT")
-    property Fact cal_mag1_rot:     controller.getParameterFact(-1, "COMPASS_ORIENT2")
-    property Fact cal_mag2_rot:     controller.getParameterFact(-1, "COMPASS_ORIENT3")
-
-    property Fact sens_board_rot:   controller.getParameterFact(-1, "AHRS_ORIENTATION")
+
+    property Fact compass1Id:       controller.getParameterFact(-1, "COMPASS_DEV_ID")
+    property Fact compass2Id:       controller.getParameterFact(-1, "COMPASS_DEV_ID2")
+    property Fact compass3Id:       controller.getParameterFact(-1, "COMPASS_DEV_ID3")
+    property Fact compass1External: controller.getParameterFact(-1, "COMPASS_EXTERNAL")
+    property Fact compass2External: controller.getParameterFact(-1, "COMPASS_EXTERN2")
+    property Fact compass3External: controller.getParameterFact(-1, "COMPASS_EXTERN3")
+    property Fact compass1Rot:      controller.getParameterFact(-1, "COMPASS_ORIENT")
+    property Fact compass2Rot:      controller.getParameterFact(-1, "COMPASS_ORIENT2")
+    property Fact compass3Rot:      controller.getParameterFact(-1, "COMPASS_ORIENT3")
+    property Fact compass1Use:      controller.getParameterFact(-1, "COMPASS_USE")
+    property Fact compass2Use:      controller.getParameterFact(-1, "COMPASS_USE2")
+    property Fact compass3Use:      controller.getParameterFact(-1, "COMPASS_USE3")
+
+    property Fact boardRot:     controller.getParameterFact(-1, "AHRS_ORIENTATION")
 
     property bool accelCalNeeded:   controller.accelSetupNeeded
     property bool compassCalNeeded: controller.compassSetupNeeded
 
     // Id > = signals compass available, rot < 0 signals internal compass
-    property bool showCompass0Rot: cal_mag0_id.value > 0 && cal_mag0_rot.value >= 0
-    property bool showCompass1Rot: cal_mag1_id.value > 0 && cal_mag1_rot.value >= 0
-    property bool showCompass2Rot: cal_mag2_id.value > 0 && cal_mag2_rot.value >= 0
+    property bool showCompass1Rot: compass1Id.value > 0 && compass1External.value != 0 && compass1Use.value != 0
+    property bool showCompass2Rot: compass2Id.value > 0 && compass2External.value != 0 && compass2Use.value != 0
+    property bool showCompass3Rot: compass3Id.value > 0 && compass3External.value != 0 && compass3Use.value != 0
 
     APMSensorsComponentController {
         id:                         controller
@@ -133,12 +99,6 @@ QGCView {
 
         onResetStatusTextArea: statusLog.text = statusTextAreaDefaultText
 
-        onSetCompassRotations: {
-            if (showCompass0Rot || showCompass1Rot || showCompass2Rot) {
-            showDialog(compassRotationDialogComponent, "Set Compass Rotation(s)", 50, StandardButton.Ok)
-            }
-        }
-
         onWaitingForCancelChanged: {
             if (controller.waitingForCancel) {
                 showMessage("Calibration Cancel", "Waiting for Vehicle to response to Cancel. This may take a few seconds.", 0)
@@ -157,152 +117,158 @@ QGCView {
             id: preCalibrationDialog
 
             function accept() {
-                if (preCalibrationDialogType == "gyro") {
-                    controller.calibrateGyro()
-                } else if (preCalibrationDialogType == "accel") {
+                if (preCalibrationDialogType == "accel") {
                     controller.calibrateAccel()
-                } else if (preCalibrationDialogType == "level") {
-                    controller.calibrateLevel()
                 } else if (preCalibrationDialogType == "compass") {
                     controller.calibrateCompass()
-                } else if (preCalibrationDialogType == "airspeed") {
-                    controller.calibrateAirspeed()
                 }
                 preCalibrationDialog.hideDialog()
             }
 
-            Column {
-                anchors.fill: parent
-                spacing:                5
-
             QGCLabel {
-                    width:      parent.width
+                id:             label
+                anchors.left:   parent.left
+                anchors.right:  parent.right
                 wrapMode:       Text.WordWrap
-                    text:       preCalibrationDialogHelp
+                text:           "Before calibrating make sure orientation settings are correct."
             }
 
-                QGCLabel {
-                    width:      parent.width
-                    wrapMode:   Text.WordWrap
-                    visible:    (preCalibrationDialogType != "airspeed") && (preCalibrationDialogType != "gyro")
-                    text:       boardRotationText
-                }
+            Loader {
+                id:                 rotationsLoader
+                anchors.topMargin:  ScreenTools.defaultFontPixelHeight
+                anchors.top:        label.bottom
+                anchors.left:       parent.left
+                anchors.right:      parent.right
+                sourceComponent:    rotationCombosComponent
 
-                FactComboBox {
-                    width:      rotationColumnWidth
-                    model:      rotations
-                    visible:    preCalibrationDialogType != "airspeed" && (preCalibrationDialogType != "gyro")
-                    fact:       sens_board_rot
-                }
+                property bool showCompassRotations: preCalibrationDialogType == "accel" ? false : true
             }
         }
     }
 
     Component {
-        id: compassRotationDialogComponent
-
-        QGCViewDialog {
-            id: compassRotationDialog
+        id: rotationCombosComponent
 
         Column {
-                anchors.fill: parent
-                spacing:      ScreenTools.defaultFontPixelHeight
+            QGCLabel {
+                font.pixelSize: sideBarH1PointSize
+                text:           "Set Orientations"
+            }
+
+            Item {
+                width:  1
+                height: ScreenTools.defaultFontPixelHeight
+            }
 
             QGCLabel {
                 width:      parent.width
                 wrapMode:   Text.WordWrap
-                    text:       compassRotationText
+                text:       orientationHelp
             }
 
-                // Compass 0 rotation
-                Component {
-                    id: compass0ComponentLabel
-
-                    QGCLabel {
-                        font.pixelSize: sideBarH1PointSize
-                        text: "External Compass Orientation"
+            Item {
+                width:  1
+                height: ScreenTools.defaultFontPixelHeight
             }
 
+            // Board rotation
+            QGCLabel {
+                text:           "Autopilot Orientation"
             }
-                Component {
-                    id: compass0ComponentCombo
 
             FactComboBox {
-                        id:     compass0RotationCombo
-                        width:  rotationColumnWidth
-                        model:  rotations
-                        fact:   cal_mag0_rot
+                id:         boardRotationCombo
+                width:      parent.width
+                fact:       boardRot
+                indexModel: false
             }
+
+            Item {
+                width:  1
+                height: ScreenTools.defaultFontPixelHeight
             }
-                Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel : null }
-                Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo : null }
-                // Compass 1 rotation
-                Component {
-                    id: compass1ComponentLabel
 
-                    QGCLabel { text: "Compass 1 Orientation" }
+            // Compass 1 rotation
+            QGCLabel {
+                text:       "Compass 1 Orientation"
+                visible:    showCompassRotations && showCompass1Rot
             }
-                Component {
-                    id: compass1ComponentCombo
 
             FactComboBox {
-                        id:     compass1RotationCombo
-                        width:  rotationColumnWidth
-                        model:  rotations
-                        fact:   cal_mag1_rot
+                width:      parent.width
+                fact:       compass1Rot
+                indexModel: false
+                visible:    showCompassRotations && showCompass1Rot
             }
+
+            Item {
+                width:  1
+                height: ScreenTools.defaultFontPixelHeight
             }
-                Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel : null }
-                Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo : null }
 
             // Compass 2 rotation
-                Component {
-                    id: compass2ComponentLabel
-
-                    QGCLabel { text: "Compass 2 Orientation" }
+            QGCLabel {
+                text:       "Compass 2 Orientation"
+                visible:    showCompassRotations && showCompass2Rot
             }
-                Component {
-                    id: compass2ComponentCombo
 
             FactComboBox {
-                        id:     compass1RotationCombo
-                        width:  rotationColumnWidth
-                        model:  rotations
-                        fact:   cal_mag2_rot
+                width:      parent.width
+                fact:       compass2Rot
+                indexModel: false
+                visible:    showCompassRotations && showCompass2Rot
+            }
+
+            Item {
+                width:  1
+                height: ScreenTools.defaultFontPixelHeight
             }
+
+            // Compass 3 rotation
+            QGCLabel {
+                text:       "Compass 3 Orientation"
+                visible:    showCompassRotations && showCompass3Rot
+            }
+
+            FactComboBox {
+                width:      parent.width
+                fact:       compass3Rot
+                indexModel: false
+                visible:    showCompassRotations && showCompass3Rot
             }
-                Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel : null }
-                Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo : null }
         } // Column
-        } // QGCViewDialog
-    } // Component - compassRotationDialogComponent
+    } // Component - Rotation combos
 
     QGCViewPanel {
         id:             panel
         anchors.fill:   parent
 
-        Column {
-            anchors.fill: parent
-
         Row {
-                readonly property int buttonWidth: ScreenTools.defaultFontPixelWidth * 15
-
+            id:         buttonsRow
             spacing:    ScreenTools.defaultFontPixelWidth
 
+            readonly property int buttonWidth: ScreenTools.defaultFontPixelWidth * 15
+
             QGCLabel { text: "Calibrate:"; anchors.baseline: compassButton.baseline }
 
             IndicatorButton {
                 id:             compassButton
                 width:          parent.buttonWidth
-                    text:           "Compass - NYI"
+                text:           "Compass"
                 indicatorGreen: !compassCalNeeded
 
                 onClicked: {
+                    if (controller.accelSetupNeeded) {
+                        showMessage("Calibrate Compass", "Accelerometer must be calibrated prior to Compass.", StandardButton.Ok)
+                    } else if (compass3Id.value != 0 && compass3Use.value !=0) {
+                        showMessage("Unabled to calibrate", "Support for calibrating compass 3 is currently not supported by QGroundControl.", StandardButton.Ok)
+                    } else {
                         preCalibrationDialogType = "compass"
                         preCalibrationDialogHelp = compassHelp
                         showDialog(preCalibrationDialogComponent, "Calibrate Compass", 50, StandardButton.Cancel | StandardButton.Ok)
                     }
                 }
+            }
 
             IndicatorButton {
                 id:             accelButton
@@ -331,27 +297,27 @@ QGCView {
                 enabled:    false
                 onClicked:  controller.cancelCalibration()
             }
-            }
-
-            Item { height: ScreenTools.defaultFontPixelHeight; width: 10 } // spacer
+        } // Row - Cal buttons
 
         ProgressBar {
             id:                 progressBar
-                width: parent.width - rotationColumnWidth
+            anchors.topMargin:  ScreenTools.defaultFontPixelHeight
+            anchors.top:        buttonsRow.bottom
+            anchors.left:       parent.left
+            anchors.right:      rotationsLoader.left
         }
 
-            Item { height: ScreenTools.defaultFontPixelHeight; width: 10 } // spacer
-
         Item {
-                property int calDisplayAreaWidth: parent.width - rotationColumnWidth
-
-                width:  parent.width
-                height: parent.height - y
+            anchors.topMargin:      ScreenTools.defaultFontPixelHeight
+            anchors.rightMargin:    ScreenTools.defaultFontPixelHeight
+            anchors.top:            progressBar.bottom
+            anchors.bottom:         parent.bottom
+            anchors.left:           parent.left
+            anchors.right:          rotationsLoader.left
 
             TextArea {
                 id:             statusTextArea
-                    width:          parent.calDisplayAreaWidth
-                    height:         parent.height
+                anchors.fill:   parent
                 readOnly:       true
                 frameVisible:   false
                 text:           statusTextAreaDefaultText
@@ -364,8 +330,7 @@ QGCView {
 
             Rectangle {
                 id:             orientationCalArea
-                    width:      parent.calDisplayAreaWidth
-                    height:     parent.height
+                anchors.fill:   parent
                 visible:        controller.showOrientationCalArea
                 color:          qgcPal.windowShade
 
@@ -431,119 +396,16 @@ QGCView {
                     }
                 }
             }
+        } // Item - Cal display area
 
-                Column {
-                    anchors.leftMargin: ScreenTools.defaultFontPixelWidth
-                    anchors.left:       orientationCalArea.right
-                    x:                  parent.width - rotationColumnWidth
-                    spacing:            ScreenTools.defaultFontPixelWidth
-
-                    Column {
-                        spacing: ScreenTools.defaultFontPixelWidth
-
-                        QGCLabel {
-                            font.pixelSize: sideBarH1PointSize
-                            text: "Autopilot Orientation"
-                        }
-
-                        QGCLabel {
-                            width:      parent.width
-                            wrapMode:   Text.WordWrap
-                            text: boardRotationText
-                        }
-
-                        FactComboBox {
-                            id:     boardRotationCombo
-                            width:  rotationColumnWidth;
-                            model:  rotations
-                            fact:   sens_board_rot
-                        }
-                    }
-
-                    Column {
-                        spacing: ScreenTools.defaultFontPixelWidth
-
-                        // Compass 0 rotation
-                        Component {
-                            id: compass0ComponentLabel2
-
-                            QGCLabel {
-                                font.pixelSize: sideBarH1PointSize
-                                text: "External Compass Orientation"
-                            }
-                        }
-
-                        Component {
-                            id: compass0ComponentCombo2
-
-                            FactComboBox {
-                                id:     compass0RotationCombo
-                                width:  rotationColumnWidth
-                                model:  rotations
-                                fact:   cal_mag0_rot
-                            }
-                        }
-
-                        Loader { sourceComponent: showCompass0Rot ? compass0ComponentLabel2 : null }
-                        Loader { sourceComponent: showCompass0Rot ? compass0ComponentCombo2 : null }
-                    }
-
-                    Column {
-                        spacing: ScreenTools.defaultFontPixelWidth
-
-                        // Compass 1 rotation
-                        Component {
-                            id: compass1ComponentLabel2
-
-                            QGCLabel {
-                                font.pixelSize: sideBarH1PointSize
-                                text: "External Compass 1 Orientation"
-                            }
-                        }
-
-                        Component {
-                            id: compass1ComponentCombo2
-
-                            FactComboBox {
-                                id:     compass1RotationCombo
+        Loader {
+            id:                 rotationsLoader
+            anchors.topMargin:  ScreenTools.defaultFontPixelHeight
+            anchors.right:      parent.right
             width:              rotationColumnWidth
-                                model:  rotations
-                                fact:   cal_mag1_rot
-                            }
-                        }
+            sourceComponent:    rotationCombosComponent
 
-                        Loader { sourceComponent: showCompass1Rot ? compass1ComponentLabel2 : null }
-                        Loader { sourceComponent: showCompass1Rot ? compass1ComponentCombo2 : null }
-                    }
-
-                    Column {
-                        spacing: ScreenTools.defaultFontPixelWidth
-
-                        // Compass 2 rotation
-                        Component {
-                            id: compass2ComponentLabel2
-
-                            QGCLabel {
-                                font.pixelSize: sidebarH1PointSize
-                                text: "Compass 2 Orientation"
-                            }
-                        }
-
-                        Component {
-                            id: compass2ComponentCombo2
-
-                            FactComboBox {
-                                id:     compass1RotationCombo
-                                width:  rotationColumnWidth
-                                model:  rotations
-                                fact:   cal_mag2_rot
-                            }
-                        }
-                        Loader { sourceComponent: showCompass2Rot ? compass2ComponentLabel2 : null }
-                        Loader { sourceComponent: showCompass2Rot ? compass2ComponentCombo2 : null }
-                    }
-                }
-            }
+            property bool showCompassRotations: true
         }
     } // QGCViewPanel
 } // QGCView

src/AutoPilotPlugins/APM/APMSensorsComponentController.cc

@@ -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);
+    if (_accelCalInProgress) {
         _nextButton->setEnabled(true);
+    }
     _cancelButton->setEnabled(false);
 }
 
@@ -162,9 +166,6 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     case StopCalibrationSuccess:
         _orientationCalAreaHelpText->setProperty("text", "Calibration complete");
         emit resetStatusTextArea();
-            if (_magCalInProgress) {
-                emit setCompassRotations();
-            }
         break;
 
     case StopCalibrationCancelled:
@@ -181,18 +182,18 @@ void APMSensorsComponentController::_stopCalibration(APMSensorsComponentControll
     
     _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)
@@ -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);
+
+    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);
 
+    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);