1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +01:00

OP-1351 initial cleanup of thermal calibration + added attempt at simulating the temperature rise

This commit is contained in:
Philippe Renon 2014-05-31 18:42:59 +02:00
parent 14e8e769c8
commit 5182ccc2eb
3 changed files with 86 additions and 133 deletions

View File

@ -31,18 +31,27 @@
#include <uavtalk/telemetrymanager.h> #include <uavtalk/telemetrymanager.h>
#include "version_info/version_info.h" #include "version_info/version_info.h"
//#define SIMULATE
namespace OpenPilot { namespace OpenPilot {
ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) : ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) :
QObject(parent) QObject(parent)
{ {
m_tempdir.reset(new QTemporaryDir()); m_tempdir.reset(new QTemporaryDir());
m_boardInitialSettings = thermalCalibrationBoardSettings(); m_memento = Memento();
m_boardInitialSettings.statusSaved = false; m_memento.statusSaved = false;
m_results = thermalCalibrationResults(); m_results = thermalCalibrationResults();
m_results.accelCalibrated = false; m_results.accelCalibrated = false;
m_results.baroCalibrated = false; m_results.baroCalibrated = false;
m_results.gyroCalibrated = false; m_results.gyroCalibrated = false;
accelSensor = AccelSensor::GetInstance(getObjectManager());
gyroSensor = GyroSensor::GetInstance(getObjectManager());
baroSensor = BaroSensor::GetInstance(getObjectManager());
magSensor = MagSensor::GetInstance(getObjectManager());
accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
revoSettings = RevoSettings::GetInstance(getObjectManager());
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
TelemetryManager *telMngr = pm->getObject<TelemetryManager>(); TelemetryManager *telMngr = pm->getObject<TelemetryManager>();
connect(telMngr, SIGNAL(disconnected()), this, SLOT(cleanup())); connect(telMngr, SIGNAL(disconnected()), this, SLOT(cleanup()));
@ -56,27 +65,16 @@ bool ThermalCalibrationHelper::setupBoardForCalibration()
{ {
qDebug() << "setupBoardForCalibration"; qDebug() << "setupBoardForCalibration";
UAVObjectManager *objManager = getObjectManager();
Q_ASSERT(objManager);
// accelSensor Meta // accelSensor Meta
AccelSensor *accelSensor = AccelSensor::GetInstance(objManager);
Q_ASSERT(accelSensor);
setMetadataForCalibration(accelSensor); setMetadataForCalibration(accelSensor);
// gyroSensor Meta // gyroSensor Meta
GyroSensor *gyroSensor = GyroSensor::GetInstance(objManager);
Q_ASSERT(gyroSensor);
setMetadataForCalibration(gyroSensor); setMetadataForCalibration(gyroSensor);
// baroSensor Meta // baroSensor Meta
BaroSensor *baroSensor = BaroSensor::GetInstance(objManager);
Q_ASSERT(baroSensor);
setMetadataForCalibration(baroSensor); setMetadataForCalibration(baroSensor);
// Clean up any gyro/accel correction before calibrating // Clean up any gyro/accel correction before calibrating
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager);
Q_ASSERT(accelGyroSettings);
AccelGyroSettings::DataFields data = accelGyroSettings->getData(); AccelGyroSettings::DataFields data = accelGyroSettings->getData();
for (uint i = 0; i < AccelGyroSettings::ACCEL_TEMP_COEFF_NUMELEM; i++) { for (uint i = 0; i < AccelGyroSettings::ACCEL_TEMP_COEFF_NUMELEM; i++) {
data.accel_temp_coeff[i] = 0.0f; data.accel_temp_coeff[i] = 0.0f;
@ -92,8 +90,6 @@ bool ThermalCalibrationHelper::setupBoardForCalibration()
accelGyroSettings->setData(data); accelGyroSettings->setData(data);
// clean any correction before calibrating // clean any correction before calibrating
RevoSettings *revoSettings = RevoSettings::GetInstance(objManager);
Q_ASSERT(revoSettings);
RevoSettings::DataFields revoSettingsData = revoSettings->getData(); RevoSettings::DataFields revoSettingsData = revoSettings->getData();
for (uint i = 0; i < RevoSettings::BAROTEMPCORRECTIONPOLYNOMIAL_NUMELEM; i++) { for (uint i = 0; i < RevoSettings::BAROTEMPCORRECTIONPOLYNOMIAL_NUMELEM; i++) {
revoSettingsData.BaroTempCorrectionPolynomial[i] = 0.0f; revoSettingsData.BaroTempCorrectionPolynomial[i] = 0.0f;
@ -115,41 +111,28 @@ bool ThermalCalibrationHelper::saveBoardInitialSettings()
// Store current board status: // Store current board status:
qDebug() << "Save initial settings"; qDebug() << "Save initial settings";
UAVObjectManager *objManager = getObjectManager();
Q_ASSERT(objManager);
// accelSensor Meta // accelSensor Meta
AccelSensor *accelSensor = AccelSensor::GetInstance(objManager); m_memento.accelSensorMeta = accelSensor->getMetadata();
Q_ASSERT(accelSensor);
m_boardInitialSettings.accelSensorMeta = accelSensor->getMetadata();
// gyroSensor Meta // gyroSensor Meta
GyroSensor *gyroSensor = GyroSensor::GetInstance(objManager); m_memento.gyroSensorMeta = gyroSensor->getMetadata();
Q_ASSERT(gyroSensor);
m_boardInitialSettings.gyroSensorMeta = gyroSensor->getMetadata();
// baroSensor Meta // baroSensor Meta
BaroSensor *baroSensor = BaroSensor::GetInstance(objManager); m_memento.baroensorMeta = baroSensor->getMetadata();
Q_ASSERT(baroSensor);
m_boardInitialSettings.baroensorMeta = baroSensor->getMetadata();
// accelGyroSettings data // accelGyroSettings data
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager); m_memento.accelGyroSettings = accelGyroSettings->getData();
Q_ASSERT(accelGyroSettings);
m_boardInitialSettings.accelGyroSettings = accelGyroSettings->getData();
// revoSettings data // revoSettings data
RevoSettings *revoSettings = RevoSettings::GetInstance(objManager); m_memento.revoSettings = revoSettings->getData();
Q_ASSERT(revoSettings);
m_boardInitialSettings.revoSettings = revoSettings->getData();
// accelGyroSettings data // accelGyroSettings data
/* /*
* TODO: for revolution it is not neede but in case of CC we would prevent having * TODO: for revolution it is not needed but in case of CC we would prevent having
* a new set of xxxSensor UAVOs beside actual xxxState so it may be needed to reset the following * a new set of xxxSensor UAVOs beside actual xxxState so it may be needed to reset the following
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager); m_memento.accelGyroSettings = accelGyroSettings->getData();
Q_ASSERT(accelGyroSettings);
m_boardInitialSettings.accelGyroSettings = accelGyroSettings->getData();
*/ */
m_boardInitialSettings.statusSaved = true; m_memento.statusSaved = true;
return true; return true;
} }
@ -159,37 +142,15 @@ bool ThermalCalibrationHelper::saveBoardInitialSettings()
*/ */
bool ThermalCalibrationHelper::restoreInitialSettings() bool ThermalCalibrationHelper::restoreInitialSettings()
{ {
if (!m_boardInitialSettings.statusSaved) { if (!m_memento.statusSaved) {
return false; return false;
} }
// restore initial board status
UAVObjectManager *objManager = getObjectManager();
Q_ASSERT(objManager);
// accelSensor Meta accelSensor->setMetadata(m_memento.accelSensorMeta);
AccelSensor *accelSensor = AccelSensor::GetInstance(objManager); gyroSensor->setMetadata(m_memento.gyroSensorMeta);
Q_ASSERT(accelSensor); baroSensor->setMetadata(m_memento.baroensorMeta);
accelSensor->setMetadata(m_boardInitialSettings.accelSensorMeta); accelGyroSettings->setData(m_memento.accelGyroSettings);
revoSettings->setData(m_memento.revoSettings);
// gyroSensor Meta
GyroSensor *gyroSensor = GyroSensor::GetInstance(objManager);
Q_ASSERT(gyroSensor);
gyroSensor->setMetadata(m_boardInitialSettings.gyroSensorMeta);
// baroSensor Meta
BaroSensor *baroSensor = BaroSensor::GetInstance(objManager);
Q_ASSERT(baroSensor);
baroSensor->setMetadata(m_boardInitialSettings.baroensorMeta);
// AccelGyroSettings data
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager);
Q_ASSERT(accelGyroSettings);
accelGyroSettings->setData(m_boardInitialSettings.accelGyroSettings);
// revoSettings data
RevoSettings *revoSettings = RevoSettings::GetInstance(objManager);
Q_ASSERT(revoSettings);
revoSettings->setData(m_boardInitialSettings.revoSettings);
return true; return true;
} }
@ -245,9 +206,7 @@ void ThermalCalibrationHelper::initAcquisition()
// retrieve current temperature/time as initial checkpoint. // retrieve current temperature/time as initial checkpoint.
m_lastCheckpointTime = QTime::currentTime(); m_lastCheckpointTime = QTime::currentTime();
m_startTime = m_lastCheckpointTime; m_startTime = m_lastCheckpointTime;
BaroSensor *baroSensor = BaroSensor::GetInstance(getObjectManager()); m_lastCheckpointTemp = m_temperature = getTemperature();
Q_ASSERT(baroSensor);
m_lastCheckpointTemp = baroSensor->getTemperature();
m_gradient = 0; m_gradient = 0;
connectUAVOs(); connectUAVOs();
} }
@ -259,9 +218,7 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
switch (sample->getObjID()) { switch (sample->getObjID()) {
case AccelSensor::OBJID: case AccelSensor::OBJID:
{ {
AccelSensor *reading = AccelSensor::GetInstance(getObjectManager()); m_accelSamples.append(accelSensor->getData());
Q_ASSERT(reading);
m_accelSamples.append(reading->getData());
m_debugStream << "ACCEL:: " << m_accelSamples.last().temperature << m_debugStream << "ACCEL:: " << m_accelSamples.last().temperature <<
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
"\t" << m_accelSamples.last().x << "\t" << m_accelSamples.last().x <<
@ -272,9 +229,7 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
} }
case GyroSensor::OBJID: case GyroSensor::OBJID:
{ {
GyroSensor *reading = GyroSensor::GetInstance(getObjectManager()); m_gyroSamples.append(gyroSensor->getData());
Q_ASSERT(reading);
m_gyroSamples.append(reading->getData());
m_debugStream << "GYRO:: " << m_gyroSamples.last().temperature << m_debugStream << "GYRO:: " << m_gyroSamples.last().temperature <<
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
"\t" << m_gyroSamples.last().x << "\t" << m_gyroSamples.last().x <<
@ -284,23 +239,23 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
} }
case BaroSensor::OBJID: case BaroSensor::OBJID:
{ {
BaroSensor *reading = BaroSensor::GetInstance(getObjectManager()); // this is needed as temperature is low pass filtered
Q_ASSERT(reading); float temp = getTemperature();
m_baroSamples.append(reading->getData()); updateTemp(temp);
BaroSensor::DataFields data = baroSensor->getData();
#ifdef SIMULATE
data.Temperature = temp;
#endif
m_baroSamples.append(data);
m_debugStream << "BARO:: " << m_baroSamples.last().Temperature << m_debugStream << "BARO:: " << m_baroSamples.last().Temperature <<
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
"\t" << m_baroSamples.last().Pressure << "\t" << m_baroSamples.last().Pressure <<
"\t" << m_baroSamples.last().Altitude << endl; "\t" << m_baroSamples.last().Altitude << endl;
// this is needed as temperature is low pass filtered
m_temperature = reading->getTemperature();
updateTemp(m_temperature);
break; break;
} }
case MagSensor::OBJID: case MagSensor::OBJID:
{ {
MagSensor *reading = MagSensor::GetInstance(getObjectManager()); m_magSamples.append(magSensor->getData());
Q_ASSERT(reading);
m_magSamples.append(reading->getData());
m_debugStream << "MAG:: " << m_debugStream << "MAG:: " <<
"\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") <<
"\t" << m_magSamples.last().x << "\t" << m_magSamples.last().x <<
@ -315,6 +270,16 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
} }
} }
float ThermalCalibrationHelper::getTemperature() {
#ifdef SIMULATE
float t = m_startTime.msecsTo(QTime::currentTime()) / 1000.0f;
// simulate a temperature rise from 20°C to 40°C
return 40 - 20 / (t + 1);
#else
return baroSensor->getTemperature();
#endif
}
void ThermalCalibrationHelper::cleanup() void ThermalCalibrationHelper::cleanup()
{ {
disconnectUAVOs(); disconnectUAVOs();
@ -338,6 +303,9 @@ void ThermalCalibrationHelper::calculate()
} }
m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro, &m_results.baroInSigma, &m_results.baroOutSigma); m_results.baroCalibrated = ThermalCalibration::BarometerCalibration(datax, datat, m_results.baro, &m_results.baroInSigma, &m_results.baroOutSigma);
if (!m_results.baroCalibrated) {
qDebug() << "Failed to calibrate baro!";
}
m_results.baroTempMin = datat.array().minCoeff(); m_results.baroTempMin = datat.array().minCoeff();
m_results.baroTempMax = datat.array().maxCoeff(); m_results.baroTempMax = datat.array().maxCoeff();
@ -356,6 +324,9 @@ void ThermalCalibrationHelper::calculate()
} }
m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro, m_results.gyroInSigma, m_results.gyroOutSigma); m_results.gyroCalibrated = ThermalCalibration::GyroscopeCalibration(datax, datay, dataz, datat, m_results.gyro, m_results.gyroInSigma, m_results.gyroOutSigma);
if (!m_results.gyroCalibrated) {
qDebug() << "Failed to calibrate gyro!";
}
m_results.accelGyroTempMin = datat.array().minCoeff(); m_results.accelGyroTempMin = datat.array().minCoeff();
m_results.accelGyroTempMax = datat.array().maxCoeff(); m_results.accelGyroTempMax = datat.array().maxCoeff();
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable. // TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
@ -454,34 +425,19 @@ void ThermalCalibrationHelper::endAcquisition()
void ThermalCalibrationHelper::connectUAVOs() void ThermalCalibrationHelper::connectUAVOs()
{ {
createDebugLog(); createDebugLog();
AccelSensor *accel = AccelSensor::GetInstance(getObjectManager());
connect(accel, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); connect(accelSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
GyroSensor *gyro = GyroSensor::GetInstance(getObjectManager()); connect(baroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
connect(gyro, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
BaroSensor *baro = BaroSensor::GetInstance(getObjectManager());
connect(baro, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
MagSensor *mag = MagSensor::GetInstance(getObjectManager());
connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
} }
void ThermalCalibrationHelper::disconnectUAVOs() void ThermalCalibrationHelper::disconnectUAVOs()
{ {
AccelSensor *accel = AccelSensor::GetInstance(getObjectManager()); disconnect(accelSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
disconnect(accel, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); disconnect(baroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
disconnect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
GyroSensor *gyro = GyroSensor::GetInstance(getObjectManager());
disconnect(gyro, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
BaroSensor *baro = BaroSensor::GetInstance(getObjectManager());
disconnect(baro, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
MagSensor *mag = MagSensor::GetInstance(getObjectManager());
disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *)));
} }
void ThermalCalibrationHelper::createDebugLog() void ThermalCalibrationHelper::createDebugLog()
@ -531,27 +487,17 @@ void ThermalCalibrationHelper::closeDebugLog()
void ThermalCalibrationHelper::copyResultToSettings() void ThermalCalibrationHelper::copyResultToSettings()
{ {
UAVObjectManager *objManager = getObjectManager();
Q_ASSERT(objManager);
if (calibrationSuccessful()) { if (calibrationSuccessful()) {
RevoSettings *revosettings = RevoSettings::GetInstance(objManager); RevoSettings::DataFields revoSettingsData = revoSettings->getData();
Q_ASSERT(revosettings); revoSettingsData.BaroTempCorrectionPolynomial[0] = m_results.baro[0];
RevoSettings::DataFields revosettingsdata = revosettings->getData(); revoSettingsData.BaroTempCorrectionPolynomial[1] = m_results.baro[1];
revosettingsdata.BaroTempCorrectionPolynomial[0] = m_results.baro[0]; revoSettingsData.BaroTempCorrectionPolynomial[2] = m_results.baro[2];
revosettingsdata.BaroTempCorrectionPolynomial[1] = m_results.baro[1]; revoSettingsData.BaroTempCorrectionPolynomial[3] = m_results.baro[3];
revosettingsdata.BaroTempCorrectionPolynomial[2] = m_results.baro[2]; revoSettingsData.BaroTempCorrectionExtent[0] = m_results.baroTempMin;
revosettingsdata.BaroTempCorrectionPolynomial[3] = m_results.baro[3]; revoSettingsData.BaroTempCorrectionExtent[1] = m_results.baroTempMax;
revosettingsdata.BaroTempCorrectionExtent[0] = m_results.baroTempMin; revoSettings->setData(revoSettingsData);
revosettingsdata.BaroTempCorrectionExtent[1] = m_results.baroTempMax;
revosettings->setData(revosettingsdata);
revosettings->updated();
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager);
Q_ASSERT(accelGyroSettings);
AccelGyroSettings::DataFields data = accelGyroSettings->getData(); AccelGyroSettings::DataFields data = accelGyroSettings->getData();
if (m_results.gyroCalibrated) { if (m_results.gyroCalibrated) {
data.gyro_temp_coeff[0] = m_results.gyro[0]; data.gyro_temp_coeff[0] = m_results.gyro[0];
data.gyro_temp_coeff[1] = m_results.gyro[1]; data.gyro_temp_coeff[1] = m_results.gyro[1];
@ -570,7 +516,6 @@ void ThermalCalibrationHelper::copyResultToSettings()
data.temp_calibrated_extent[1] = m_results.accelGyroTempMax; data.temp_calibrated_extent[1] = m_results.accelGyroTempMax;
accelGyroSettings->setData(data); accelGyroSettings->setData(data);
accelGyroSettings->updated();
} }
} }

View File

@ -64,7 +64,7 @@ typedef struct {
UAVObject::Metadata accelSensorMeta; UAVObject::Metadata accelSensorMeta;
UAVObject::Metadata baroensorMeta; UAVObject::Metadata baroensorMeta;
bool statusSaved; bool statusSaved;
} thermalCalibrationBoardSettings; } Memento;
typedef struct { typedef struct {
bool baroCalibrated; bool baroCalibrated;
@ -113,8 +113,7 @@ public:
bool calibrationSuccessful() bool calibrationSuccessful()
{ {
return m_results.baroCalibrated && return m_results.baroCalibrated && ((m_results.baroTempMax - m_results.baroTempMin) > TargetTempDelta);
((m_results.baroTempMax - m_results.baroTempMin) > 10.0f);
} }
signals: signals:
@ -208,24 +207,34 @@ private:
const static int ProcessPercentageBaseCalculation = 85; const static int ProcessPercentageBaseCalculation = 85;
const static int ProcessPercentageSaveResults = 95; const static int ProcessPercentageSaveResults = 95;
const static float TargetGradient = 0.20f; const static float TargetGradient = 0.20f;
const static float TargetTempDelta = 10.0f;
int m_targetduration; int m_targetduration;
int m_processPercentage; int m_processPercentage;
// convenience pointers
AccelSensor *accelSensor;
GyroSensor *gyroSensor;
BaroSensor *baroSensor;
MagSensor *magSensor;
AccelGyroSettings *accelGyroSettings;
RevoSettings *revoSettings;
/* board settings save/restore */ /* board settings save/restore */
bool setupBoardForCalibration(); bool setupBoardForCalibration();
bool saveBoardInitialSettings(); bool saveBoardInitialSettings();
bool restoreInitialSettings(); bool restoreInitialSettings();
bool isBoardInitialSettingsSaved() bool isBoardInitialSettingsSaved()
{ {
return m_boardInitialSettings.statusSaved; return m_memento.statusSaved;
} }
void clearBoardInitialSettingsSaved() void clearBoardInitialSettingsSaved()
{ {
m_boardInitialSettings.statusSaved = false; m_memento.statusSaved = false;
} }
thermalCalibrationBoardSettings m_boardInitialSettings; Memento m_memento;
thermalCalibrationResults m_results; thermalCalibrationResults m_results;
float getTemperature();
void setMetadataForCalibration(UAVDataObject *uavo); void setMetadataForCalibration(UAVDataObject *uavo);
UAVObjectManager *getObjectManager(); UAVObjectManager *getObjectManager();
}; };

View File

@ -210,7 +210,6 @@ public slots:
wizardStopped(); wizardStopped();
} }
}; };
} }
#endif // THERMALCALIBRATIONMODEL_H #endif // THERMALCALIBRATIONMODEL_H