OP-975 Move six points calibration to a reusable class

ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.cpp

@@ -423,6 +423,33 @@ int CalibrationUtils::LinearEquationsSolve(int nDim, double *pfMatr, double *pfV
 
     return 1;
 }
+
+double CalibrationUtils::listMean(QList<double> list)
+{
+    double accum = 0;
+
+    for (int i = 0; i < list.size(); i++) {
+        accum += list[i];
+    }
+    return accum / list.size();
 }
 
+double CalibrationUtils::listVar(QList<double> list)
+{
+    double mean_accum = 0;
+    double var_accum  = 0;
+    double mean;
 
+    for (int i = 0; i < list.size(); i++) {
+        mean_accum += list[i];
+    }
+    mean = mean_accum / list.size();
+
+    for (int i = 0; i < list.size(); i++) {
+        var_accum += (list[i] - mean) * (list[i] - mean);
+    }
+
+    // Use unbiased estimator
+    return var_accum / (list.size() - 1);
+}
+}

ground/openpilotgcs/src/plugins/config/calibration/calibrationutils.h

@@ -32,7 +32,7 @@
 #include <Eigen/Eigenvalues>
 #include <Eigen/Dense>
 #include <Eigen/LU>
-
+#include <QList>
 namespace OpenPilot {
 class CalibrationUtils {
 public:
@@ -48,7 +48,8 @@ public:
     static float ComputeSigma(Eigen::VectorXf *samplesY);
 
     static int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3]);
-
+    static double listMean(QList<double> list);
+    static double listVar(QList<double> list);
 private:
     static void EllipsoidFit(Eigen::VectorXf *samplesX, Eigen::VectorXf *samplesY, Eigen::VectorXf *samplesZ,
                              Eigen::Vector3f *center,

ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp

@@ -0,0 +1,385 @@
+/**
+ ******************************************************************************
+ *
+ * @file       sixpointcalibrationmodel.cpp
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @brief      Six point calibration for Magnetometer and Accelerometer
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#include "sixpointcalibrationmodel.h"
+#include <QThread>
+#include "extensionsystem/pluginmanager.h"
+#include <QMessageBox>
+#include "math.h"
+
+#define POINT_SAMPLE_SIZE 50
+#define GRAVITY           9.81f
+#define sign(x) ((x < 0) ? -1 : 1)
+
+namespace OpenPilot {
+SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
+    QObject(parent),
+    collectingData(false),
+    calibratingMag(false),
+    calibratingAccel(false),
+    position(-1)
+{}
+
+/********** Six point calibration **************/
+
+void SixPointCalibrationModel::magStart()
+{
+    start(false, true);
+}
+void SixPointCalibrationModel::accelStart()
+{
+    start(true, false);
+}
+
+/**
+ * Called by the "Start" button.  Sets up the meta data and enables the
+ * buttons to perform six point calibration of the magnetometer (optionally
+ * accel) to compute the scale and bias of this sensor based on the current
+ * home location magnetic strength.
+ */
+void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag)
+{
+    calibratingAccel = calibrateAccel;
+    calibratingMag   = calibrateMag;
+    // Store and reset board rotation before calibration starts
+    isBoardRotationStored = false;
+    storeAndClearBoardRotation();
+
+    RevoCalibration *revoCalibration     = RevoCalibration::GetInstance(getObjectManager());
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+    AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
+
+    Q_ASSERT(revoCalibration);
+    Q_ASSERT(homeLocation);
+    RevoCalibration::DataFields revoCalibrationData     = revoCalibration->getData();
+    savedSettings.revoCalibration   = revoCalibration->getData();
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
+    AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
+    savedSettings.accelGyroSettings = accelGyroSettings->getData();
+
+    // check if Homelocation is set
+    if (!homeLocationData.Set) {
+        QMessageBox msgBox;
+        msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
+        msgBox.setStandardButtons(QMessageBox::Ok);
+        msgBox.setDefaultButton(QMessageBox::Ok);
+        msgBox.setIcon(QMessageBox::Information);
+        msgBox.exec();
+        return;
+    }
+
+    // Calibration accel
+    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
+    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
+    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
+    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]   = 0;
+    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]   = 0;
+    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]   = 0;
+
+    accel_accum_x.clear();
+    accel_accum_y.clear();
+    accel_accum_z.clear();
+
+    // Calibration mag
+    // Reset the transformation matrix to identity
+    for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) {
+        revoCalibrationData.mag_transform[i] = 0;
+    }
+    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1;
+    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1;
+    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 1;
+    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = 0;
+    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = 0;
+    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0;
+
+    // Disable adaptive mag nulling
+    initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
+    revoCalibrationData.MagBiasNullingRate = 0;
+
+    revoCalibration->setData(revoCalibrationData);
+    accelGyroSettings->setData(accelGyroSettingsData);
+
+    QThread::usleep(100000);
+
+    mag_accum_x.clear();
+    mag_accum_y.clear();
+    mag_accum_z.clear();
+
+    UAVObject::Metadata mdata;
+
+    /* Need to get as many accel updates as possible */
+    AccelState *accelState = AccelState::GetInstance(getObjectManager());
+    Q_ASSERT(accelState);
+
+    initialAccelStateMdata = accelState->getMetadata();
+    mdata = initialAccelStateMdata;
+    UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
+    mdata.flightTelemetryUpdatePeriod = 100;
+    accelState->setMetadata(mdata);
+
+    /* Need to get as many mag updates as possible */
+    MagState *mag = MagState::GetInstance(getObjectManager());
+    Q_ASSERT(mag);
+    initialMagStateMdata = mag->getMetadata();
+    mdata = initialMagStateMdata;
+    UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
+    mdata.flightTelemetryUpdatePeriod = 100;
+    mag->setMetadata(mdata);
+
+    /* Show instructions and enable controls */
+    displayInstructions("Place horizontally and click save position...", true);
+    displayVisualHelp("plane-horizontal");
+    disableAllCalibrations();
+    savePositionEnabledChanged(true);
+    position = 0;
+}
+
+/**
+ * Saves the data from the aircraft in one of six positions.
+ * This is called when they click "save position" and starts
+ * averaging data for this position.
+ */
+void SixPointCalibrationModel::savePositionData()
+{
+    QMutexLocker lock(&sensorsUpdateLock);
+
+    savePositionEnabledChanged(false);
+
+    accel_accum_x.clear();
+    accel_accum_y.clear();
+    accel_accum_z.clear();
+    mag_accum_x.clear();
+    mag_accum_y.clear();
+    mag_accum_z.clear();
+
+    collectingData = true;
+
+    AccelState *accelState = AccelState::GetInstance(getObjectManager());
+    Q_ASSERT(accelState);
+    MagState *mag = MagState::GetInstance(getObjectManager());
+    Q_ASSERT(mag);
+
+    connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+    connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+
+    displayInstructions("Hold...", false);
+}
+
+/**
+ * Grab a sample of mag (optionally accel) data while in this position and
+ * store it for averaging.  When sufficient points are collected advance
+ * to the next position (give message to user) or compute the scale and bias
+ */
+void SixPointCalibrationModel::getSample(UAVObject *obj)
+{
+    QMutexLocker lock(&sensorsUpdateLock);
+
+    // This is necessary to prevent a race condition on disconnect signal and another update
+    if (collectingData == true) {
+        if (obj->getObjID() == AccelState::OBJID) {
+            AccelState *accelState = AccelState::GetInstance(getObjectManager());
+            Q_ASSERT(accelState);
+            AccelState::DataFields accelStateData = accelState->getData();
+            accel_accum_x.append(accelStateData.x);
+            accel_accum_y.append(accelStateData.y);
+            accel_accum_z.append(accelStateData.z);
+        } else if (obj->getObjID() == MagState::OBJID) {
+            MagState *mag = MagState::GetInstance(getObjectManager());
+            Q_ASSERT(mag);
+            MagState::DataFields magData = mag->getData();
+            mag_accum_x.append(magData.x);
+            mag_accum_y.append(magData.y);
+            mag_accum_z.append(magData.z);
+        } else {
+            Q_ASSERT(0);
+        }
+    }
+
+    if (accel_accum_x.size() >= POINT_SAMPLE_SIZE && mag_accum_x.size() >= POINT_SAMPLE_SIZE && collectingData == true) {
+        collectingData = false;
+
+        savePositionEnabledChanged(true);
+
+        // Store the mean for this position for the accel
+        AccelState *accelState = AccelState::GetInstance(getObjectManager());
+        Q_ASSERT(accelState);
+        disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+        accel_data_x[position] = CalibrationUtils::listMean(accel_accum_x);
+        accel_data_y[position] = CalibrationUtils::listMean(accel_accum_y);
+        accel_data_z[position] = CalibrationUtils::listMean(accel_accum_z);
+
+        // Store the mean for this position for the mag
+        MagState *mag = MagState::GetInstance(getObjectManager());
+        Q_ASSERT(mag);
+        disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
+        mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x);
+        mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y);
+        mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z);
+
+        position = (position + 1) % 6;
+        if (position == 1) {
+            displayInstructions("Place with left side down and click save position...", false);
+            displayVisualHelp("plane-left");
+        }
+        if (position == 2) {
+            displayInstructions("Place upside down and click save position...", false);
+            displayVisualHelp("plane-flip");
+        }
+        if (position == 3) {
+            displayInstructions("Place with right side down and click save position...", false);
+            displayVisualHelp("plane-right");
+        }
+        if (position == 4) {
+            displayInstructions("Place with nose up and click save position...", false);
+            displayVisualHelp("plane-up");
+        }
+        if (position == 5) {
+            displayInstructions("Place with nose down and click save position...", false);
+            displayVisualHelp("plane-down");
+        }
+        if (position == 0) {
+            compute(calibratingMag, calibratingAccel);
+            savePositionEnabledChanged(false);
+
+            enableAllCalibrations();
+
+            /* Cleanup original settings */
+            accelState->setMetadata(initialAccelStateMdata);
+
+            mag->setMetadata(initialMagStateMdata);
+
+            // Recall saved board rotation
+            recallBoardRotation();
+        }
+    }
+}
+
+/**
+ * Computes the scale and bias for the magnetomer and (compile option)
+ * for the accel once all the data has been collected in 6 positions.
+ */
+void SixPointCalibrationModel::compute(bool mag, bool accel)
+{
+    double S[3], b[3];
+    double Be_length;
+    AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
+    RevoCalibration *revoCalibration     = RevoCalibration::GetInstance(getObjectManager());
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+
+    Q_ASSERT(revoCalibration);
+    Q_ASSERT(homeLocation);
+    AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
+    RevoCalibration::DataFields revoCalibrationData     = revoCalibration->getData();
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
+
+    // Calibration accel
+    if (accel) {
+        OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
+        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
+        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
+        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
+
+        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]   = -sign(S[0]) * b[0];
+        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]   = -sign(S[1]) * b[1];
+        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]   = -sign(S[2]) * b[2];
+    }
+
+    // Calibration mag
+    if (mag) {
+        Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
+        OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
+        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = fabs(S[0]);
+        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = fabs(S[1]);
+        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = fabs(S[2]);
+
+        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] = -sign(S[0]) * b[0];
+        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] = -sign(S[1]) * b[1];
+        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = -sign(S[2]) * b[2];
+    }
+    // Restore the previous setting
+    revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
+
+
+    bool good_calibration = true;
+
+    // Check the mag calibration is good
+    if (mag) {
+        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] ==
+                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0];
+        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
+                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
+        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] ==
+                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2];
+        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
+                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
+        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
+                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
+        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
+                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
+    }
+    // Check the accel calibration is good
+    if (accel) {
+        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
+                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
+        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
+                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
+        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
+                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
+        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
+                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
+        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
+                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
+        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
+                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
+    }
+    if (good_calibration) {
+        if (mag) {
+            revoCalibration->setData(revoCalibrationData);
+        } else {
+            revoCalibration->setData(savedSettings.revoCalibration);
+        }
+
+        if (accel) {
+            accelGyroSettings->setData(accelGyroSettingsData);
+        } else {
+            accelGyroSettings->setData(savedSettings.accelGyroSettings);
+        }
+        displayInstructions("Computed sensor scale and bias...", true);
+    } else {
+        displayInstructions("Bad calibration. Please repeat.", true);
+    }
+    position = -1; // set to run again
+}
+UAVObjectManager *SixPointCalibrationModel::getObjectManager()
+{
+    ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
+    UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
+
+    Q_ASSERT(objMngr);
+    return objMngr;
+}
+}

ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h

@@ -0,0 +1,101 @@
+/**
+ ******************************************************************************
+ *
+ * @file       sixpointcalibrationmodel.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @brief      Six point calibration for Magnetometer and Accelerometer
+ * @see        The GNU Public License (GPL) Version 3
+ * @defgroup
+ * @{
+ *
+ *****************************************************************************/
+/*
+ * This program 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.
+ *
+ * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+#ifndef SIXPOINTCALIBRATIONMODEL_H
+#define SIXPOINTCALIBRATIONMODEL_H
+#include <QMutex>
+#include <QObject>
+#include <QList>
+#include "calibration/calibrationutils.h"
+
+#include <revocalibration.h>
+#include <accelgyrosettings.h>
+#include <homelocation.h>
+#include <accelstate.h>
+#include <magstate.h>
+namespace OpenPilot {
+class SixPointCalibrationModel : public QObject {
+    Q_OBJECT
+
+    typedef struct {
+        RevoCalibration::DataFields   revoCalibration;
+        AccelGyroSettings::DataFields accelGyroSettings;
+    } SavedSettings;
+public:
+    explicit SixPointCalibrationModel(QObject *parent = 0);
+
+signals:
+    void displayVisualHelp(QString elementID);
+    void displayInstructions(QString instructions, bool replace);
+    void disableAllCalibrations();
+    void enableAllCalibrations();
+    void storeAndClearBoardRotation();
+    void recallBoardRotation();
+    void savePositionEnabledChanged(bool state);
+
+public slots:
+    // Slots for calibrating the mags
+    void magStart();
+    void accelStart();
+    void savePositionData();
+private slots:
+    void getSample(UAVObject *obj);
+private:
+    void start(bool calibrateAccel, bool calibrateMag);
+    UAVObjectManager *getObjectManager();
+
+
+    UAVObject::Metadata initialAccelStateMdata;
+    UAVObject::Metadata initialMagStateMdata;
+    float initialMagCorrectionRate;
+    SavedSettings savedSettings;
+
+    int position;
+
+    bool calibratingMag;
+    bool calibratingAccel;
+
+    bool isBoardRotationStored;
+
+    double accel_data_x[6], accel_data_y[6], accel_data_z[6];
+    double mag_data_x[6], mag_data_y[6], mag_data_z[6];
+
+    QMutex sensorsUpdateLock;
+
+    // ! Computes the scale and bias of the mag based on collected data
+    void compute(bool mag, bool accel);
+
+    bool collectingData;
+    QList<double> accel_accum_x;
+    QList<double> accel_accum_y;
+    QList<double> accel_accum_z;
+    QList<double> mag_accum_x;
+    QList<double> mag_accum_y;
+    QList<double> mag_accum_z;
+};
+}
+#endif // SIXPOINTCALIBRATIONMODEL_H

ground/openpilotgcs/src/plugins/config/config.pro

@@ -51,7 +51,8 @@ HEADERS += configplugin.h \
     calibration/thermal/boardsetuptransition.h \
     calibration/thermal/dataacquisitiontransition.h \
     calibration/thermal/settingshandlingtransitions.h \
-    calibration/thermal/compensationcalculationtransition.h
+    calibration/thermal/compensationcalculationtransition.h \
+    calibration/sixpointcalibrationmodel.h
 
 SOURCES += configplugin.cpp \
     configgadgetwidget.cpp \
@@ -86,7 +87,8 @@ SOURCES += configplugin.cpp \
     calibration/wizardmodel.cpp \
     calibration/thermal/thermalcalibration.cpp \
     calibration/thermal/thermalcalibrationhelper.cpp \
-    calibration/thermal/thermalcalibrationmodel.cpp
+    calibration/thermal/thermalcalibrationmodel.cpp \
+    calibration/sixpointcalibrationmodel.cpp
 
 FORMS += airframe.ui \
     airframe_ccpm.ui \

ground/openpilotgcs/src/plugins/config/configrevowidget.cpp

@@ -50,10 +50,9 @@
 #include <gyrostate.h>
 #include <magstate.h>
 
-#define GRAVITY 9.81f
 #include "assertions.h"
 #include "calibration.h"
-
+#include "calibration/calibrationutils.h"
 #define sign(x) ((x < 0) ? -1 : 1)
 
 // Uncomment this to enable 6 point calibration on the accels
@@ -76,8 +75,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     ConfigTaskWidget(parent),
     m_ui(new Ui_RevoSensorsWidget()),
     collectingData(false),
-    calibratingMag(false),
-    calibratingAccel(false),
     position(-1),
     isBoardRotationStored(false)
 {
@@ -113,6 +110,20 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
     // note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue.
 
+    m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this);
+    // six point calibrations
+    connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(accelStart()));
+    connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(magStart()));
+    connect(m_ui->sixPointsSave, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(savePositionData()));
+
+    connect(m_sixPointCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
+    connect(m_sixPointCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations()));
+    connect(m_sixPointCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation()));
+    connect(m_sixPointCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation()));
+    connect(m_sixPointCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool)));
+    connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
+    connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool)));
+
     // Connect the signals
     // gyro zero calibration
     connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart()));
@@ -121,10 +132,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart()));
     connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition()));
 
-    // six point calibrations
-    connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), this, SLOT(sixPointCalibrationAccelStart()));
-    connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), this, SLOT(sixPointCalibrationMagStart()));
-    connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(sixPointCalibrationSavePositionData()));
 
     connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
 
@@ -241,7 +248,7 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
         collectingData == true) {
         collectingData = false;
 
-        GyroState *gyroState   = GyroState::GetInstance(getObjectManager());
+        GyroState *gyroState = GyroState::GetInstance(getObjectManager());
 
         disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
 
@@ -258,9 +265,9 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
         revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
 
         // Update biases based on collected data
-        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X]   += listMean(gyro_accum_x);
+        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
-        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y]   += listMean(gyro_accum_y);
+        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
-        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z]   += listMean(gyro_accum_z);
+        accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
 
         revoCalibration->setData(revoCalibrationData);
         revoCalibration->updated();
@@ -282,22 +289,19 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj)
 }
 
 
-
 /******* Level calibration *******/
 /**
  * Starts an accelerometer bias calibration.
  */
 void ConfigRevoWidget::levelCalibrationStart()
 {
-
-
     // Store and reset board rotation before calibration starts
 
     disableAllCalibrations();
     m_ui->boardLevelProgress->setValue(0);
 
     rot_data_pitch = 0;
-    rot_data_roll = 0;
+    rot_data_roll  = 0;
     UAVObject::Metadata mdata;
 
     AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
@@ -314,11 +318,12 @@ void ConfigRevoWidget::levelCalibrationStart()
     disableAllCalibrations();
     m_ui->boardLevelSavePos->setEnabled(true);
     position = 0;
-
 }
 
-void ConfigRevoWidget::levelCalibrationSavePosition(){
+void ConfigRevoWidget::levelCalibrationSavePosition()
+{
     QMutexLocker lock(&sensorsUpdateLock);
+
     Q_UNUSED(lock);
 
     m_ui->boardLevelSavePos->setEnabled(false);
@@ -341,6 +346,7 @@ void ConfigRevoWidget::levelCalibrationSavePosition(){
 void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
 {
     QMutexLocker lock(&sensorsUpdateLock);
+
     Q_UNUSED(lock);
 
     switch (obj->getObjID()) {
@@ -370,10 +376,10 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
         disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *)));
 
         position++;
-        switch(position){
+        switch (position) {
         case 1:
-            rot_data_pitch = listMean(rot_accum_pitch);
+            rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
-            rot_data_roll = listMean(rot_accum_roll);
+            rot_data_roll  = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
 
             displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true);
             displayVisualHelp("plane-horizontal-rotated");
@@ -383,19 +389,19 @@ void ConfigRevoWidget::levelCalibrationGetSample(UAVObject *obj)
             m_ui->boardLevelSavePos->setEnabled(true);
             break;
         case 2:
-            rot_data_pitch += listMean(rot_accum_pitch);
+            rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
             rot_data_pitch /= 2;
-            rot_data_roll += listMean(rot_accum_roll);
+            rot_data_roll  += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
-            rot_data_roll /= 2;
+            rot_data_roll  /= 2;
             attitudeState->setMetadata(initialAttitudeStateMdata);
             levelCalibrationCompute();
             enableAllCalibrations();
             break;
         }
-
     }
 }
-void ConfigRevoWidget::levelCalibrationCompute(){
+void ConfigRevoWidget::levelCalibrationCompute()
+{
     enableAllCalibrations();
 
     AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
@@ -405,346 +411,10 @@ void ConfigRevoWidget::levelCalibrationCompute(){
     // Update the biases based on collected data
     // "rotate" the board in the opposite direction as the calculated offset
     attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch;
-    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll;
+    attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]  -= rot_data_roll;
 
     attitudeSettings->setData(attitudeSettingsData);
     attitudeSettings->updated();
-
-
-}
-
-/********** Six point calibration **************/
-
-void ConfigRevoWidget::sixPointCalibrationMagStart(){
-    sixPointCalibrationStart(false, true);
-}
-void ConfigRevoWidget::sixPointCalibrationAccelStart(){
-    sixPointCalibrationStart(true, false);
-}
-
-/**
- * Called by the "Start" button.  Sets up the meta data and enables the
- * buttons to perform six point calibration of the magnetometer (optionally
- * accel) to compute the scale and bias of this sensor based on the current
- * home location magnetic strength.
- */
-void ConfigRevoWidget::sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag)
-{
-    calibratingAccel = calibrateAccel;
-    calibratingMag = calibrateMag;
-    // Store and reset board rotation before calibration starts
-    isBoardRotationStored = false;
-    storeAndClearBoardRotation();
-
-    RevoCalibration *revoCalibration     = RevoCalibration::GetInstance(getObjectManager());
-    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
-    AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
-
-    Q_ASSERT(revoCalibration);
-    Q_ASSERT(homeLocation);
-    RevoCalibration::DataFields revoCalibrationData     = revoCalibration->getData();
-    savedSettings.revoCalibration = revoCalibration->getData();
-    HomeLocation::DataFields homeLocationData = homeLocation->getData();
-    AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
-    savedSettings.accelGyroSettings = accelGyroSettings->getData();
-
-    // check if Homelocation is set
-    if (!homeLocationData.Set) {
-        QMessageBox msgBox;
-        msgBox.setInformativeText(tr("<p>HomeLocation not SET.</p><p>Please set your HomeLocation and try again. Aborting calibration!</p>"));
-        msgBox.setStandardButtons(QMessageBox::Ok);
-        msgBox.setDefaultButton(QMessageBox::Ok);
-        msgBox.setIcon(QMessageBox::Information);
-        msgBox.exec();
-        return;
-    }
-
-    // Calibration accel
-    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1;
-    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1;
-    accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1;
-    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]   = 0;
-    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]   = 0;
-    accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]   = 0;
-
-    accel_accum_x.clear();
-    accel_accum_y.clear();
-    accel_accum_z.clear();
-
-    // Calibration mag
-    // Reset the transformation matrix to identity
-    for(int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++){
-        revoCalibrationData.mag_transform[i] = 0;
-    }
-    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = 1;
-    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = 1;
-    revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = 1;
-    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]   = 0;
-    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]   = 0;
-    revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]   = 0;
-
-    // Disable adaptive mag nulling
-    initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate;
-    revoCalibrationData.MagBiasNullingRate = 0;
-
-    revoCalibration->setData(revoCalibrationData);
-    accelGyroSettings->setData(accelGyroSettingsData);
-
-    Thread::usleep(100000);
-
-    gyro_accum_x.clear();
-    gyro_accum_y.clear();
-    gyro_accum_z.clear();
-    mag_accum_x.clear();
-    mag_accum_y.clear();
-    mag_accum_z.clear();
-
-    UAVObject::Metadata mdata;
-
-    /* Need to get as many accel updates as possible */
-    AccelState *accelState = AccelState::GetInstance(getObjectManager());
-    Q_ASSERT(accelState);
-
-    initialAccelStateMdata = accelState->getMetadata();
-    mdata = initialAccelStateMdata;
-    UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
-    mdata.flightTelemetryUpdatePeriod = 100;
-    accelState->setMetadata(mdata);
-
-    /* Need to get as many mag updates as possible */
-    MagState *mag = MagState::GetInstance(getObjectManager());
-    Q_ASSERT(mag);
-    initialMagStateMdata = mag->getMetadata();
-    mdata = initialMagStateMdata;
-    UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
-    mdata.flightTelemetryUpdatePeriod = 100;
-    mag->setMetadata(mdata);
-
-    /* Show instructions and enable controls */
-    displayInstructions("Place horizontally and click save position...", true);
-    displayVisualHelp("plane-horizontal");
-    disableAllCalibrations();
-    m_ui->sixPointsSave->setEnabled(true);
-    position = 0;
-}
-
-/**
- * Saves the data from the aircraft in one of six positions.
- * This is called when they click "save position" and starts
- * averaging data for this position.
- */
-void ConfigRevoWidget::sixPointCalibrationSavePositionData()
-{
-    QMutexLocker lock(&sensorsUpdateLock);
-
-    m_ui->sixPointsSave->setEnabled(false);
-
-    accel_accum_x.clear();
-    accel_accum_y.clear();
-    accel_accum_z.clear();
-    mag_accum_x.clear();
-    mag_accum_y.clear();
-    mag_accum_z.clear();
-
-    collectingData = true;
-
-    AccelState *accelState = AccelState::GetInstance(getObjectManager());
-    Q_ASSERT(accelState);
-    MagState *mag = MagState::GetInstance(getObjectManager());
-    Q_ASSERT(mag);
-
-    connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
-    connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
-
-    displayInstructions("Hold...");
-}
-
-/**
- * Grab a sample of mag (optionally accel) data while in this position and
- * store it for averaging.  When sufficient points are collected advance
- * to the next position (give message to user) or compute the scale and bias
- */
-void ConfigRevoWidget::sixPointCalibrationGetSample(UAVObject *obj)
-{
-    QMutexLocker lock(&sensorsUpdateLock);
-
-    // This is necessary to prevent a race condition on disconnect signal and another update
-    if (collectingData == true) {
-        if (obj->getObjID() == AccelState::OBJID) {
-            AccelState *accelState = AccelState::GetInstance(getObjectManager());
-            Q_ASSERT(accelState);
-            AccelState::DataFields accelStateData = accelState->getData();
-            accel_accum_x.append(accelStateData.x);
-            accel_accum_y.append(accelStateData.y);
-            accel_accum_z.append(accelStateData.z);
-
-        } else if (obj->getObjID() == MagState::OBJID) {
-            MagState *mag = MagState::GetInstance(getObjectManager());
-            Q_ASSERT(mag);
-            MagState::DataFields magData = mag->getData();
-            mag_accum_x.append(magData.x);
-            mag_accum_y.append(magData.y);
-            mag_accum_z.append(magData.z);
-        } else {
-            Q_ASSERT(0);
-        }
-    }
-
-    if (accel_accum_x.size() >= SAMPLE_SIZE && mag_accum_x.size() >= SAMPLE_SIZE && collectingData == true) {
-
-        collectingData = false;
-
-        m_ui->sixPointsSave->setEnabled(true);
-
-        // Store the mean for this position for the accel
-        AccelState *accelState = AccelState::GetInstance(getObjectManager());
-        Q_ASSERT(accelState);
-        disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
-        accel_data_x[position] = listMean(accel_accum_x);
-        accel_data_y[position] = listMean(accel_accum_y);
-        accel_data_z[position] = listMean(accel_accum_z);
-
-        // Store the mean for this position for the mag
-        MagState *mag = MagState::GetInstance(getObjectManager());
-        Q_ASSERT(mag);
-        disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(sixPointCalibrationGetSample(UAVObject *)));
-        mag_data_x[position] = listMean(mag_accum_x);
-        mag_data_y[position] = listMean(mag_accum_y);
-        mag_data_z[position] = listMean(mag_accum_z);
-
-        position = (position + 1) % 6;
-        if (position == 1) {
-            displayInstructions("Place with left side down and click save position...");
-            displayVisualHelp("plane-left");
-        }
-        if (position == 2) {
-            displayInstructions("Place upside down and click save position...");
-            displayVisualHelp("plane-flip");
-        }
-        if (position == 3) {
-            displayInstructions("Place with right side down and click save position...");
-            displayVisualHelp("plane-right");
-        }
-        if (position == 4) {
-            displayInstructions("Place with nose up and click save position...");
-            displayVisualHelp("plane-up");
-        }
-        if (position == 5) {
-            displayInstructions("Place with nose down and click save position...");
-            displayVisualHelp("plane-down");
-        }
-        if (position == 0) {
-            sixPointCalibrationCompute(calibratingMag, calibratingAccel);
-            m_ui->sixPointsSave->setEnabled(false);
-
-            enableAllCalibrations();
-
-            /* Cleanup original settings */
-            accelState->setMetadata(initialAccelStateMdata);
-
-            mag->setMetadata(initialMagStateMdata);
-
-            // Recall saved board rotation
-            recallBoardRotation();
-        }
-    }
-}
-
-/**
- * Computes the scale and bias for the magnetomer and (compile option)
- * for the accel once all the data has been collected in 6 positions.
- */
-void ConfigRevoWidget::sixPointCalibrationCompute(bool mag, bool accel)
-{
-    double S[3], b[3];
-    double Be_length;
-    AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
-    RevoCalibration *revoCalibration     = RevoCalibration::GetInstance(getObjectManager());
-    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
-
-    Q_ASSERT(revoCalibration);
-    Q_ASSERT(homeLocation);
-    AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
-    RevoCalibration::DataFields revoCalibrationData     = revoCalibration->getData();
-    HomeLocation::DataFields homeLocationData = homeLocation->getData();
-
-    // Calibration accel
-    if(accel) {
-        OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
-        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]);
-        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]);
-        accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = fabs(S[2]);
-
-        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]   = -sign(S[0]) * b[0];
-        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]   = -sign(S[1]) * b[1];
-        accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]   = -sign(S[2]) * b[2];
-    }
-
-    // Calibration mag
-    if(mag){
-        Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
-        OpenPilot::CalibrationUtils::SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] = fabs(S[0]);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] = fabs(S[1]);
-        revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] = fabs(S[2]);
-
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]   = -sign(S[0]) * b[0];
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]   = -sign(S[1]) * b[1];
-        revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]   = -sign(S[2]) * b[2];
-    }
-    // Restore the previous setting
-    revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate;
-
-
-    bool good_calibration = true;
-
-    // Check the mag calibration is good
-    if(mag){
-        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] ==
-                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0];
-        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] ==
-                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1];
-        good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] ==
-                            revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2];
-        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] ==
-                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X];
-        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] ==
-                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y];
-        good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] ==
-                            revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z];
-    }
-    // Check the accel calibration is good
-    if(accel){
-        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] ==
-                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X];
-        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] ==
-                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y];
-        good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] ==
-                            accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z];
-        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] ==
-                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X];
-        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] ==
-                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y];
-        good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] ==
-                            accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z];
-    }
-    if (good_calibration) {
-        if(mag){
-            revoCalibration->setData(revoCalibrationData);
-        } else {
-            revoCalibration->setData(savedSettings.revoCalibration);
-        }
-
-        if(accel){
-            accelGyroSettings->setData(accelGyroSettingsData);
-        } else {
-            accelGyroSettings->setData(savedSettings.accelGyroSettings);
-        }
-        displayInstructions("Computed sensor scale and bias...", true);
-    } else {
-        displayInstructions("Bad calibration. Please repeat.", true);
-    }
-    position = -1; // set to run again
 }
 
 void ConfigRevoWidget::storeAndClearBoardRotation()
@@ -795,11 +465,12 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID)
     m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding);
 }
 
-void ConfigRevoWidget::displayInstructions(QString instructions, bool replace){
+void ConfigRevoWidget::displayInstructions(QString instructions, bool replace)
-    if(replace || instructions.isNull()){
+{
+    if (replace || instructions.isNull()) {
         m_ui->calibrationInstructions->clear();
     }
-    if(!instructions.isNull()){
+    if (!instructions.isNull()) {
         m_ui->calibrationInstructions->append(instructions);
     }
 }

ground/openpilotgcs/src/plugins/config/configrevowidget.h

@@ -40,33 +40,24 @@
 #include <QTimer>
 #include <QMutex>
 #include "calibration/thermal/thermalcalibrationmodel.h"
-#include "calibration/calibrationutils.h"
+#include "calibration/sixpointcalibrationmodel.h"
 
 class Ui_Widget;
 
 
 class ConfigRevoWidget : public ConfigTaskWidget {
     Q_OBJECT
-    typedef struct {
+
-        RevoCalibration::DataFields revoCalibration;
-        AccelGyroSettings::DataFields accelGyroSettings;
-    } SavedSettings;
 public:
     ConfigRevoWidget(QWidget *parent = 0);
     ~ConfigRevoWidget();
 
 private:
-    void displayVisualHelp(QString elementID);
+    OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
-
-    // ! Computes the scale and bias of the mag based on collected data
-    void sixPointCalibrationCompute(bool mag, bool accel);
-
-    SavedSettings savedSettings;
-
     OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
     Ui_RevoSensorsWidget *m_ui;
     QMutex sensorsUpdateLock;
-    double maxBarHeight;
+
     int phaseCounter;
     const static double maxVarValue;
     const static int calibrationDelay = 10;
@@ -76,32 +67,17 @@ private:
     QList<double> gyro_accum_x;
     QList<double> gyro_accum_y;
     QList<double> gyro_accum_z;
-    QList<double> accel_accum_x;
-    QList<double> accel_accum_y;
-    QList<double> accel_accum_z;
-    QList<double> mag_accum_x;
-    QList<double> mag_accum_y;
-    QList<double> mag_accum_z;
 
     QList<double> rot_accum_roll;
     QList<double> rot_accum_pitch;
 
 
-
-    double accel_data_x[6], accel_data_y[6], accel_data_z[6];
-    double mag_data_x[6], mag_data_y[6], mag_data_z[6];
-
     double rot_data_roll;
     double rot_data_pitch;
 
-    bool calibratingMag;
-    bool calibratingAccel;
 
     UAVObject::Metadata initialAttitudeStateMdata;
-    UAVObject::Metadata initialAccelStateMdata;
     UAVObject::Metadata initialGyroStateMdata;
-    UAVObject::Metadata initialMagStateMdata;
-    float initialMagCorrectionRate;
 
     int position;
 
@@ -110,22 +86,16 @@ private:
     // Board rotation store/recall
     qint16 storedBoardRotation[3];
     bool isBoardRotationStored;
+
+private slots:
+    void displayVisualHelp(QString elementID);
     void storeAndClearBoardRotation();
     void recallBoardRotation();
     void displayInstructions(QString instructions = QString(), bool replace = false);
 
-
-private slots:
     // ! Overriden method from the configTaskWidget to update UI
     virtual void refreshWidgetsValues(UAVObject *object = NULL);
 
-    // Slots for calibrating the mags
-    void sixPointCalibrationMagStart();
-    void sixPointCalibrationAccelStart();
-    void sixPointCalibrationStart(bool calibrateAccel, bool calibrateMag);
-    void sixPointCalibrationGetSample(UAVObject *obj);
-    void sixPointCalibrationSavePositionData();
-
     // Slots for calibrating the accel and gyro
     void levelCalibrationStart();
     void levelCalibrationSavePosition();

ground/openpilotgcs/src/plugins/uavobjectwidgetutils/configtaskwidget.cpp

@@ -228,34 +228,6 @@ UAVObjectManager *ConfigTaskWidget::getObjectManager()
     return objMngr;
 }
 
-double ConfigTaskWidget::listMean(QList<double> list)
-{
-    double accum = 0;
-
-    for (int i = 0; i < list.size(); i++) {
-        accum += list[i];
-    }
-    return accum / list.size();
-}
-
-double ConfigTaskWidget::listVar(QList<double> list)
-{
-    double mean_accum = 0;
-    double var_accum  = 0;
-    double mean;
-
-    for (int i = 0; i < list.size(); i++) {
-        mean_accum += list[i];
-    }
-    mean = mean_accum / list.size();
-
-    for (int i = 0; i < list.size(); i++) {
-        var_accum += (list[i] - mean) * (list[i] - mean);
-    }
-
-    // Use unbiased estimator
-    return var_accum / (list.size() - 1);
-}
 
 void ConfigTaskWidget::onAutopilotDisconnect()
 {