diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h index 84a00762c..741fe6c20 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h @@ -54,8 +54,12 @@ public: Q_UNUSED(e); QString nextStateName; if (m_helper->calibrationSuccessful()) { + m_helper->setProgressMax(100); + m_helper->setProgress(100); m_helper->addInstructions(tr("Calibration completed successfully."), WizardModel::Success); } else { + m_helper->setProgressMax(100); + m_helper->setProgress(0); m_helper->addInstructions(tr("Calibration failed! Please read the instructions and retry."), WizardModel::Failure); } } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h index dabca0e96..d3bb4b45b 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h @@ -57,6 +57,8 @@ public: public slots: void enterState() { + m_helper->setProgressMax(0); + m_helper->setProgress(0); m_helper->addInstructions(tr("Please wait during samples acquisition. This can take several minutes..."), WizardModel::Prompt); m_helper->initAcquisition(); } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp index cb2fec593..15e0f7dcc 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp @@ -45,12 +45,15 @@ ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) : m_results.baroCalibrated = false; m_results.gyroCalibrated = false; - accelSensor = AccelSensor::GetInstance(getObjectManager()); - gyroSensor = GyroSensor::GetInstance(getObjectManager()); - baroSensor = BaroSensor::GetInstance(getObjectManager()); - magSensor = MagSensor::GetInstance(getObjectManager()); + m_progress = -1; + m_progressMax = -1; + + accelSensor = AccelSensor::GetInstance(getObjectManager()); + gyroSensor = GyroSensor::GetInstance(getObjectManager()); + baroSensor = BaroSensor::GetInstance(getObjectManager()); + magSensor = MagSensor::GetInstance(getObjectManager()); accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); - revoSettings = RevoSettings::GetInstance(getObjectManager()); + revoSettings = RevoSettings::GetInstance(getObjectManager()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); @@ -105,7 +108,6 @@ bool ThermalCalibrationHelper::saveBoardInitialSettings() m_memento.accelSensorMeta = accelSensor->getMetadata(); m_memento.gyroSensorMeta = gyroSensor->getMetadata(); m_memento.baroensorMeta = baroSensor->getMetadata(); - m_memento.accelGyroSettings = accelGyroSettings->getData(); m_memento.revoSettings = revoSettings->getData(); @@ -137,12 +139,10 @@ bool ThermalCalibrationHelper::restoreInitialSettings() return true; } - /* Methods called from transitions */ void ThermalCalibrationHelper::setupBoard() { - setProgress(ProcessPercentageSetupBoard); if (setupBoardForCalibration()) { emit setupBoardCompleted(true); } else { @@ -162,7 +162,6 @@ void ThermalCalibrationHelper::statusRestore() void ThermalCalibrationHelper::statusSave() { - setProgress(ProcessPercentageSaveSettings); // prevent saving multiple times if (!isBoardInitialSettingsSaved() && saveBoardInitialSettings()) { emit statusSaveCompleted(true); @@ -173,8 +172,8 @@ void ThermalCalibrationHelper::statusSave() void ThermalCalibrationHelper::initAcquisition() { - setProgress(ProcessPercentageBaseAcquisition); QMutexLocker lock(&sensorsUpdateLock); + m_targetduration = 0; m_gradient = 0.0f; m_initialGradient = m_gradient; @@ -209,22 +208,22 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample) 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; + 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") << - "\t" << m_gyroSamples.last().x << - "\t" << m_gyroSamples.last().y << - "\t" << m_gyroSamples.last().z << endl; + m_debugStream << "GYRO:: " << m_gyroSamples.last().temperature + << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") + << "\t" << m_gyroSamples.last().x + << "\t" << m_gyroSamples.last().y + << "\t" << m_gyroSamples.last().z << endl; break; } case BaroSensor::OBJID: @@ -233,13 +232,13 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample) BaroSensor::DataFields data = baroSensor->getData(); #ifdef SIMULATE data.Temperature = temp; + data.Pressure += 10.0f * temp; #endif m_baroSamples.append(data); - m_debugStream << "BARO:: " << m_baroSamples.last().Temperature << - "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") << - "\t" << m_baroSamples.last().Pressure << - "\t" << m_baroSamples.last().Altitude << endl; - // this is needed as temperature is low pass filtered + m_debugStream << "BARO:: " << m_baroSamples.last().Temperature + << "\t" << QDateTime::currentDateTime().toString("hh.mm.ss.zzz") + << "\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); break; @@ -247,11 +246,10 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample) 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; + 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: @@ -288,8 +286,6 @@ void ThermalCalibrationHelper::cleanup() void ThermalCalibrationHelper::calculate() { - setProgress(ProcessPercentageBaseCalculation); - int count = m_baroSamples.count(); Eigen::VectorXf datax(count); Eigen::VectorXf datay(1); @@ -301,14 +297,15 @@ void ThermalCalibrationHelper::calculate() datat[x] = m_baroSamples[x].Temperature; } - 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.baroTempMax = datat.array().maxCoeff(); - setProgress(processPercentage() + 2); + count = m_gyroSamples.count(); datax.resize(count); datay.resize(count); @@ -322,7 +319,8 @@ void ThermalCalibrationHelper::calculate() datat[x] = m_gyroSamples[x].temperature; } - 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!"; } @@ -330,7 +328,6 @@ void ThermalCalibrationHelper::calculate() m_results.accelGyroTempMax = datat.array().maxCoeff(); // TODO: sanity checks needs to be enforced before accel calibration can be enabled and usable. /* - setProgress(processPercentage() + 2); count = m_accelSamples.count(); datax.resize(count); datay.resize(count); @@ -338,43 +335,42 @@ void ThermalCalibrationHelper::calculate() 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; + 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; - str += QStringLiteral("INFO::Calibration results"); - + 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) + QChar::CarriageReturn; + .arg(m_results.baroInSigma).arg(m_results.baroOutSigma) + "\n"; str += QStringLiteral("INFO::Gyro cal x{%1, %2} y{%3, %4} z{%5, %6}; initial variance: {%7, %8, %9}; Calibrated variance {%10, %11, %12}") - .arg(m_results.gyro[0]).arg(m_results.gyro[1]).arg(m_results.gyro[2]) - .arg(m_results.gyro[3]).arg(m_results.gyro[4]).arg(m_results.gyro[5]) + .arg(m_results.gyro[0]).arg(m_results.gyro[1]) + .arg(m_results.gyro[2]).arg(m_results.gyro[3]) + .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]) + QChar::CarriageReturn; + .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]) + QChar::CarriageReturn; + .arg(m_results.accelOutSigma[0]).arg(m_results.accelOutSigma[1]).arg(m_results.accelOutSigma[2]) + "\n"; qDebug() << str; m_debugStream << str; emit calculationCompleted(); closeDebugLog(); } - /* helper methods */ void ThermalCalibrationHelper::updateTemp(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); @@ -386,6 +382,7 @@ void ThermalCalibrationHelper::updateTemp(float temp) qDebug() << "Temp Gradient " << gradient << " Elapsed" << elapsed; m_debugStream << "INFO::Trace Temp Gradient " << gradient << " Elapsed" << elapsed << endl; + m_lastCheckpointTime = QTime::currentTime(); m_lastCheckpointTemp = m_temperature; } @@ -397,18 +394,19 @@ void ThermalCalibrationHelper::updateTemp(float temp) } if (m_targetduration != 0) { - int tmp = ProcessPercentageBaseAcquisition + ((ProcessPercentageBaseCalculation - ProcessPercentageBaseAcquisition) * elapsed) / m_targetduration; - tmp = tmp > ProcessPercentageBaseCalculation - 5 ? ProcessPercentageBaseCalculation - 5 : tmp; + int tmp = (100 * elapsed) / m_targetduration; setProgress(tmp); - } else if (m_gradient > .1 && m_initialGradient / 2.0f > m_gradient) { - qDebug() << "M_gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient; + } else if ((m_gradient > .1) && ((m_initialGradient / 2.0f) > m_gradient)) { // make a rough estimation of the time needed m_targetduration = elapsed * 8; - m_debugStream << "INFO::Trace gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient - << " target:" << m_targetduration << endl; + setProgressMax(100); + 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) { + if ((m_gradient < TargetGradient) || m_forceStopAcquisition) { m_acquiring = false; emit collectionCompleted(); } @@ -452,18 +450,20 @@ void ThermalCalibrationHelper::createDebugLog() UAVObjectUtilManager *utilMngr = pm->getObject(); deviceDescriptorStruct board = utilMngr->getBoardDescriptionStruct(); - m_debugStream << "INFO::Hardware"; - m_debugStream << " type:" << QString().setNum(board.boardType, 16); - m_debugStream << " revision:" << QString().setNum(board.boardRevision, 16); - m_debugStream << " serial:" << QString(utilMngr->getBoardCPUSerial().toHex()) << endl; + m_debugStream << "INFO::Hardware" << " type:" << QString().setNum(board.boardType, 16) + << " revision:" << QString().setNum(board.boardRevision, 16) + << " serial:" << QString(utilMngr->getBoardCPUSerial().toHex()) << endl; QString uavo = board.uavoHash.toHex(); - m_debugStream << "INFO::firmware tag:" << board.gitTag << " date:" << board.gitDate << " hash:" << board.gitHash << - "uavo:" << uavo.left(8) << endl; + m_debugStream << "INFO::firmware tag:" << board.gitTag + << " date:" << board.gitDate + << " hash:" << board.gitHash + << " uavo:" << uavo.left(8) << endl; - - m_debugStream << "INFO::gcs tag:" << VersionInfo::tagOrBranch() + VersionInfo::dirty() << " date:" << VersionInfo::dateTime() << - " hash:" << VersionInfo::hash().left(8) << " uavo:" << VersionInfo::uavoHash().left(8) << endl; + m_debugStream << "INFO::gcs tag:" << VersionInfo::tagOrBranch() + VersionInfo::dirty() + << " date:" << VersionInfo::dateTime() + << " hash:" << VersionInfo::hash().left(8) + << " uavo:" << VersionInfo::uavoHash().left(8) << endl; } } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h index a747c8d02..1272d804d 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h @@ -106,7 +106,7 @@ public: int processPercentage() { - return m_processPercentage; + return m_progress; } void endAcquisition(); @@ -126,9 +126,9 @@ signals: void temperatureChanged(float value); void temperatureGradientChanged(float value); void progressChanged(int value); + void progressMaxChanged(int value); void collectionCompleted(); void calculationCompleted(); - void abort(); public slots: /** @@ -162,11 +162,16 @@ public slots: void collectSample(UAVObject *sample); void setProgress(int value) { - if (m_processPercentage != value) { - m_processPercentage = value; + if (m_progress != value) { + m_progress = value; emit progressChanged(value); } } + void setProgressMax(int value) + { + m_progressMax = value; + emit progressMaxChanged(value); + } void addInstructions(QString text, WizardModel::MessageType type = WizardModel::Info) { @@ -203,15 +208,12 @@ private: float m_gradient; float m_temperature; float m_initialGradient; - 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; + int m_targetduration; + int m_progress; + int m_progressMax; + const static float TargetGradient = 0.20f; const static float TargetTempDelta = 10.0f; - int m_targetduration; - int m_processPercentage; // convenience pointers AccelSensor *accelSensor; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp index e457979c0..c6bc240ca 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp @@ -38,7 +38,8 @@ ThermalCalibrationModel::ThermalCalibrationModel(QObject *parent) : m_startEnabled(false), m_cancelEnabled(false), m_endEnabled(false), - m_initDone(false) + m_initDone(false), + m_dirty(false) { m_helper.reset(new ThermalCalibrationHelper()); @@ -63,6 +64,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(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))); connect(m_readyState, SIGNAL(entered()), this, SLOT(stopWizard())); connect(m_readyState, SIGNAL(exited()), this, SLOT(startWizard())); @@ -95,10 +97,10 @@ void ThermalCalibrationModel::stepChanged(WizardState *state) void ThermalCalibrationModel::setTransitions() { m_readyState->addTransition(this, SIGNAL(next()), m_workingState); + m_readyState->assignProperty(this, "progressMax", 100); m_readyState->assignProperty(this, "progress", 0); m_completedState->addTransition(this, SIGNAL(next()), m_workingState); - m_completedState->assignProperty(this, "progress", 100); // handles board initial status save // Ready->WorkingState->saveSettings->setup @@ -115,6 +117,7 @@ void ThermalCalibrationModel::setTransitions() // abort causes initial settings to be restored and acquisition stopped. m_abortState->addTransition(new BoardStatusRestoreTransition(m_helper.data(), m_abortState, m_readyState)); + m_workingState->addTransition(this, SIGNAL(abort()), m_abortState); // Ready } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h index 30b78e3b8..b6701aa6f 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h @@ -45,6 +45,7 @@ class ThermalCalibrationModel : public WizardModel { Q_PROPERTY(float temperature READ temperature NOTIFY temperatureChanged) Q_PROPERTY(float temperatureGradient READ temperatureGradient NOTIFY temperatureGradientChanged) Q_PROPERTY(int progress READ progress WRITE setProgress NOTIFY progressChanged) + Q_PROPERTY(int progressMax READ progressMax WRITE setProgressMax NOTIFY progressMaxChanged) public: explicit ThermalCalibrationModel(QObject *parent = 0); @@ -100,6 +101,11 @@ public slots: return m_progress; } + int progressMax() + { + return m_progressMax; + } + float temperature() { return m_temperature; @@ -126,10 +132,16 @@ public slots: } } - void setProgress(int progress) + void setProgress(int value) { - m_progress = progress; - emit progressChanged(progress); + m_progress = value; + emit progressChanged(value); + } + + void setProgressMax(int value) + { + m_progressMax = value; + emit progressMaxChanged(value); } private: @@ -139,6 +151,7 @@ private: bool m_initDone; bool m_dirty; int m_progress; + int m_progressMax; float m_temperature; float m_temperatureGradient; @@ -178,6 +191,7 @@ signals: void temperatureChanged(float temp); void temperatureGradientChanged(float tempGradient); void progressChanged(int value); + void progressMaxChanged(int value); void next(); void previous(); diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 2b68e8e3d..504f511ff 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -174,6 +174,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(float)), this, SLOT(displayTemperature(float))); connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(float)), this, SLOT(displayTemperatureGradient(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);