From ebb075fcfd2d4f89396edf3d2baee4b8dfe06d3f Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 18 Jan 2014 13:43:45 +0100 Subject: [PATCH] OP-1150 UI for thermal calibration: Add calibration calculation --- .../compensationcalculationtransition.h | 66 +++++++++++ .../thermal/thermalcalibrationhelper.cpp | 105 +++++++++++++++++- .../thermal/thermalcalibrationhelper.h | 34 ++++-- .../src/plugins/config/config.pro | 1 + 4 files changed, 192 insertions(+), 14 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h new file mode 100644 index 000000000..87792c092 --- /dev/null +++ b/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 +#include + +#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 diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp index c5065dd6e..c3c9131d7 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp +++ b/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); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h index 88b65e597..801044224 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h +++ b/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 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 int ProcessPercentageSetupBoard = 10; - const int ProcessPercentageBaseAcquisition = 15; - const int ProcessPercentageBaseCalculation = 85; - const float TargetGradient = 0.5f; + const static int ProcessPercentageSaveSettings = 5; + const static int ProcessPercentageSetupBoard = 10; + const static int ProcessPercentageBaseAcquisition = 15; + const static int ProcessPercentageBaseCalculation = 85; + 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(); diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index e8f370a0f..5f95c20e4 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/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 \