explicit InstructionSet(const Range& range) {
addInstructions(range);
}
/* helper methods */
void ThermalCalibrationHelper::updateTemperature(float temp)
{
int elapsed = m_startTime.secsTo(QTime::currentTime());
int secondsSinceLastCheck = m_lastCheckpointTime.secsTo(QTime::currentTime());
// temperature is low pass filtered
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;
}
if (!m_rangeReached && (range() >= TargetTempDelta)) {
m_rangeReached = true;
addInstructions(tr("Target temperature span has been acquired. Acquisition may be ended or, preferably, continued."));
}
emit temperatureRangeChanged(range());
if (secondsSinceLastCheck > TimeBetweenCheckpoints) {
// gradient is expressed in °C/min
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;
m_lastCheckpointTime = QTime::currentTime();
m_lastCheckpointTemp = m_temperature;
}
// at least a checkpoint has been reached
if (elapsed > TimeBetweenCheckpoints) {
// .1 is a "very" small value in this case thats > 0
if (m_initialGradient < .1 && m_gradient > .1) {
m_initialGradient = m_gradient;
}
if (m_targetduration != 0) {
int tmp = (100 * elapsed) / m_targetduration;
setProgress(tmp);
} else if ((m_gradient > .1) && ((m_initialGradient / 2.0f) > m_gradient)) {
// make a rough estimation of the time needed
m_targetduration = elapsed * 8;
setProgressMax(100);
QTime time = QTime(0, 0).addSecs(m_targetduration);
QString timeString = time.toString(tr("m''s''''"));
addInstructions(tr("Estimated acquisition duration is %1.").arg(timeString));
QString str = QStringLiteral("INFO::Trace gradient : %1, elapsed : %2 initial gradient : %3, target : %4")
.arg(m_gradient).arg(elapsed).arg(m_initialGradient).arg(m_targetduration);
qDebug() << str;
m_debugStream << str << endl;
}
if ((m_gradient < TargetGradient) || m_forceStopAcquisition) {
m_acquiring = false;
emit collectionCompleted();
}
}
}
InstructionSet& operator+=(const Range& range) {
return addInstructions(range);
}
void ThermalCalibrationHelper::calculate()
{
// baro
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,
&m_results.baroInSigma, &m_results.baroOutSigma);
if (m_results.baroCalibrated) {
addInstructions(tr("Barometer is calibrated."));
} else {
qDebug() << "Failed to calibrate baro!";
addInstructions(tr("Failed to calibrate barometer!"), WizardModel::Warn);
}
m_results.baroTempMin = datat.array().minCoeff();
m_results.baroTempMax = datat.array().maxCoeff();
// gyro
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, m_results.gyroBias,
m_results.gyroInSigma, m_results.gyroOutSigma);
if (m_results.gyroCalibrated) {
addInstructions(tr("Gyro is calibrated."));
} else {
qDebug() << "Failed to calibrate gyro!";
addInstructions(tr("Failed to calibrate gyro!"), WizardModel::Warn);
}
// 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.
/*
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;
QString str = QStringLiteral("INFO::Calibration results") + "\n";
str += QStringLiteral("INFO::Baro cal {%1, %2, %3, %4}; initial variance: %5; Calibrated variance %6")
.arg(m_results.baro[0]).arg(m_results.baro[1]).arg(m_results.baro[2]).arg(m_results.baro[3])
.arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + "\n";
str += QStringLiteral("INFO::Gyro cal x{%1, %2, %3} y{%4, %5, %6} z{%7, %8, %9}; initial variance: {%10, %11, %12}; Calibrated variance {%13, %14, %15}")
.arg(m_results.gyroBias[0]).arg(m_results.gyro[0]).arg(m_results.gyro[1])
.arg(m_results.gyroBias[1]).arg(m_results.gyro[2]).arg(m_results.gyro[3])
.arg(m_results.gyroBias[2]).arg(m_results.gyro[4]).arg(m_results.gyro[5])
.arg(m_results.gyroInSigma[0]).arg(m_results.gyroInSigma[1]).arg(m_results.gyroInSigma[2])
.arg(m_results.gyroOutSigma[0]).arg(m_results.gyroOutSigma[1]).arg(m_results.gyroOutSigma[2]) + "\n";
str += QStringLiteral("INFO::Accel cal x{%1} y{%2} z{%3}; initial variance: {%4, %5, %6}; Calibrated variance {%7, %8, %9}")
.arg(m_results.accel[0]).arg(m_results.accel[1]).arg(m_results.accel[2])
.arg(m_results.accelInSigma[0]).arg(m_results.accelInSigma[1]).arg(m_results.accelInSigma[2])
.arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + "\n";
qDebug() << str;
m_debugStream << str;
emit calculationCompleted();
closeDebugLog();
}
