rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +01:00
Code Issues Releases Activity

OP-1150 UI for thermal calibration: Add calibration calculation

Browse Source
This commit is contained in:
Alessio Morale 2014-01-18 13:43:45 +01:00
parent eef8bebf15
commit ebb075fcfd
4 changed files with 192 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h Normal file
View File

@ -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

105
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp
View File

@ -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);

34
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h
View File

@ -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 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();

1
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -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 \