From 3b7c274c785850abeef92b15bb6915fbc41242a8 Mon Sep 17 00:00:00 2001
From: Fredrik Arvidsson <fredrik@arvidssons.org>
Date: Sun, 12 May 2013 11:27:12 +0200
Subject: [PATCH] OP-935 Renames 'Camera stab' to 'Gimbal'. Adds functionality
 to store-clear and recall board rotation during calibration.

---
 .../src/plugins/config/configgadgetwidget.cpp |   4 +-
 .../src/plugins/config/configrevowidget.cpp   | 898 ++++++++++--------
 .../src/plugins/config/configrevowidget.h     |  26 +-
 .../src/plugins/config/revosensors.ui         | 456 +++++++--
 4 files changed, 887 insertions(+), 497 deletions(-)

diff --git a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
index 74dfea483..5f2f268ba 100644
--- a/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
@@ -104,7 +104,7 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
     icon->addFile(":/configgadget/images/camstab_normal.png", QSize(), QIcon::Normal, QIcon::Off);
     icon->addFile(":/configgadget/images/camstab_selected.png", QSize(), QIcon::Selected, QIcon::Off);
     qwd  = new ConfigCameraStabilizationWidget(this);
-    ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, *icon, QString("Camera Stab"));
+    ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, *icon, QString("Gimbal"));
 
     icon = new QIcon();
     icon->addFile(":/configgadget/images/txpid_normal.png", QSize(), QIcon::Normal, QIcon::Off);
@@ -192,7 +192,7 @@ void ConfigGadgetWidget::onAutopilotConnect()
     // First of all, check what Board type we are talking to, and
     // if necessary, remove/add tabs in the config gadget:
     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
-    UAVObjectUtilManager *utilMngr     = pm->getObject<UAVObjectUtilManager>();
+    UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
     if (utilMngr) {
         int selectedIndex = ftw->currentIndex();
         int board = utilMngr->getBoardModel();
diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
index c346f5f97..86f6e6c83 100644
--- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
+++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
@@ -61,8 +61,7 @@ const double ConfigRevoWidget::maxVarValue = 0.1;
 
 // *****************
 
-class Thread : public QThread
-{
+class Thread : public QThread {
 public:
     static void usleep(unsigned long usecs)
     {
@@ -74,9 +73,10 @@ public:
 
 ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     ConfigTaskWidget(parent),
-    collectingData(false),
     m_ui(new Ui_RevoSensorsWidget()),
-    position(-1)
+    collectingData(false),
+    position(-1),
+    isBoardRotationStored(false)
 {
     m_ui->setupUi(this);
 
@@ -105,9 +105,9 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     // Initialize the 9 bargraph values:
 
     QMatrix lineMatrix = renderer->matrixForElement("accel_x");
-    QRectF rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_x"));
+    QRectF rect  = lineMatrix.mapRect(renderer->boundsOnElement("accel_x"));
     qreal startX = rect.x();
-    qreal startY = rect.y()+ rect.height();
+    qreal startY = rect.y() + rect.height();
     // maxBarHeight will be used for scaling it later.
     maxBarHeight = rect.height();
     // Then once we have the initial location, we can put it
@@ -119,101 +119,103 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     accel_x->setElementId("accel_x");
     m_ui->sensorsBargraph->scene()->addItem(accel_x);
     accel_x->setPos(startX, startY);
-    accel_x->setTransform(QTransform::fromScale(1,0),true);
+    accel_x->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("accel_y");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_y"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
-    accel_y = new QGraphicsSvgItem();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
+    accel_y    = new QGraphicsSvgItem();
     accel_y->setSharedRenderer(renderer);
     accel_y->setElementId("accel_y");
     m_ui->sensorsBargraph->scene()->addItem(accel_y);
-    accel_y->setPos(startX,startY);
-    accel_y->setTransform(QTransform::fromScale(1,0),true);
+    accel_y->setPos(startX, startY);
+    accel_y->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("accel_z");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("accel_z"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
-    accel_z = new QGraphicsSvgItem();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
+    accel_z    = new QGraphicsSvgItem();
     accel_z->setSharedRenderer(renderer);
     accel_z->setElementId("accel_z");
     m_ui->sensorsBargraph->scene()->addItem(accel_z);
-    accel_z->setPos(startX,startY);
-    accel_z->setTransform(QTransform::fromScale(1,0),true);
+    accel_z->setPos(startX, startY);
+    accel_z->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("gyro_x");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_x"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
-    gyro_x = new QGraphicsSvgItem();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
+    gyro_x     = new QGraphicsSvgItem();
     gyro_x->setSharedRenderer(renderer);
     gyro_x->setElementId("gyro_x");
     m_ui->sensorsBargraph->scene()->addItem(gyro_x);
-    gyro_x->setPos(startX,startY);
-    gyro_x->setTransform(QTransform::fromScale(1,0),true);
+    gyro_x->setPos(startX, startY);
+    gyro_x->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("gyro_y");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_y"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
-    gyro_y = new QGraphicsSvgItem();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
+    gyro_y     = new QGraphicsSvgItem();
     gyro_y->setSharedRenderer(renderer);
     gyro_y->setElementId("gyro_y");
     m_ui->sensorsBargraph->scene()->addItem(gyro_y);
-    gyro_y->setPos(startX,startY);
-    gyro_y->setTransform(QTransform::fromScale(1,0),true);
+    gyro_y->setPos(startX, startY);
+    gyro_y->setTransform(QTransform::fromScale(1, 0), true);
 
 
     lineMatrix = renderer->matrixForElement("gyro_z");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("gyro_z"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
-    gyro_z = new QGraphicsSvgItem();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
+    gyro_z     = new QGraphicsSvgItem();
     gyro_z->setSharedRenderer(renderer);
     gyro_z->setElementId("gyro_z");
     m_ui->sensorsBargraph->scene()->addItem(gyro_z);
-    gyro_z->setPos(startX,startY);
-    gyro_z->setTransform(QTransform::fromScale(1,0),true);
+    gyro_z->setPos(startX, startY);
+    gyro_z->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("mag_x");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_x"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
     mag_x = new QGraphicsSvgItem();
     mag_x->setSharedRenderer(renderer);
     mag_x->setElementId("mag_x");
     m_ui->sensorsBargraph->scene()->addItem(mag_x);
-    mag_x->setPos(startX,startY);
-    mag_x->setTransform(QTransform::fromScale(1,0),true);
+    mag_x->setPos(startX, startY);
+    mag_x->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("mag_y");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_y"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
     mag_y = new QGraphicsSvgItem();
     mag_y->setSharedRenderer(renderer);
     mag_y->setElementId("mag_y");
     m_ui->sensorsBargraph->scene()->addItem(mag_y);
-    mag_y->setPos(startX,startY);
-    mag_y->setTransform(QTransform::fromScale(1,0),true);
+    mag_y->setPos(startX, startY);
+    mag_y->setTransform(QTransform::fromScale(1, 0), true);
 
     lineMatrix = renderer->matrixForElement("mag_z");
     rect = lineMatrix.mapRect(renderer->boundsOnElement("mag_z"));
-    startX = rect.x();
-    startY = rect.y()+ rect.height();
+    startX     = rect.x();
+    startY     = rect.y() + rect.height();
     mag_z = new QGraphicsSvgItem();
     mag_z->setSharedRenderer(renderer);
     mag_z->setElementId("mag_z");
     m_ui->sensorsBargraph->scene()->addItem(mag_z);
-    mag_z->setPos(startX,startY);
-    mag_z->setTransform(QTransform::fromScale(1,0),true);
+    mag_z->setPos(startX, startY);
+    mag_z->setTransform(QTransform::fromScale(1, 0), true);
 
     // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
     // will be dealing with some null pointers
     addUAVObject("RevoCalibration");
     addUAVObject("EKFConfiguration");
+    addUAVObject("HomeLocation");
+    addUAVObject("AttitudeSettings");
     autoLoadWidgets();
 
     // Connect the signals
@@ -222,10 +224,17 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
     connect(m_ui->sixPointsSave, SIGNAL(clicked()), this, SLOT(savePositionData()));
     connect(m_ui->noiseMeasurementStart, SIGNAL(clicked()), this, SLOT(doStartNoiseMeasurement()));
 
+    connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
+
     addUAVObjectToWidgetRelation("RevoSettings", "FusionAlgorithm", m_ui->FusionAlgorithm);
 
+    addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->rollRotation, AttitudeSettings::BOARDROTATION_ROLL);
+    addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->pitchRotation, AttitudeSettings::BOARDROTATION_PITCH);
+    addUAVObjectToWidgetRelation("AttitudeSettings", "BoardRotation", m_ui->yawRotation, AttitudeSettings::BOARDROTATION_YAW);
+
     populateWidgets();
     refreshWidgetsValues();
+    m_ui->tabWidget->setCurrentIndex(0);
 }
 
 ConfigRevoWidget::~ConfigRevoWidget()
@@ -241,25 +250,29 @@ void ConfigRevoWidget::showEvent(QShowEvent *event)
     // widget is shown, otherwise it cannot compute its values and
     // the result is usually a sensorsBargraph that is way too small.
     m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
-    m_ui->sixPointsHelp->fitInView(paperplane,Qt::KeepAspectRatio);
+    m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
 }
 
 void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
 {
     Q_UNUSED(event)
     m_ui->sensorsBargraph->fitInView(sensorsBargraph, Qt::KeepAspectRatio);
-    m_ui->sixPointsHelp->fitInView(paperplane,Qt::KeepAspectRatio);
+    m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
 }
 
 /**
-  * Starts an accelerometer bias calibration.
-  */
+ * Starts an accelerometer bias calibration.
+ */
 void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
 {
+    // Store and reset board rotation before calibration starts
+    isBoardRotationStored = false;
+    storeAndClearBoardRotation();
+
     m_ui->accelBiasStart->setEnabled(false);
     m_ui->accelBiasProgress->setValue(0);
 
-    RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
+    RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
     Q_ASSERT(revoCalibration);
     RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
     revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
@@ -267,7 +280,7 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
     revoCalibration->updated();
 
     // Disable gyro bias correction while calibrating
-    AttitudeSettings * attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
+    AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
     Q_ASSERT(attitudeSettings);
     AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
@@ -284,7 +297,7 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
     UAVObject::Metadata mdata;
 
     /* Need to get as many accel updates as possible */
-    Accels * accels = Accels::GetInstance(getObjectManager());
+    Accels *accels = Accels::GetInstance(getObjectManager());
     Q_ASSERT(accels);
     initialAccelsMdata = accels->getMetadata();
     mdata = initialAccelsMdata;
@@ -292,7 +305,7 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
     mdata.flightTelemetryUpdatePeriod = 100;
     accels->setMetadata(mdata);
 
-    Gyros * gyros = Gyros::GetInstance(getObjectManager());
+    Gyros *gyros = Gyros::GetInstance(getObjectManager());
     Q_ASSERT(gyros);
     initialGyrosMdata = gyros->getMetadata();
     mdata = initialGyrosMdata;
@@ -302,22 +315,23 @@ void ConfigRevoWidget::doStartAccelGyroBiasCalibration()
 
     // Now connect to the accels and mag updates, gather for 100 samples
     collectingData = true;
-    connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetAccelGyroBiasData(UAVObject*)));
-    connect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetAccelGyroBiasData(UAVObject*)));
+    connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
+    connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
 }
 
 /**
-  Updates the accel bias raw values
-  */
+   Updates the accel bias raw values
+ */
 void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
 {
     QMutexLocker lock(&sensorsUpdateLock);
+
     Q_UNUSED(lock);
 
-    switch(obj->getObjID()) {
+    switch (obj->getObjID()) {
     case Accels::OBJID:
     {
-        Accels * accels = Accels::GetInstance(getObjectManager());
+        Accels *accels = Accels::GetInstance(getObjectManager());
         Q_ASSERT(accels);
         Accels::DataFields accelsData = accels->getData();
 
@@ -328,7 +342,7 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
     }
     case Gyros::OBJID:
     {
-        Gyros * gyros = Gyros::GetInstance(getObjectManager());
+        Gyros *gyros = Gyros::GetInstance(getObjectManager());
         Q_ASSERT(gyros);
         Gyros::DataFields gyrosData = gyros->getData();
 
@@ -342,25 +356,24 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
     }
 
     // Work out the progress based on whichever has less
-    double p1 = (double) accel_accum_x.size() / (double) NOISE_SAMPLES;
-    double p2 = (double) accel_accum_y.size() / (double) NOISE_SAMPLES;
+    double p1 = (double)accel_accum_x.size() / (double)NOISE_SAMPLES;
+    double p2 = (double)accel_accum_y.size() / (double)NOISE_SAMPLES;
     m_ui->accelBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100);
 
-    if(accel_accum_x.size() >= NOISE_SAMPLES &&
-            gyro_accum_y.size() >= NOISE_SAMPLES &&
-            collectingData == true) {
-
+    if (accel_accum_x.size() >= NOISE_SAMPLES &&
+        gyro_accum_y.size() >= NOISE_SAMPLES &&
+        collectingData == true) {
         collectingData = false;
 
-        Accels * accels = Accels::GetInstance(getObjectManager());
-        Gyros * gyros = Gyros::GetInstance(getObjectManager());
+        Accels *accels = Accels::GetInstance(getObjectManager());
+        Gyros *gyros   = Gyros::GetInstance(getObjectManager());
 
-        disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetAccelGyroBiasData(UAVObject*)));
-        disconnect(gyros,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetAccelGyroBiasData(UAVObject*)));
+        disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
+        disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetAccelGyroBiasData(UAVObject *)));
 
         m_ui->accelBiasStart->setEnabled(true);
 
-        RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
+        RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
         Q_ASSERT(revoCalibration);
         RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
         revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE;
@@ -368,15 +381,15 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
         // Update the biases based on collected data
         revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] += listMean(accel_accum_x);
         revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] += listMean(accel_accum_y);
-        revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += ( listMean(accel_accum_z) + GRAVITY );
-        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] += listMean(gyro_accum_x);
-        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] += listMean(gyro_accum_y);
-        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] += listMean(gyro_accum_z);
+        revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] += (listMean(accel_accum_z) + GRAVITY);
+        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X]   += listMean(gyro_accum_x);
+        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y]   += listMean(gyro_accum_y);
+        revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z]   += listMean(gyro_accum_z);
 
         revoCalibration->setData(revoCalibrationData);
         revoCalibration->updated();
 
-        AttitudeSettings * attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
+        AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
         Q_ASSERT(attitudeSettings);
         AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
         attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
@@ -385,132 +398,132 @@ void ConfigRevoWidget::doGetAccelGyroBiasData(UAVObject *obj)
 
         accels->setMetadata(initialAccelsMdata);
         gyros->setMetadata(initialGyrosMdata);
+
+        // Recall saved board rotation
+        recallBoardRotation();
     }
 }
 
 
-
-int LinearEquationsSolving(int nDim, double* pfMatr, double* pfVect, double* pfSolution)
+int LinearEquationsSolving(int nDim, double *pfMatr, double *pfVect, double *pfSolution)
 {
- double fMaxElem;
- double fAcc;
+    double fMaxElem;
+    double fAcc;
 
- int i , j, k, m;
+    int i, j, k, m;
 
- for(k=0; k<(nDim-1); k++) // base row of matrix
- {
-   // search of line with max element
-   fMaxElem = fabs( pfMatr[k*nDim + k] );
-   m = k;
-   for(i=k+1; i<nDim; i++)
-   {
-     if(fMaxElem < fabs(pfMatr[i*nDim + k]) )
-     {
-       fMaxElem = pfMatr[i*nDim + k];
-       m = i;
-     }
-   }
+    for (k = 0; k < (nDim - 1); k++) { // base row of matrix
+        // search of line with max element
+        fMaxElem = fabs(pfMatr[k * nDim + k]);
+        m = k;
+        for (i = k + 1; i < nDim; i++) {
+            if (fMaxElem < fabs(pfMatr[i * nDim + k])) {
+                fMaxElem = pfMatr[i * nDim + k];
+                m = i;
+            }
+        }
 
-   // permutation of base line (index k) and max element line(index m)
-   if(m != k)
-   {
-     for(i=k; i<nDim; i++)
-     {
-       fAcc               = pfMatr[k*nDim + i];
-       pfMatr[k*nDim + i] = pfMatr[m*nDim + i];
-       pfMatr[m*nDim + i] = fAcc;
-     }
-     fAcc = pfVect[k];
-     pfVect[k] = pfVect[m];
-     pfVect[m] = fAcc;
-   }
+        // permutation of base line (index k) and max element line(index m)
+        if (m != k) {
+            for (i = k; i < nDim; i++) {
+                fAcc = pfMatr[k * nDim + i];
+                pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
+                pfMatr[m * nDim + i] = fAcc;
+            }
+            fAcc = pfVect[k];
+            pfVect[k] = pfVect[m];
+            pfVect[m] = fAcc;
+        }
 
-   if( pfMatr[k*nDim + k] == 0.) return 0; // needs improvement !!!
+        if (pfMatr[k * nDim + k] == 0.) {
+            return 0; // needs improvement !!!
+        }
+        // triangulation of matrix with coefficients
+        for (j = (k + 1); j < nDim; j++) { // current row of matrix
+            fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
+            for (i = k; i < nDim; i++) {
+                pfMatr[j * nDim + i] = pfMatr[j * nDim + i] + fAcc * pfMatr[k * nDim + i];
+            }
+            pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
+        }
+    }
 
-   // triangulation of matrix with coefficients
-   for(j=(k+1); j<nDim; j++) // current row of matrix
-   {
-     fAcc = - pfMatr[j*nDim + k] / pfMatr[k*nDim + k];
-     for(i=k; i<nDim; i++)
-     {
-       pfMatr[j*nDim + i] = pfMatr[j*nDim + i] + fAcc*pfMatr[k*nDim + i];
-     }
-     pfVect[j] = pfVect[j] + fAcc*pfVect[k]; // free member recalculation
-   }
- }
+    for (k = (nDim - 1); k >= 0; k--) {
+        pfSolution[k] = pfVect[k];
+        for (i = (k + 1); i < nDim; i++) {
+            pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
+        }
+        pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
+    }
 
- for(k=(nDim-1); k>=0; k--)
- {
-   pfSolution[k] = pfVect[k];
-   for(i=(k+1); i<nDim; i++)
-   {
-     pfSolution[k] -= (pfMatr[k*nDim + i]*pfSolution[i]);
-   }
-   pfSolution[k] = pfSolution[k] / pfMatr[k*nDim + k];
- }
-
- return 1;
+    return 1;
 }
 
 
-int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3] )
+int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3], double b[3])
 {
- int i;
- double A[5][5];
- double f[5], c[5];
- double xp, yp, zp, Sx;
+    int i;
+    double A[5][5];
+    double f[5], c[5];
+    double xp, yp, zp, Sx;
 
- // Fill in matrix A -
- // write six difference-in-magnitude equations of the form
- // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
- // or in other words
- // 2*Sx*bx*(x2-x1)/Sx^2  + Sy^2(y2^2-y1^2)/Sx^2  + 2*Sy*by*(y2-y1)/Sx^2  + Sz^2(z2^2-z1^2)/Sx^2  + 2*Sz*bz*(z2-z1)/Sx^2  = (x1^2-x2^2)
- for (i=0;i<5;i++){
-     A[i][0] = 2.0 * (x[i+1] - x[i]);
-     A[i][1] = y[i+1]*y[i+1] - y[i]*y[i];
-     A[i][2] = 2.0 * (y[i+1] - y[i]);
-     A[i][3] = z[i+1]*z[i+1] - z[i]*z[i];
-     A[i][4] = 2.0 * (z[i+1] - z[i]);
-     f[i]    = x[i]*x[i] - x[i+1]*x[i+1];
- }
+    // Fill in matrix A -
+    // write six difference-in-magnitude equations of the form
+    // Sx^2(x2^2-x1^2) + 2*Sx*bx*(x2-x1) + Sy^2(y2^2-y1^2) + 2*Sy*by*(y2-y1) + Sz^2(z2^2-z1^2) + 2*Sz*bz*(z2-z1) = 0
+    // or in other words
+    // 2*Sx*bx*(x2-x1)/Sx^2  + Sy^2(y2^2-y1^2)/Sx^2  + 2*Sy*by*(y2-y1)/Sx^2  + Sz^2(z2^2-z1^2)/Sx^2  + 2*Sz*bz*(z2-z1)/Sx^2  = (x1^2-x2^2)
+    for (i = 0; i < 5; i++) {
+        A[i][0] = 2.0 * (x[i + 1] - x[i]);
+        A[i][1] = y[i + 1] * y[i + 1] - y[i] * y[i];
+        A[i][2] = 2.0 * (y[i + 1] - y[i]);
+        A[i][3] = z[i + 1] * z[i + 1] - z[i] * z[i];
+        A[i][4] = 2.0 * (z[i + 1] - z[i]);
+        f[i]    = x[i] * x[i] - x[i + 1] * x[i + 1];
+    }
 
- // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
- if (  !LinearEquationsSolving( 5, (double *)A, f, c) ) return 0;
+    // solve for c0=bx/Sx, c1=Sy^2/Sx^2; c2=Sy*by/Sx^2, c3=Sz^2/Sx^2, c4=Sz*bz/Sx^2
+    if (!LinearEquationsSolving(5, (double *)A, f, c)) {
+        return 0;
+    }
 
- // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
- xp = x[0]; yp = y[0]; zp = z[0];
- Sx = sqrt(ConstMag*ConstMag / (xp*xp + 2*c[0]*xp + c[0]*c[0] + c[1]*yp*yp + 2*c[2]*yp + c[2]*c[2]/c[1] + c[3]*zp*zp + 2*c[4]*zp + c[4]*c[4]/c[3]));
+    // use one magnitude equation and c's to find Sx - doesn't matter which - all give the same answer
+    xp   = x[0]; yp = y[0]; zp = z[0];
+    Sx   = sqrt(ConstMag * ConstMag / (xp * xp + 2 * c[0] * xp + c[0] * c[0] + c[1] * yp * yp + 2 * c[2] * yp + c[2] * c[2] / c[1] + c[3] * zp * zp + 2 * c[4] * zp + c[4] * c[4] / c[3]));
 
- S[0] = Sx;
- b[0] = Sx*c[0];
- S[1] = sqrt(c[1]*Sx*Sx);
- b[1] = c[2]*Sx*Sx/S[1];
- S[2] = sqrt(c[3]*Sx*Sx);
- b[2] = c[4]*Sx*Sx/S[2];
+    S[0] = Sx;
+    b[0] = Sx * c[0];
+    S[1] = sqrt(c[1] * Sx * Sx);
+    b[1] = c[2] * Sx * Sx / S[1];
+    S[2] = sqrt(c[3] * Sx * Sx);
+    b[2] = c[4] * Sx * Sx / S[2];
 
- return 1;
+    return 1;
 }
 
 /********** Functions for six point calibration **************/
 
 /**
-  * 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.
-  */
+ * 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::doStartSixPointCalibration()
 {
-    RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
-    HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
+    // Store and reset board rotation before calibration starts
+    isBoardRotationStored = false;
+    storeAndClearBoardRotation();
+
+    RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+
     Q_ASSERT(revoCalibration);
     Q_ASSERT(homeLocation);
     RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 
-    //check if Homelocation is set
-    if(!homeLocationData.Set)
-    {
+    // 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);
@@ -525,9 +538,9 @@ void ConfigRevoWidget::doStartSixPointCalibration()
     revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = 1;
     revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = 1;
     revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = 1;
-    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
-    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
-    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X]   = 0;
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y]   = 0;
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z]   = 0;
 
     accel_accum_x.clear();
     accel_accum_y.clear();
@@ -538,9 +551,9 @@ void ConfigRevoWidget::doStartSixPointCalibration()
     revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = 1;
     revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = 1;
     revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 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;
+    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;
@@ -561,7 +574,7 @@ void ConfigRevoWidget::doStartSixPointCalibration()
 
 #ifdef SIX_POINT_CAL_ACCEL
     /* Need to get as many accel updates as possible */
-    Accels * accels = Accels::GetInstance(getObjectManager());
+    Accels *accels = Accels::GetInstance(getObjectManager());
     Q_ASSERT(accels);
 
     initialAccelsMdata = accels->getMetadata();
@@ -572,7 +585,7 @@ void ConfigRevoWidget::doStartSixPointCalibration()
 #endif
 
     /* Need to get as many mag updates as possible */
-    Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
+    Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
     Q_ASSERT(mag);
     initialMagMdata = mag->getMetadata();
     mdata = initialMagMdata;
@@ -590,13 +603,14 @@ void ConfigRevoWidget::doStartSixPointCalibration()
 }
 
 /**
-  * 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.
-  */
+ * 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::savePositionData()
 {
     QMutexLocker lock(&sensorsUpdateLock);
+
     m_ui->sixPointsSave->setEnabled(false);
 
     accel_accum_x.clear();
@@ -608,39 +622,39 @@ void ConfigRevoWidget::savePositionData()
 
     collectingData = true;
 
-    Accels * accels = Accels::GetInstance(getObjectManager());
+    Accels *accels    = Accels::GetInstance(getObjectManager());
     Q_ASSERT(accels);
-    Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
+    Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
     Q_ASSERT(mag);
 
-    connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
-    connect(mag, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
+    connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
+    connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(UAVObject *)));
 
     m_ui->sixPointCalibInstructions->append("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::doGetSixPointCalibrationMeasurement(UAVObject * obj)
+ * 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::doGetSixPointCalibrationMeasurement(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() == Accels::OBJID ) {
+        if (obj->getObjID() == Accels::OBJID) {
 #ifdef SIX_POINT_CAL_ACCEL
-            Accels * accels = Accels::GetInstance(getObjectManager());
+            Accels *accels = Accels::GetInstance(getObjectManager());
             Q_ASSERT(accels);
             Accels::DataFields accelsData = accels->getData();
             accel_accum_x.append(accelsData.x);
             accel_accum_y.append(accelsData.y);
             accel_accum_z.append(accelsData.z);
 #endif
-        } else if( obj->getObjID() == Magnetometer::OBJID ) {
-            Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
+        } else if (obj->getObjID() == Magnetometer::OBJID) {
+            Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
             Q_ASSERT(mag);
             Magnetometer::DataFields magData = mag->getData();
             mag_accum_x.append(magData.x);
@@ -652,9 +666,9 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject * obj)
     }
 
 #ifdef SIX_POINT_CAL_ACCEL
-    if(accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) {
+    if (accel_accum_x.size() >= 20 && mag_accum_x.size() >= 20 && collectingData == true) {
 #else
-    if(mag_accum_x.size() >= 20 && collectingData == true) {
+    if (mag_accum_x.size() >= 20 && collectingData == true) {
 #endif
         collectingData = false;
 
@@ -662,44 +676,44 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject * obj)
 
 #ifdef SIX_POINT_CAL_ACCEL
         // Store the mean for this position for the accel
-        Accels * accels = Accels::GetInstance(getObjectManager());
+        Accels *accels = Accels::GetInstance(getObjectManager());
         Q_ASSERT(accels);
-        disconnect(accels,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
+        disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(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);
 #endif
 
         // Store the mean for this position for the mag
-        Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
+        Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
         Q_ASSERT(mag);
-        disconnect(mag,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(doGetSixPointCalibrationMeasurement(UAVObject*)));
+        disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetSixPointCalibrationMeasurement(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) {
+        if (position == 1) {
             m_ui->sixPointCalibInstructions->append("Place with left side down and click save position...");
             displayPlane("plane-left");
         }
-        if(position == 2) {
+        if (position == 2) {
             m_ui->sixPointCalibInstructions->append("Place upside down and click save position...");
             displayPlane("plane-flip");
         }
-        if(position == 3) {
+        if (position == 3) {
             m_ui->sixPointCalibInstructions->append("Place with right side down and click save position...");
             displayPlane("plane-right");
         }
-        if(position == 4) {
+        if (position == 4) {
             m_ui->sixPointCalibInstructions->append("Place with nose up and click save position...");
             displayPlane("plane-up");
         }
-        if(position == 5) {
+        if (position == 5) {
             m_ui->sixPointCalibInstructions->append("Place with nose down and click save position...");
             displayPlane("plane-down");
         }
-        if(position == 0) {
+        if (position == 0) {
             computeScaleBias();
             m_ui->sixPointsStart->setEnabled(true);
             m_ui->sixPointsSave->setEnabled(false);
@@ -709,144 +723,187 @@ void ConfigRevoWidget::doGetSixPointCalibrationMeasurement(UAVObject * obj)
             accels->setMetadata(initialAccelsMdata);
 #endif
             mag->setMetadata(initialMagMdata);
+
+            // 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.
-  */
+ * 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::computeScaleBias()
 {
-   double S[3], b[3];
-   double Be_length;
-   RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
-   HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
-   Q_ASSERT(revoCalibration);
-   Q_ASSERT(homeLocation);
-   RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
-   HomeLocation::DataFields homeLocationData = homeLocation->getData();
+    double S[3], b[3];
+    double Be_length;
+    RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
 
-#ifdef SIX_POINT_CAL_ACCEL
-   // Calibration accel
-   SixPointInConstFieldCal( homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
-   revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]);
-   revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]);
-   revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]);
-
-   revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = -sign(S[0]) * b[0];
-   revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = -sign(S[1]) * b[1];
-   revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
-#endif
-
-   // Calibration mag
-   Be_length = sqrt(pow(homeLocationData.Be[0],2)+pow(homeLocationData.Be[1],2)+pow(homeLocationData.Be[2],2));
-   SixPointInConstFieldCal( Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
-   revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
-   revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
-   revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 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;
-
-#ifdef SIX_POINT_CAL_ACCEL
-   bool good_calibration = true;
-
-   // Check the mag calibration is good
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
-   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
-   good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] ==
-           revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X];
-   good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] ==
-           revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y];
-   good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] ==
-           revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z];
-   good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] ==
-           revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X];
-   good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] ==
-           revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y];
-   good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] ==
-           revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z];
-   if (good_calibration) {
-       revoCalibration->setData(revoCalibrationData);
-       m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
-   } else {
-       revoCalibrationData = revoCalibration->getData();
-       m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
-   }
-#else
-   bool good_calibration = true;
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
-   good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
-           revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
-   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];
-   if (good_calibration) {
-       revoCalibration->setData(revoCalibrationData);
-       m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
-   } else {
-       revoCalibrationData = revoCalibration->getData();
-       m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
-   }
-#endif
-
-   position = -1; //set to run again
-}
-
-/**
-  Rotate the paper plane
-  */
-void ConfigRevoWidget::displayPlane(QString elementID)
-{
-    paperplane->setElementId(elementID);
-    m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
-    m_ui->sixPointsHelp->fitInView(paperplane,Qt::KeepAspectRatio);
-
-}
-
-/*********** Noise measurement functions **************/
-/**
-  * Connect sensor updates and timeout for measuring the noise
-  */
-void ConfigRevoWidget::doStartNoiseMeasurement()
-{
-    QMutexLocker lock(&sensorsUpdateLock);
-    Q_UNUSED(lock);
-
-    RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
-    HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
     Q_ASSERT(revoCalibration);
     Q_ASSERT(homeLocation);
     RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
     HomeLocation::DataFields homeLocationData = homeLocation->getData();
 
-    //check if Homelocation is set
-    if(!homeLocationData.Set)
-    {
+#ifdef SIX_POINT_CAL_ACCEL
+    // Calibration accel
+    SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
+    revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]);
+    revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]);
+    revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]);
+
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X]   = -sign(S[0]) * b[0];
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y]   = -sign(S[1]) * b[1];
+    revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z]   = -sign(S[2]) * b[2];
+#endif
+
+    // Calibration mag
+    Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2));
+    SixPointInConstFieldCal(Be_length, mag_data_x, mag_data_y, mag_data_z, S, b);
+    revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
+    revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
+    revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = 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;
+
+#ifdef SIX_POINT_CAL_ACCEL
+    bool good_calibration = true;
+
+    // Check the mag calibration is good
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
+    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
+    good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] ==
+                        revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X];
+    good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] ==
+                        revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y];
+    good_calibration &= revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] ==
+                        revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z];
+    good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] ==
+                        revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X];
+    good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] ==
+                        revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y];
+    good_calibration &= revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] ==
+                        revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z];
+    if (good_calibration) {
+        revoCalibration->setData(revoCalibrationData);
+        m_ui->sixPointCalibInstructions->append("Computed accel and mag scale and bias...");
+    } else {
+        revoCalibrationData = revoCalibration->getData();
+        m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
+    }
+#else // ifdef SIX_POINT_CAL_ACCEL
+    bool good_calibration = true;
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X];
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y];
+    good_calibration &= revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] ==
+                        revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z];
+    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];
+    if (good_calibration) {
+        revoCalibration->setData(revoCalibrationData);
+        m_ui->sixPointCalibInstructions->append("Computed mag scale and bias...");
+    } else {
+        revoCalibrationData = revoCalibration->getData();
+        m_ui->sixPointCalibInstructions->append("Bad calibration. Please repeat.");
+    }
+#endif // ifdef SIX_POINT_CAL_ACCEL
+
+    position = -1; // set to run again
+}
+
+void ConfigRevoWidget::storeAndClearBoardRotation()
+{
+    if(!isBoardRotationStored) {
+        // Store current board rotation
+        isBoardRotationStored = true;
+        AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
+        Q_ASSERT(attitudeSettings);
+        AttitudeSettings::DataFields data = attitudeSettings->getData();
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW] = data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW];
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
+        storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+
+        // Set board rotation to no rotation
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = 0;
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = 0;
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = 0;
+        attitudeSettings->setData(data);
+    }
+}
+
+void ConfigRevoWidget::recallBoardRotation()
+{
+    if(isBoardRotationStored) {
+        // Recall current board rotation
+        isBoardRotationStored = false;
+
+        AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
+        Q_ASSERT(attitudeSettings);
+        AttitudeSettings::DataFields data = attitudeSettings->getData();
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_YAW] = storedBoardRotation[AttitudeSettings::BOARDROTATION_YAW];
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] = storedBoardRotation[AttitudeSettings::BOARDROTATION_ROLL];
+        data.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] = storedBoardRotation[AttitudeSettings::BOARDROTATION_PITCH];
+        attitudeSettings->setData(data);
+    }
+}
+
+/**
+   Rotate the paper plane
+ */
+void ConfigRevoWidget::displayPlane(QString elementID)
+{
+    paperplane->setElementId(elementID);
+    m_ui->sixPointsHelp->setSceneRect(paperplane->boundingRect());
+    m_ui->sixPointsHelp->fitInView(paperplane, Qt::KeepAspectRatio);
+}
+
+/*********** Noise measurement functions **************/
+/**
+ * Connect sensor updates and timeout for measuring the noise
+ */
+void ConfigRevoWidget::doStartNoiseMeasurement()
+{
+    QMutexLocker lock(&sensorsUpdateLock);
+
+    // Store and reset board rotation before calibration starts
+    isBoardRotationStored = false;
+    storeAndClearBoardRotation();
+
+    Q_UNUSED(lock);
+
+    RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+    Q_ASSERT(revoCalibration);
+    Q_ASSERT(homeLocation);
+    RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
+    HomeLocation::DataFields homeLocationData = homeLocation->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);
@@ -867,11 +924,11 @@ void ConfigRevoWidget::doStartNoiseMeasurement()
     mag_accum_z.clear();
 
     /* Need to get as many accel, mag and gyro updates as possible */
-    Accels * accels = Accels::GetInstance(getObjectManager());
+    Accels *accels    = Accels::GetInstance(getObjectManager());
     Q_ASSERT(accels);
-    Gyros * gyros = Gyros::GetInstance(getObjectManager());
+    Gyros *gyros      = Gyros::GetInstance(getObjectManager());
     Q_ASSERT(gyros);
-    Magnetometer * mag = Magnetometer::GetInstance(getObjectManager());
+    Magnetometer *mag = Magnetometer::GetInstance(getObjectManager());
     Q_ASSERT(mag);
 
     UAVObject::Metadata mdata;
@@ -895,26 +952,27 @@ void ConfigRevoWidget::doStartNoiseMeasurement()
     mag->setMetadata(mdata);
 
     /* Connect for updates */
-    connect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
-    connect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
-    connect(mag, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
+    connect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
+    connect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
+    connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
 }
 
 /**
-  * Called when any of the sensors are updated.  Stores the sample for measuring the
-  * variance at the end
-  */
-void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
+ * Called when any of the sensors are updated.  Stores the sample for measuring the
+ * variance at the end
+ */
+void ConfigRevoWidget::doGetNoiseSample(UAVObject *obj)
 {
     QMutexLocker lock(&sensorsUpdateLock);
+
     Q_UNUSED(lock);
 
     Q_ASSERT(obj);
 
-    switch(obj->getObjID()) {
+    switch (obj->getObjID()) {
     case Gyros::OBJID:
     {
-        Gyros * gyros = Gyros::GetInstance(getObjectManager());
+        Gyros *gyros = Gyros::GetInstance(getObjectManager());
         Q_ASSERT(gyros);
         Gyros::DataFields gyroData = gyros->getData();
         gyro_accum_x.append(gyroData.x);
@@ -924,7 +982,7 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
     }
     case Accels::OBJID:
     {
-        Accels * accels = Accels::GetInstance(getObjectManager());
+        Accels *accels = Accels::GetInstance(getObjectManager());
         Q_ASSERT(accels);
         Accels::DataFields accelsData = accels->getData();
         accel_accum_x.append(accelsData.x);
@@ -934,7 +992,7 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
     }
     case Magnetometer::OBJID:
     {
-        Magnetometer * mags = Magnetometer::GetInstance(getObjectManager());
+        Magnetometer *mags = Magnetometer::GetInstance(getObjectManager());
         Q_ASSERT(mags);
         Magnetometer::DataFields magData = mags->getData();
         mag_accum_x.append(magData.x);
@@ -946,95 +1004,109 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
         Q_ASSERT(0);
     }
 
-    float p1 = (float) mag_accum_x.length() / (float) NOISE_SAMPLES;
-    float p2 = (float) gyro_accum_x.length() / (float) NOISE_SAMPLES;
-    float p3 = (float) accel_accum_x.length() / (float) NOISE_SAMPLES;
+    float p1   = (float)mag_accum_x.length() / (float)NOISE_SAMPLES;
+    float p2   = (float)gyro_accum_x.length() / (float)NOISE_SAMPLES;
+    float p3   = (float)accel_accum_x.length() / (float)NOISE_SAMPLES;
 
     float prog = (p1 < p2) ? p1 : p2;
     prog = (prog < p3) ? prog : p3;
 
     m_ui->noiseMeasurementProgress->setValue(prog * 100);
 
-    if(mag_accum_x.length() >= NOISE_SAMPLES &&
-            gyro_accum_x.length() >= NOISE_SAMPLES &&
-            accel_accum_x.length() >= NOISE_SAMPLES) {
+    if (mag_accum_x.length() >= NOISE_SAMPLES &&
+        gyro_accum_x.length() >= NOISE_SAMPLES &&
+        accel_accum_x.length() >= NOISE_SAMPLES) {
 
         // No need to for more updates
-        Magnetometer * mags = Magnetometer::GetInstance(getObjectManager());
-        Accels * accels = Accels::GetInstance(getObjectManager());
-        Gyros * gyros = Gyros::GetInstance(getObjectManager());
-        disconnect(accels, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
-        disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
-        disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
+        Magnetometer *mags = Magnetometer::GetInstance(getObjectManager());
+        Accels *accels     = Accels::GetInstance(getObjectManager());
+        Gyros *gyros = Gyros::GetInstance(getObjectManager());
+        disconnect(accels, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
+        disconnect(gyros, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
+        disconnect(mags, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(doGetNoiseSample(UAVObject *)));
 
         EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
         Q_ASSERT(ekfConfiguration);
-        if(ekfConfiguration) {
+        if (ekfConfiguration) {
             EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData();
             revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x);
             revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y);
             revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z);
-            revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x);
-            revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y);
-            revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z);
-            revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x);
-            revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y);
-            revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z);
+            revoCalData.Q[EKFConfiguration::Q_GYROX]  = listVar(gyro_accum_x);
+            revoCalData.Q[EKFConfiguration::Q_GYROY]  = listVar(gyro_accum_y);
+            revoCalData.Q[EKFConfiguration::Q_GYROZ]  = listVar(gyro_accum_z);
+            revoCalData.R[EKFConfiguration::R_MAGX]   = listVar(mag_accum_x);
+            revoCalData.R[EKFConfiguration::R_MAGY]   = listVar(mag_accum_y);
+            revoCalData.R[EKFConfiguration::R_MAGZ]   = listVar(mag_accum_z);
             ekfConfiguration->setData(revoCalData);
         }
+
+        // Recall saved board rotation
+        recallBoardRotation();
     }
 }
 
 /********** UI Functions *************/
 /**
-  Draws the sensor variances bargraph
-  */
+   Draws the sensor variances bargraph
+ */
 void ConfigRevoWidget::drawVariancesGraph()
 {
-    EKFConfiguration * ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
+    EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
+
     Q_ASSERT(ekfConfiguration);
-    if(!ekfConfiguration)
+    if (!ekfConfiguration) {
         return;
+    }
     EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData();
 
     // The expected range is from 1E-6 to 1E-1
     double steps = 6; // 6 bars on the graph
-    float accel_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
-    if(accel_x)
-        accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false);
-    float accel_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
-    if(accel_y)
-        accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false);
-    float accel_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
-    if(accel_z)
-        accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false);
+    float accel_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
+    if (accel_x) {
+        accel_x->setTransform(QTransform::fromScale(1, accel_x_var), false);
+    }
+    float accel_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
+    if (accel_y) {
+        accel_y->setTransform(QTransform::fromScale(1, accel_y_var), false);
+    }
+    float accel_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
+    if (accel_z) {
+        accel_z->setTransform(QTransform::fromScale(1, accel_z_var), false);
+    }
 
-    float gyro_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
-    if(gyro_x)
-        gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false);
-    float gyro_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
-    if(gyro_y)
-        gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false);
-    float gyro_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
-    if(gyro_z)
-        gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false);
+    float gyro_x_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
+    if (gyro_x) {
+        gyro_x->setTransform(QTransform::fromScale(1, gyro_x_var), false);
+    }
+    float gyro_y_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
+    if (gyro_y) {
+        gyro_y->setTransform(QTransform::fromScale(1, gyro_y_var), false);
+    }
+    float gyro_z_var = -1 / steps * (1 + steps + log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
+    if (gyro_z) {
+        gyro_z->setTransform(QTransform::fromScale(1, gyro_z_var), false);
+    }
 
     // Scale by 1e-3 because mag vars are much higher.
-    float mag_x_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
-    if(mag_x)
-        mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false);
-    float mag_y_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
-    if(mag_y)
-        mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false);
-    float mag_z_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
-    if(mag_z)
-        mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false);
+    float mag_x_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
+    if (mag_x) {
+        mag_x->setTransform(QTransform::fromScale(1, mag_x_var), false);
+    }
+    float mag_y_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
+    if (mag_y) {
+        mag_y->setTransform(QTransform::fromScale(1, mag_y_var), false);
+    }
+    float mag_z_var = -1 / steps * (1 + steps + log10(1e-3 * ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
+    if (mag_z) {
+        mag_z->setTransform(QTransform::fromScale(1, mag_z_var), false);
+    }
 }
 
 /**
-  * Called by the ConfigTaskWidget parent when RevoCalibration is updated
-  * to update the UI
-  */
+ * Called by the ConfigTaskWidget parent when RevoCalibration is updated
+ * to update the UI
+ */
 void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
 {
     ConfigTaskWidget::refreshWidgetsValues(object);
@@ -1045,11 +1117,31 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object)
     m_ui->sixPointsStart->setEnabled(true);
     m_ui->accelBiasStart->setEnabled(true);
     m_ui->startDriftCalib->setEnabled(true);
-
     m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
+
+    m_ui->isSetCheckBox->setEnabled(false);
+
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+    Q_ASSERT(homeLocation);
+    HomeLocation::DataFields homeLocationData = homeLocation->getData();
+
+    QString beStr = QString("%1:%2:%3").arg(QString::number(homeLocationData.Be[0]), QString::number(homeLocationData.Be[1]), QString::number(homeLocationData.Be[2]));
+    m_ui->beBox->setText(beStr);
 }
 
-/**
-  @}
-  @}
-  */
+void ConfigRevoWidget::clearHomeLocation()
+{
+    HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager());
+
+    Q_ASSERT(homeLocation);
+    HomeLocation::DataFields homeLocationData;
+    homeLocationData.Latitude  = 0;
+    homeLocationData.Longitude = 0;
+    homeLocationData.Altitude  = 0;
+    homeLocationData.Be[0]     = 0;
+    homeLocationData.Be[1]     = 0;
+    homeLocationData.Be[2]     = 0;
+    homeLocationData.g_e = 9.81f;
+    homeLocationData.Set = HomeLocation::SET_FALSE;
+    homeLocation->setData(homeLocationData);
+}
diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h
index 9bc35004c..9eb349612 100644
--- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h
+++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h
@@ -43,19 +43,18 @@
 
 class Ui_Widget;
 
-class ConfigRevoWidget: public ConfigTaskWidget
-{
+class ConfigRevoWidget : public ConfigTaskWidget {
     Q_OBJECT
 
 public:
     ConfigRevoWidget(QWidget *parent = 0);
     ~ConfigRevoWidget();
-    
+
 private:
     void drawVariancesGraph();
     void displayPlane(QString elementID);
 
-    //! Computes the scale and bias of the mag based on collected data
+    // ! Computes the scale and bias of the mag based on collected data
     void computeScaleBias();
 
     Ui_RevoSensorsWidget *m_ui;
@@ -101,27 +100,36 @@ private:
 
     static const int NOISE_SAMPLES = 100;
 
+    // Board rotation store/recall
+    qint16 storedBoardRotation[3];
+    bool isBoardRotationStored;
+    void storeAndClearBoardRotation();
+    void recallBoardRotation();
+
+
 private slots:
-    //! Overriden method from the configTaskWidget to update UI
-    virtual void refreshWidgetsValues(UAVObject *object=NULL);
+    // ! Overriden method from the configTaskWidget to update UI
+    virtual void refreshWidgetsValues(UAVObject *object = NULL);
 
     // Slots for calibrating the mags
     void doStartSixPointCalibration();
-    void doGetSixPointCalibrationMeasurement(UAVObject * obj);
+    void doGetSixPointCalibrationMeasurement(UAVObject *obj);
     void savePositionData();
 
     // Slots for calibrating the accel and gyro
     void doStartAccelGyroBiasCalibration();
-    void doGetAccelGyroBiasData(UAVObject*);
+    void doGetAccelGyroBiasData(UAVObject *);
 
     // Slots for measuring the sensor noise
     void doStartNoiseMeasurement();
     void doGetNoiseSample(UAVObject *);
 
+    // Slot for clearing home location
+    void clearHomeLocation();
+
 protected:
     void showEvent(QShowEvent *event);
     void resizeEvent(QResizeEvent *event);
-
 };
 
 #endif // ConfigRevoWidget_H
diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui
index 2ecf1ee04..6c6e68aca 100644
--- a/ground/openpilotgcs/src/plugins/config/revosensors.ui
+++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui
@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>720</width>
-    <height>567</height>
+    <width>643</width>
+    <height>531</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -16,8 +16,11 @@
   <layout class="QVBoxLayout" name="verticalLayout">
    <item>
     <widget class="QTabWidget" name="tabWidget">
+     <property name="autoFillBackground">
+      <bool>false</bool>
+     </property>
      <property name="currentIndex">
-      <number>0</number>
+      <number>1</number>
      </property>
      <widget class="QWidget" name="tab_2">
       <attribute name="title">
@@ -499,104 +502,394 @@ p, li { white-space: pre-wrap; }
       <layout class="QVBoxLayout" name="verticalLayout_6">
        <item>
         <layout class="QHBoxLayout" name="ahrsSettingsLayout">
+         <property name="margin">
+          <number>6</number>
+         </property>
          <item>
-          <widget class="QLabel" name="label_2">
-           <property name="font">
-            <font>
-             <weight>75</weight>
-             <bold>true</bold>
-            </font>
+          <widget class="QGroupBox" name="groupBox_2">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
            </property>
-           <property name="text">
-            <string>Attitude Algorithm:</string>
+           <property name="title">
+            <string>Attitude Estimation Algorithm</string>
            </property>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
+           </property>
+           <layout class="QGridLayout" name="gridLayout_3">
+            <item row="0" column="0" alignment="Qt::AlignTop">
+             <widget class="QComboBox" name="FusionAlgorithm">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="toolTip">
+               <string>Selects the sensor integration algorithm to be used by the Revolution board.</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <spacer name="verticalSpacer_2">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeType">
+               <enum>QSizePolicy::MinimumExpanding</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
           </widget>
          </item>
          <item>
-          <widget class="QComboBox" name="FusionAlgorithm">
-           <property name="toolTip">
-            <string>Selects the sensor integration algorithm to be used by the Revolution board.</string>
+          <widget class="QGroupBox" name="groupBox">
+           <property name="sizePolicy">
+            <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
+             <horstretch>0</horstretch>
+             <verstretch>0</verstretch>
+            </sizepolicy>
            </property>
-          </widget>
-         </item>
-         <item>
-          <spacer name="horizontalSpacer_2">
-           <property name="orientation">
-            <enum>Qt::Horizontal</enum>
+           <property name="title">
+            <string>Home Location</string>
            </property>
-           <property name="sizeHint" stdset="0">
-            <size>
-             <width>40</width>
-             <height>20</height>
-            </size>
+           <property name="alignment">
+            <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
            </property>
-          </spacer>
-         </item>
-         <item>
-          <widget class="QLabel" name="label_4">
-           <property name="font">
-            <font>
-             <weight>75</weight>
-             <bold>true</bold>
-            </font>
-           </property>
-           <property name="text">
-            <string>Home Location:</string>
-           </property>
-          </widget>
-         </item>
-         <item>
-          <widget class="QRadioButton" name="homeLocationSet">
-           <property name="enabled">
-            <bool>false</bool>
-           </property>
-           <property name="toolTip">
-            <string>Saves the Home Location. This is only enabled
-if the Home Location is set, i.e. if the GPS fix is
-successful.
-
-Disabled if there is no GPS fix.</string>
-           </property>
-           <property name="text">
-            <string>Set</string>
-           </property>
-           <property name="checked">
-            <bool>true</bool>
-           </property>
-           <attribute name="buttonGroup">
-            <string notr="true">buttonGroup</string>
-           </attribute>
-          </widget>
-         </item>
-         <item>
-          <widget class="QRadioButton" name="homeLocationClear">
-           <property name="enabled">
-            <bool>true</bool>
-           </property>
-           <property name="toolTip">
-            <string>Clears the HomeLocation: only makes sense if you save
-to SD. This will force the INS to use the next GPS fix as the
-new home location unless it is in indoor mode.</string>
-           </property>
-           <property name="text">
-            <string>Clear</string>
-           </property>
-           <attribute name="buttonGroup">
-            <string notr="true">buttonGroup</string>
-           </attribute>
+           <layout class="QGridLayout" name="gridLayout_2">
+            <property name="sizeConstraint">
+             <enum>QLayout::SetDefaultConstraint</enum>
+            </property>
+            <item row="2" column="3">
+             <widget class="QLabel" name="label_10">
+              <property name="text">
+               <string>Gravity acceleration:</string>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="0">
+             <widget class="QLabel" name="label_2">
+              <property name="text">
+               <string>Latitude:</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="5">
+             <widget class="QLineEdit" name="geBox">
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="objrelation" stdset="0">
+               <stringlist>
+                <string>objname:HomeLocation</string>
+                <string>fieldname:g_e</string>
+               </stringlist>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="2">
+             <widget class="QLineEdit" name="lattitudeBox">
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="objrelation" stdset="0">
+               <stringlist>
+                <string>objname:HomeLocation</string>
+                <string>fieldname:Latitude</string>
+               </stringlist>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="5">
+             <widget class="QLineEdit" name="beBox">
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="0">
+             <widget class="QLabel" name="label_8">
+              <property name="text">
+               <string>Altitude:</string>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="2">
+             <widget class="QLineEdit" name="altitudeBox">
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="objrelation" stdset="0">
+               <stringlist>
+                <string>objname:HomeLocation</string>
+                <string>fieldname:Altitude</string>
+               </stringlist>
+              </property>
+             </widget>
+            </item>
+            <item row="1" column="3">
+             <widget class="QLabel" name="label_9">
+              <property name="text">
+               <string>Magnetic field vector:</string>
+              </property>
+             </widget>
+            </item>
+            <item row="0" column="0" colspan="6">
+             <widget class="QLabel" name="label_11">
+              <property name="text">
+               <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;This information must be set to enable calibration the Revolution controllers sensors. &lt;br/&gt;Set home location using context menu in the map widget.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="2">
+             <widget class="QLineEdit" name="longitudeBox">
+              <property name="alignment">
+               <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+              </property>
+              <property name="readOnly">
+               <bool>true</bool>
+              </property>
+              <property name="objrelation" stdset="0">
+               <stringlist>
+                <string>objname:HomeLocation</string>
+                <string>fieldname:Longitude</string>
+               </stringlist>
+              </property>
+             </widget>
+            </item>
+            <item row="3" column="5">
+             <widget class="QCheckBox" name="isSetCheckBox">
+              <property name="enabled">
+               <bool>false</bool>
+              </property>
+              <property name="text">
+               <string>Is Set</string>
+              </property>
+              <property name="checkable">
+               <bool>true</bool>
+              </property>
+              <property name="objrelation" stdset="0">
+               <stringlist>
+                <string>objname:HomeLocation</string>
+                <string>fieldname:Set</string>
+               </stringlist>
+              </property>
+             </widget>
+            </item>
+            <item row="2" column="0">
+             <widget class="QLabel" name="label_4">
+              <property name="text">
+               <string>Longitude:</string>
+              </property>
+             </widget>
+            </item>
+            <item row="5" column="5" alignment="Qt::AlignRight">
+             <widget class="QPushButton" name="hlClearButton">
+              <property name="sizePolicy">
+               <sizepolicy hsizetype="Fixed" vsizetype="Fixed">
+                <horstretch>0</horstretch>
+                <verstretch>0</verstretch>
+               </sizepolicy>
+              </property>
+              <property name="text">
+               <string>Clear</string>
+              </property>
+             </widget>
+            </item>
+            <item row="4" column="5">
+             <spacer name="verticalSpacer_3">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>0</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
           </widget>
          </item>
         </layout>
        </item>
+       <item>
+        <widget class="QGroupBox" name="groupBox_3">
+         <property name="title">
+          <string>Rotate virtual attitude relative to board</string>
+         </property>
+         <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1,0,1,0">
+          <item row="0" column="1">
+           <widget class="QLabel" name="label_12">
+            <property name="styleSheet">
+             <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
+color: rgb(255, 255, 255);
+border-radius: 5;
+font: bold 12px;
+margin:1px;</string>
+            </property>
+            <property name="text">
+             <string>Roll</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="1">
+           <widget class="QSpinBox" name="rollRotation">
+            <property name="minimum">
+             <number>-180</number>
+            </property>
+            <property name="maximum">
+             <number>180</number>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="4">
+           <spacer name="horizontalSpacer_9">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+          <item row="1" column="5">
+           <widget class="QSpinBox" name="yawRotation">
+            <property name="minimum">
+             <number>-180</number>
+            </property>
+            <property name="maximum">
+             <number>180</number>
+            </property>
+           </widget>
+          </item>
+          <item row="0" column="5">
+           <widget class="QLabel" name="label_13">
+            <property name="styleSheet">
+             <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
+color: rgb(255, 255, 255);
+border-radius: 5;
+font: bold 12px;
+margin:1px;</string>
+            </property>
+            <property name="text">
+             <string>Yaw</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="2">
+           <spacer name="horizontalSpacer_8">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+          <item row="0" column="3">
+           <widget class="QLabel" name="label_15">
+            <property name="styleSheet">
+             <string notr="true">background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255));
+color: rgb(255, 255, 255);
+border-radius: 5;
+font: bold 12px;
+margin:1px;</string>
+            </property>
+            <property name="text">
+             <string>Pitch</string>
+            </property>
+            <property name="alignment">
+             <set>Qt::AlignCenter</set>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="3">
+           <widget class="QSpinBox" name="pitchRotation">
+            <property name="minimum">
+             <number>-90</number>
+            </property>
+            <property name="maximum">
+             <number>90</number>
+            </property>
+           </widget>
+          </item>
+          <item row="1" column="0">
+           <spacer name="horizontalSpacer_7">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+          <item row="1" column="6">
+           <spacer name="horizontalSpacer_10">
+            <property name="orientation">
+             <enum>Qt::Horizontal</enum>
+            </property>
+            <property name="sizeHint" stdset="0">
+             <size>
+              <width>40</width>
+              <height>20</height>
+             </size>
+            </property>
+           </spacer>
+          </item>
+         </layout>
+        </widget>
+       </item>
        <item>
         <spacer name="verticalSpacer">
          <property name="orientation">
           <enum>Qt::Vertical</enum>
          </property>
+         <property name="sizeType">
+          <enum>QSizePolicy::Expanding</enum>
+         </property>
          <property name="sizeHint" stdset="0">
           <size>
            <width>20</width>
-           <height>40</height>
+           <height>100</height>
           </size>
          </property>
         </spacer>
@@ -716,7 +1009,4 @@ specific calibration button on top of the screen.</string>
   <include location="../coreplugin/core.qrc"/>
  </resources>
  <connections/>
- <buttongroups>
-  <buttongroup name="buttonGroup"/>
- </buttongroups>
 </ui>