OP-1150 UI for thermal calibration: Add calibration calculation

ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h

@@ -0,0 +1,66 @@
+/**
+ ******************************************************************************
+ *
+ * @file       compensationcalculation.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @brief
+ * @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 COMPENSATIONCALCULATIONTRANSITION_H
+#define COMPENSATIONCALCULATIONTRANSITION_H
+
+#include <QSignalTransition>
+#include <QEventTransition>
+
+#include "thermalcalibrationhelper.h"
+namespace OpenPilot {
+
+class CompensationCalculationTransition : public QSignalTransition {
+    Q_OBJECT
+public:
+    CompensationCalculationTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState)
+        : QSignalTransition(helper, SIGNAL(calculationCompleted())),
+        m_helper(helper)
+    {
+        QObject::connect(currentState, SIGNAL(entered()), this, SLOT(enterState()));
+
+        setTargetState(targetState);
+    }
+
+    virtual void onTransition(QEvent *e)
+    {
+        Q_UNUSED(e);
+        qDebug() << "CompensationCalculationTransition::collectionCompleted";
+    }
+
+public slots:
+    void enterState()
+    {
+        qDebug() << "CompensationCalculationTransition::enterStatus";
+        m_helper->calculate();
+    }
+private:
+    ThermalCalibrationHelper *m_helper;
+};
+}
+#endif // COMPENSATIONCALCULATIONTRANSITION_H

ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp

@@ -26,12 +26,18 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 #include "thermalcalibrationhelper.h"
-
+#include "thermalcalibration.h"
 namespace OpenPilot {
 ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) :
     QObject(parent)
 {
+
     m_boardInitialSettings = thermalCalibrationBoardSettings();
+    m_boardInitialSettings.statusSaved = false;
+    m_results = thermalCalibrationResults();
+    m_results.accelCalibrated = false;
+    m_results.baroCalibrated = false;
+    m_results.gyroCalibrated = false;
 }
 
 /**
@@ -64,7 +70,7 @@ bool ThermalCalibrationHelper::setupBoardForCalibration()
     RevoSettings *revoSettings = RevoSettings::GetInstance(objManager);
     Q_ASSERT(revoSettings);
     RevoSettings::DataFields revoSettingsData = revoSettings->getData();
-    for (int i = 0; i < RevoSettings::BAROTEMPCORRECTIONPOLYNOMIAL_NUMELEM; i++) {
+    for (uint i = 0; i < RevoSettings::BAROTEMPCORRECTIONPOLYNOMIAL_NUMELEM; i++) {
         revoSettingsData.BaroTempCorrectionPolynomial[i] = 0.0f;
     }
     revoSettings->setData(revoSettingsData);
@@ -147,7 +153,6 @@ bool ThermalCalibrationHelper::restoreInitialSettings()
     Q_ASSERT(revoSettings);
     revoSettings->setData(m_boardInitialSettings.revoSettings);
 
-    m_boardInitialSettings.statusSaved = false;
     return true;
 }
 
@@ -192,7 +197,7 @@ void ThermalCalibrationHelper::initAcquisition()
     m_targetduration  = 0;
     m_gradient = 0.0f;
     m_initialGradient = m_gradient;
-
+    m_forceStopAcquisition = false;
     // Clear all samples
     m_accelSamples.clear();
     m_gyroSamples.clear();
@@ -208,6 +213,7 @@ void ThermalCalibrationHelper::initAcquisition()
     m_gradient = 0;
     connectUAVOs();
 }
+
 void ThermalCalibrationHelper::collectSample(UAVObject *sample)
 {
     QMutexLocker lock(&sensorsUpdateLock);
@@ -255,6 +261,56 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
 void ThermalCalibrationHelper::calculate()
 {
     setProcessPercentage(ProcessPercentageBaseCalculation);
+    int count = m_baroSamples.count();
+    Eigen::VectorXf datax(count);
+    Eigen::VectorXf datay(1);
+    Eigen::VectorXf dataz(1);
+    Eigen::VectorXf datat(count);
+
+    for(int x = 0; x < count; x++){
+        datax[x] = m_baroSamples[x].Pressure;
+        datat[x] = m_baroSamples[x].Temperature;
+    }
+
+    m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro);
+
+    setProcessPercentage(processPercentage() + 2);
+    count = m_gyroSamples.count();
+    datax.resize(count);
+    datay.resize(count);
+    dataz.resize(count);
+    datat.resize(count);
+
+    for(int x = 0; x < count; x++){
+        datax[x] = m_gyroSamples[x].x;
+        datay[x] = m_gyroSamples[x].y;
+        dataz[x] = m_gyroSamples[x].z;
+        datat[x] = m_gyroSamples[x].temperature;
+    }
+
+    m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro);
+
+    //TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
+    /*
+    setProcessPercentage(processPercentage() + 2);
+    count = m_accelSamples.count();
+    datax.resize(count);
+    datay.resize(count);
+    dataz.resize(count);
+    datat.resize(count);
+
+    for(int x = 0; x < count; x++){
+        datax[x] = m_accelSamples[x].x;
+        datay[x] = m_accelSamples[x].y;
+        dataz[x] = m_accelSamples[x].z;
+        datat[x] = m_accelSamples[x].temperature;
+    }
+
+    m_results.accelCalibrated = ThermalCalibration::AccelerometerCalibration(datax, datay, dataz, datat, m_results.accel);
+    */
+    m_results.accelCalibrated = false;
+    copyResultToSettings();
+    emit calculationCompleted();
 }
 
 
@@ -284,7 +340,7 @@ void ThermalCalibrationHelper::updateTemp(float temp)
         if (m_initialGradient < .1 && m_gradient > .1) {
             m_initialGradient = m_gradient;
         }
-        if (m_gradient < TargetGradient) {
+        if (m_gradient < TargetGradient || m_forceStopAcquisition) {
             emit collectionCompleted();
         }
 
@@ -303,7 +359,6 @@ void ThermalCalibrationHelper::updateTemp(float temp)
 
 void ThermalCalibrationHelper::endAcquisition()
 {
-    QMutexLocker lock(&sensorsUpdateLock);
     disconnectUAVOs();
     setProcessPercentage(ProcessPercentageBaseCalculation);
 }
@@ -337,6 +392,44 @@ void ThermalCalibrationHelper::disconnectUAVOs()
     MagSensor *mag   = MagSensor::GetInstance(getObjectManager());
     disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
 }
+
+void ThermalCalibrationHelper::copyResultToSettings()
+{
+    UAVObjectManager *objManager = getObjectManager();
+    Q_ASSERT(objManager);
+
+    if(m_results.baroCalibrated) {
+        RevoSettings *revosettings = RevoSettings::GetInstance(objManager);
+        Q_ASSERT(revosettings);
+        revosettings->setBaroTempCorrectionPolynomial_a(m_results.baro[0]);
+        revosettings->setBaroTempCorrectionPolynomial_b(m_results.baro[1]);
+        revosettings->setBaroTempCorrectionPolynomial_c(m_results.baro[2]);
+        revosettings->setBaroTempCorrectionPolynomial_d(m_results.baro[3]);
+        revosettings->updated();
+    }
+
+    if(m_results.gyroCalibrated || m_results.accelCalibrated){
+        AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager);
+        Q_ASSERT(accelGyroSettings);
+        AccelGyroSettings::DataFields data = accelGyroSettings->getData();
+
+        if(m_results.gyroCalibrated){
+            data.gyro_temp_coeff[0] = m_results.gyro[0];
+            data.gyro_temp_coeff[1] = m_results.gyro[1];
+            data.gyro_temp_coeff[2] = m_results.gyro[2];
+            data.gyro_temp_coeff[3] = m_results.gyro[3];
+        }
+
+        if(m_results.accelCalibrated){
+            data.accel_temp_coeff[0] = m_results.gyro[0];
+            data.accel_temp_coeff[1] = m_results.gyro[1];
+            data.accel_temp_coeff[2] = m_results.gyro[2];
+        }
+        accelGyroSettings->setData(data);
+        accelGyroSettings->updated();
+    }
+}
+
 void ThermalCalibrationHelper::setMetadataForCalibration(UAVDataObject *uavo)
 {
     Q_ASSERT(uavo);

ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h

@@ -58,9 +58,17 @@ typedef struct {
     UAVObject::Metadata gyroSensorMeta;
     UAVObject::Metadata accelSensorMeta;
     UAVObject::Metadata baroensorMeta;
-    bool statusSaved = false;
+    bool statusSaved;
 } thermalCalibrationBoardSettings;
 
+typedef struct {
+    bool baroCalibrated;
+    float baro[4];
+    bool accelCalibrated;
+    float accel[3];
+    bool gyroCalibrated;
+    float gyro[4];
+} thermalCalibrationResults;
 class ThermalCalibrationHelper : public QObject {
     Q_OBJECT
 public:
@@ -79,6 +87,7 @@ public:
     {
         return m_processPercentage;
     }
+    void endAcquisition();
 
 signals:
     void statusRestoreCompleted(bool succesful);
@@ -112,10 +121,14 @@ public slots:
      */
     void initAcquisition();
 
+    void stopAcquisition(){
+        QMutexLocker lock(&sensorsUpdateLock);
+        emit collectionCompleted();
+    }
+
     void calculate();
 
     void collectSample(UAVObject *sample);
-    void endAcquisition();
     void setProcessPercentage(int value)
     {
         if (m_processPercentage != value) {
@@ -129,6 +142,8 @@ private:
     void connectUAVOs();
     void disconnectUAVOs();
 
+    void copyResultToSettings();
+
     QMutex sensorsUpdateLock;
 
     QList<AccelSensor::DataFields> m_accelSamples;
@@ -138,17 +153,19 @@ private:
 
     QTime m_startTime;
     // temperature checkpoints, used to calculate temp gradient
-    const int TimeBetweenCheckpoints = 10;
+    const static int TimeBetweenCheckpoints = 10;
     QTime m_lastCheckpointTime;
+    bool m_forceStopAcquisition;
     float m_lastCheckpointTemp;
     float m_gradient;
     float m_temperature;
     float m_initialGradient;
-    const int ProcessPercentageSaveSettings    = 5;
+    const static int ProcessPercentageSaveSettings    = 5;
-    const int ProcessPercentageSetupBoard      = 10;
+    const static int ProcessPercentageSetupBoard      = 10;
-    const int ProcessPercentageBaseAcquisition = 15;
+    const static int ProcessPercentageBaseAcquisition = 15;
-    const int ProcessPercentageBaseCalculation = 85;
+    const static int ProcessPercentageBaseCalculation = 85;
-    const float TargetGradient = 0.5f;
+    const static int ProcessPercentageSaveResults = 95;
+    const static float TargetGradient = 0.35f;
     int m_targetduration;
     int m_processPercentage;
 
@@ -165,6 +182,7 @@ private:
         m_boardInitialSettings.statusSaved = false;
     }
     thermalCalibrationBoardSettings m_boardInitialSettings;
+    thermalCalibrationResults m_results;
 
     void setMetadataForCalibration(UAVDataObject *uavo);
     UAVObjectManager *getObjectManager();

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

@@ -52,6 +52,7 @@ HEADERS += configplugin.h \
     calibration/thermal/boardsetuptransition.h \
     calibration/thermal/dataacquisitiontransition.h \
     calibration/thermal/settingshandlingtransitions.h \
+    calibration/thermal/compensationcalculationtransition.h
 
 SOURCES += configplugin.cpp \
     configgadgetwidget.cpp \