rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +01:00
Code Issues Releases Activity

OP-1531 thermal calibration now displays the acquired temperature range + cleaned up display and handling of temp and variance.

Browse Source
This commit is contained in:
Philippe Renon 2014-06-13 00:40:55 +02:00
parent bc7087da1a
commit 1e0028cc2f
7 changed files with 179 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -33,19 +33,21 @@
#include <math.h>
// #define SIMULATE
#define SIMULATE
namespace OpenPilot {
ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) :
QObject(parent)
{
m_tempdir.reset(new QTemporaryDir());
m_memento = Memento();
m_memento.statusSaved = false;
m_results = thermalCalibrationResults();
m_results.accelCalibrated = false;
m_results.baroCalibrated = false;
m_results.gyroCalibrated = false;
m_results.baroCalibrated = false;
m_progress = -1;
m_progressMax = -1;
@ -176,23 +178,29 @@ void ThermalCalibrationHelper::initAcquisition()
{
QMutexLocker lock(&sensorsUpdateLock);
m_targetduration = 0;
m_gradient = 0.0f;
m_initialGradient = m_gradient;
m_forceStopAcquisition = false;
m_acquiring = true;
// Clear all samples
m_accelSamples.clear();
m_gyroSamples.clear();
m_baroSamples.clear();
m_magSamples.clear();
m_results.accelCalibrated = false;
m_results.gyroCalibrated = false;
m_results.baroCalibrated = false;
// retrieve current temperature/time as initial checkpoint.
m_lastCheckpointTime = QTime::currentTime();
m_startTime = m_lastCheckpointTime;
m_lastCheckpointTemp = m_temperature = getTemperature();
m_startTime = m_lastCheckpointTime = QTime::currentTime();
m_temperature = getTemperature();
m_lastCheckpointTemp = m_temperature;
m_minTemperature = m_temperature;
m_maxTemperature = m_temperature;
m_gradient = 0;
m_initialGradient = 0;
m_targetduration = 0;
m_forceStopAcquisition = false;
m_acquiring = true;
createDebugLog();
connectUAVOs();
@ -208,18 +216,15 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
switch (sample->getObjID()) {
case AccelSensor::OBJID:
{
m_accelSamples.append(accelSensor->getData());
m_debugStream << "ACCEL:: " << m_accelSamples.last().temperature
<< "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz")
<< "\t" << m_accelSamples.last().x
<< "\t" << m_accelSamples.last().y
<< "\t" << m_accelSamples.last().z << endl;
break;
}
case GyroSensor::OBJID:
{
m_gyroSamples.append(gyroSensor->getData());
m_debugStream << "GYRO:: " << m_gyroSamples.last().temperature
<< "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz")
@ -227,7 +232,7 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
<< "\t" << m_gyroSamples.last().y
<< "\t" << m_gyroSamples.last().z << endl;
break;
}
case BaroSensor::OBJID:
{
float temp = getTemperature();
@ -242,23 +247,21 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample)
<< "\t" << m_baroSamples.last().Pressure
<< "\t" << m_baroSamples.last().Altitude << endl;
// must be done last as this call might end acquisition and close the debug log file
updateTemp(temp);
updateTemperature(temp);
break;
}
case MagSensor::OBJID:
{
m_magSamples.append(magSensor->getData());
m_debugStream << "MAG:: " << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz")
<< "\t" << m_magSamples.last().x
<< "\t" << m_magSamples.last().y
<< "\t" << m_magSamples.last().z << endl;
break;
}
default:
{
qDebug() << "Unexpected object" << sample->getObjID();
}
}
}
float ThermalCalibrationHelper::getTemperature()
@ -269,7 +272,7 @@ float ThermalCalibrationHelper::getTemperature()
// See http://en.wikipedia.org/wiki/Newton%27s_law_of_cooling#Newton.27s_law_of_cooling
// Initial temp : 20
// Final temp : 40
// Made up time constant (t0) : 10.0
// Time constant (t0) : 10.0
// For a plot of the function, see http://fooplot.com/#W3sidHlwZSI6MCwiZXEiOiI0MC0yMCplXigteC8xMCkiLCJjb2xvciI6IiMwMDAwMDAifSx7InR5cGUiOjEwMDAsIndpbmRvdyI6WyItOTIuMjAzMjYzMTkzMzY4ODMiLCI5Ni45NzE2MzQ3NzU0MDAwOCIsIi00NC4zNzkzODMzMjU1NzY3NTQiLCI3Mi4wMzU5Mzg1MDEzNTc5OSJdfV0-
double t0 = 10.0;
return 40.0 - 20.0 * exp(-t / t0);
@ -294,6 +297,7 @@ void ThermalCalibrationHelper::cleanup()
void ThermalCalibrationHelper::calculate()
{
// baro
int count = m_baroSamples.count();
Eigen::VectorXf datax(count);
Eigen::VectorXf datay(1);
@ -314,6 +318,7 @@ void ThermalCalibrationHelper::calculate()
m_results.baroTempMin = datat.array().minCoeff();
m_results.baroTempMax = datat.array().maxCoeff();
// gyro
count = m_gyroSamples.count();
datax.resize(count);
datay.resize(count);
@ -332,6 +337,8 @@ void ThermalCalibrationHelper::calculate()
if (!m_results.gyroCalibrated) {
qDebug() << "Failed to calibrate gyro!";
}
// accel
m_results.accelGyroTempMin = datat.array().minCoeff();
m_results.accelGyroTempMax = datat.array().maxCoeff();
// TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable.
@ -373,7 +380,7 @@ void ThermalCalibrationHelper::calculate()
}
/* helper methods */
void ThermalCalibrationHelper::updateTemp(float temp)
void ThermalCalibrationHelper::updateTemperature(float temp)
{
int elapsed = m_startTime.secsTo(QTime::currentTime());
int secondsSinceLastCheck = m_lastCheckpointTime.secsTo(QTime::currentTime());
@ -382,14 +389,22 @@ void ThermalCalibrationHelper::updateTemp(float temp)
m_temperature = m_temperature * 0.95f + temp * 0.05f;
emit temperatureChanged(m_temperature);
// temperature range
if (m_temperature < m_minTemperature) {
m_minTemperature = m_temperature;
}
if (m_temperature > m_maxTemperature) {
m_maxTemperature = m_temperature;
}
emit temperatureRangeChanged(range());
if (secondsSinceLastCheck > TimeBetweenCheckpoints) {
// gradient is expressed in °C/min
float gradient = 60.0 * (m_temperature - m_lastCheckpointTemp) / (float)secondsSinceLastCheck;
m_gradient = gradient;
emit temperatureGradientChanged(gradient);
m_gradient = 60.0 * (m_temperature - m_lastCheckpointTemp) / (float)secondsSinceLastCheck;
emit temperatureGradientChanged(gradient());
qDebug() << "Temp Gradient " << gradient << " Elapsed" << elapsed;
m_debugStream << "INFO::Trace Temp Gradient " << gradient << " Elapsed" << elapsed << endl;
qDebug() << "Temp Gradient " << gradient() << " Elapsed" << elapsed;
m_debugStream << "INFO::Trace Temp Gradient " << gradient() << " Elapsed" << elapsed << endl;
m_lastCheckpointTime = QTime::currentTime();
m_lastCheckpointTemp = m_temperature;
@ -411,7 +426,7 @@ void ThermalCalibrationHelper::updateTemp(float temp)
setProgressMax(100);
QTime time = QTime(0, 0).addSecs(m_targetduration);
QString timeString = time.toString(tr("m' minute(s) and 's' second(s).'"));
QString timeString = time.toString(tr("m'''s''''"));
addInstructions(tr("Estimated duration: %1").arg(timeString));
QString str = QStringLiteral("INFO::Trace gradient : %1, elapsed : %2 initial gradient : %3, target : %4")

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

@ -104,6 +104,11 @@ public:
return m_gradient;
}
float range()
{
return fabs(m_maxTemperature - m_minTemperature);
}
int processPercentage()
{
return m_progress;
@ -113,23 +118,43 @@ public:
bool calibrationSuccessful()
{
return m_results.baroCalibrated && ((m_results.baroTempMax - m_results.baroTempMin) > TargetTempDelta);
return (range() > TargetTempDelta) && baroCalibrationSuccessful();
}
bool baroCalibrationSuccessful()
{
return m_results.baroCalibrated;
}
bool gyroCalibrationSuccessful()
{
return m_results.gyroCalibrated;
}
bool accelCalibrationSuccessful()
{
return m_results.accelCalibrated;
}
void copyResultToSettings();
signals:
void instructionsAdded(QString text, WizardModel::MessageType type = WizardModel::Info);
void statusRestoreCompleted(bool succesful);
void statusSaveCompleted(bool succesful);
void setupBoardCompleted(bool succesful);
void temperatureChanged(float value);
void temperatureGradientChanged(float value);
void progressChanged(int value);
void progressMaxChanged(int value);
void collectionCompleted();
void calculationCompleted();
void temperatureChanged(float temperature);
void temperatureGradientChanged(float temperatureGradient);
void temperatureRangeChanged(float temperatureRange);
void progressChanged(int value);
void progressMaxChanged(int value);
void instructionsAdded(QString text, WizardModel::MessageType type = WizardModel::Info);
public slots:
/**
* @brief statusSave save the initial board status/configuration to be restored later
@ -181,7 +206,9 @@ public slots:
void cleanup();
private:
void updateTemp(float temp);
float getTemperature();
void updateTemperature(float temp);
void connectUAVOs();
void disconnectUAVOs();
@ -198,17 +225,24 @@ private:
QList<BaroSensor::DataFields> m_baroSamples;
QList<MagSensor::DataFields> m_magSamples;
QTime m_startTime;
// temperature checkpoints, used to calculate temp gradient
const static int TimeBetweenCheckpoints = 10;
QTime m_lastCheckpointTime;
bool m_acquiring;
bool m_forceStopAcquisition;
QTime m_startTime;
QTime m_lastCheckpointTime;
float m_lastCheckpointTemp;
float m_gradient;
float m_temperature;
float m_minTemperature;
float m_maxTemperature;
float m_gradient;
float m_initialGradient;
int m_targetduration;
int m_progress;
int m_progressMax;
@ -238,7 +272,6 @@ private:
Memento m_memento;
thermalCalibrationResults m_results;
float getTemperature();
void setMetadataForCalibration(UAVDataObject *uavo);
UAVObjectManager *getObjectManager();
};

18
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp
View File

@ -38,7 +38,6 @@ ThermalCalibrationModel::ThermalCalibrationModel(QObject *parent) :
m_startEnabled(false),
m_cancelEnabled(false),
m_endEnabled(false),
m_initDone(false),
m_dirty(false)
{
m_helper.reset(new ThermalCalibrationHelper());
@ -63,6 +62,7 @@ ThermalCalibrationModel::ThermalCalibrationModel(QObject *parent) :
connect(m_helper.data(), SIGNAL(temperatureChanged(float)), this, SLOT(setTemperature(float)));
connect(m_helper.data(), SIGNAL(temperatureGradientChanged(float)), this, SLOT(setTemperatureGradient(float)));
connect(m_helper.data(), SIGNAL(temperatureRangeChanged(float)), this, SLOT(setTemperatureRange(float)));
connect(m_helper.data(), SIGNAL(progressChanged(int)), this, SLOT(setProgress(int)));
connect(m_helper.data(), SIGNAL(progressMaxChanged(int)), this, SLOT(setProgressMax(int)));
connect(m_helper.data(), SIGNAL(instructionsAdded(QString, WizardModel::MessageType)), this, SLOT(addInstructions(QString, WizardModel::MessageType)));
@ -78,15 +78,13 @@ ThermalCalibrationModel::ThermalCalibrationModel(QObject *parent) :
void ThermalCalibrationModel::init()
{
if (!m_initDone) {
m_initDone = true;
setStartEnabled(true);
setEndEnabled(false);
setCancelEnabled(false);
setTemperature(0);
setTemperatureGradient(0);
start();
}
setStartEnabled(true);
setEndEnabled(false);
setCancelEnabled(false);
setTemperature(NAN);
setTemperatureGradient(NAN);
setTemperatureRange(NAN);
start();
}
void ThermalCalibrationModel::stepChanged(WizardState *state)

86
ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h
View File

@ -44,6 +44,7 @@ class ThermalCalibrationModel : public WizardModel {
Q_PROPERTY(bool cancelEnable READ cancelEnabled NOTIFY cancelEnabledChanged)
Q_PROPERTY(float temperature READ temperature NOTIFY temperatureChanged)
Q_PROPERTY(float temperatureGradient READ temperatureGradient NOTIFY temperatureGradientChanged)
Q_PROPERTY(float temperatureRange READ temperatureRange NOTIFY temperatureRangeChanged)
Q_PROPERTY(int progress READ progress WRITE setProgress NOTIFY progressChanged)
Q_PROPERTY(int progressMax READ progressMax WRITE setProgressMax NOTIFY progressMaxChanged)
@ -89,6 +90,21 @@ public:
}
}
float temperature()
{
return m_helper->temperature();
}
float temperatureGradient()
{
return m_helper->gradient();
}
float temperatureRange()
{
return m_helper->range();
}
bool dirty()
{
return m_dirty;
@ -96,48 +112,37 @@ public:
public slots:
void setTemperature(float temperature)
{
emit temperatureChanged(temperature);
}
void setTemperatureGradient(float temperatureGradient)
{
emit temperatureGradientChanged(temperatureGradient);
}
void setTemperatureRange(float temperatureRange)
{
emit temperatureRangeChanged(temperatureRange);
}
int progress()
{
return m_progress;
}
int progressMax()
{
return m_progressMax;
}
float temperature()
{
return m_temperature;
}
float temperatureGradient()
{
return m_temperatureGradient;
}
void setTemperature(float temp)
{
if (m_temperature != temp) {
m_temperature = temp;
emit temperatureChanged(m_temperature);
}
}
void setTemperatureGradient(float tempGradient)
{
if (m_temperatureGradient != tempGradient) {
m_temperatureGradient = tempGradient;
emit temperatureGradientChanged(m_temperatureGradient);
}
}
void setProgress(int value)
{
m_progress = value;
emit progressChanged(value);
}
int progressMax()
{
return m_progressMax;
}
void setProgressMax(int value)
{
m_progressMax = value;
@ -145,17 +150,15 @@ public slots:
}
private:
QScopedPointer<ThermalCalibrationHelper> m_helper;
bool m_startEnabled;
bool m_cancelEnabled;
bool m_endEnabled;
bool m_initDone;
bool m_dirty;
int m_progress;
int m_progressMax;
float m_temperature;
float m_temperatureGradient;
QScopedPointer<ThermalCalibrationHelper> m_helper;
// Start from here
WizardState *m_readyState;
@ -188,8 +191,10 @@ signals:
void wizardStarted();
void wizardStopped();
void temperatureChanged(float temp);
void temperatureGradientChanged(float tempGradient);
void temperatureChanged(float temperature);
void temperatureGradientChanged(float temperatureGradient);
void temperatureRangeChanged(float temperatureRange);
void progressChanged(int value);
void progressMaxChanged(int value);
@ -202,10 +207,7 @@ public slots:
void init();
void btnStart()
{
// HACKS
// clear instructions
emit temperatureGradientChanged(0);
// END OF HACKS
init();
emit next();
}
void btnEnd()

39
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -54,7 +54,7 @@
#include <QDesktopServices>
#include <QUrl>
#define sign(x) ((x < 0) ? -1 : 1)
#include <math.h>
//#define DEBUG
@ -157,10 +157,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
// thermal calibration
m_thermalCalibrationModel = new OpenPilot::ThermalCalibrationModel(this);
m_thermalCalibrationModel->init();
m_ui->temperatureLabel->setText("");
m_ui->temperatureGradientLabel->setText("");
connect(m_ui->thermalBiasStart, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnStart()));
connect(m_ui->thermalBiasEnd, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnEnd()));
connect(m_ui->thermalBiasCancel, SIGNAL(clicked()), m_thermalCalibrationModel, SLOT(btnAbort()));
@ -175,10 +171,10 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
this, SLOT(addInstructions(QString, WizardModel::MessageType)));
connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(float)), this, SLOT(displayTemperature(float)));
connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(float)), this, SLOT(displayTemperatureGradient(float)));
connect(m_thermalCalibrationModel, SIGNAL(temperatureRangeChanged(float)), this, SLOT(displayTemperatureRange(float)));
connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int)));
connect(m_thermalCalibrationModel, SIGNAL(progressMaxChanged(int)), m_ui->thermalBiasProgress, SLOT(setMaximum(int)));
m_ui->thermalBiasEnd->setEnabled(false);
m_ui->thermalBiasCancel->setEnabled(false);
m_thermalCalibrationModel->init();
// home location
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
@ -304,14 +300,35 @@ void ConfigRevoWidget::addInstructions(QString text, WizardModel::MessageType ty
}
}
void ConfigRevoWidget::displayTemperature(float temp)
static QString format(float v)
{
m_ui->temperatureLabel->setText(tr("Temperature %1 °C").arg(temp, 5, 'f', 2));
QString str;
if (!std::isnan(v)) {
// format as ##.##
str = QString("%1").arg(v, 5, 'f', 2, ' ');
str = str.replace(" ", "&nbsp;");
} else {
str = "--.--";
}
// use a fixed width font
QString style("font-family:courier new,monospace;");
return QString("<span style=\"%1\">%2</span>").arg(style).arg(str);
}
void ConfigRevoWidget::displayTemperatureGradient(float tempGradient)
void ConfigRevoWidget::displayTemperature(float temperature)
{
m_ui->temperatureGradientLabel->setText(tr("Temperature rise %1 °C/min").arg(tempGradient, 5, 'f', 2));
m_ui->temperatureLabel->setText(tr("Temperature: %1 °C").arg(format(temperature)));
}
void ConfigRevoWidget::displayTemperatureGradient(float temperatureGradient)
{
m_ui->temperatureGradientLabel->setText(tr("Variance: %1 °C/min").arg(format(temperatureGradient)));
}
void ConfigRevoWidget::displayTemperatureRange(float temperatureRange)
{
m_ui->temperatureRangeLabel->setText(tr("Acquisition range: %1 °C").arg(format(temperatureRange)));
}
/**

5
ground/openpilotgcs/src/plugins/config/configrevowidget.h
View File

@ -71,8 +71,9 @@ private slots:
void displayVisualHelp(QString elementID);
void clearInstructions();
void addInstructions(QString text, WizardModel::MessageType type = WizardModel::Info);
void displayTemperature(float temp);
void displayTemperatureGradient(float tempGradient);
void displayTemperature(float tempareture);
void displayTemperatureGradient(float temparetureGradient);
void displayTemperatureRange(float temparetureRange);
// ! Overriden method from the configTaskWidget to update UI
virtual void refreshWidgetsValues(UAVObject *object = NULL);

9
ground/openpilotgcs/src/plugins/config/revosensors.ui
View File

@ -375,7 +375,14 @@
</sizepolicy>
</property>
<property name="text">
<string>&lt;temperature gradient&gt;</string>
<string>&lt;gradient&gt;</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="temperatureRangeLabel">
<property name="text">
<string>&lt;range&gt;</string>
</property>
</widget>
</item>