diff --git a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp index 1d35f0eef..4e1c9ab31 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp @@ -25,52 +25,61 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include -#include -#include "extensionsystem/pluginmanager.h" -#include -#include -#include -#include #include "calibration/gyrobiascalibrationmodel.h" #include "calibration/calibrationutils.h" #include "calibration/calibrationuiutils.h" +#include "extensionsystem/pluginmanager.h" static const int LEVEL_SAMPLES = 100; -#include "gyrobiascalibrationmodel.h" + namespace OpenPilot { GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) : - QObject(parent), - collectingData(false) -{} + QObject(parent), collectingData(false), m_dirty(false) +{ + gyroState = GyroState::GetInstance(getObjectManager()); + Q_ASSERT(gyroState); + + gyroSensor = GyroSensor::GetInstance(getObjectManager()); + Q_ASSERT(gyroSensor); + + revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + + attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); + Q_ASSERT(attitudeSettings); + + accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); + Q_ASSERT(accelGyroSettings); +} -/******* gyro bias zero ******/ void GyroBiasCalibrationModel::start() { // Store and reset board rotation before calibration starts storeAndClearBoardRotation(); - disableAllCalibrations(); - progressChanged(0); - - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - Q_ASSERT(revoCalibration); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + memento.revoCalibrationData = revoCalibrationData; revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE; revoCalibration->setData(revoCalibrationData); - revoCalibration->updated(); // Disable gyro bias correction while calibrating - AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); - Q_ASSERT(attitudeSettings); AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); + memento.attitudeSettingsData = attitudeSettingsData; attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE; attitudeSettings->setData(attitudeSettingsData); - attitudeSettings->updated(); - displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); - displayInstructions(tr("Calibrating the gyroscopes. Keep the copter/plane steady..."), true); + UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata(); + memento.gyroStateMetadata = gyroStateMetadata; + UAVObject::SetFlightTelemetryUpdateMode(gyroStateMetadata, UAVObject::UPDATEMODE_PERIODIC); + gyroStateMetadata.flightTelemetryUpdatePeriod = 100; + gyroState->setMetadata(gyroStateMetadata); + + UAVObject::Metadata gyroSensorMetadata = gyroSensor->getMetadata(); + memento.gyroSensorMetadata = gyroSensorMetadata; + UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC); + gyroSensorMetadata.flightTelemetryUpdatePeriod = 100; + gyroSensor->setMetadata(gyroSensorMetadata); gyro_accum_x.clear(); gyro_accum_y.clear(); @@ -80,24 +89,13 @@ void GyroBiasCalibrationModel::start() gyro_state_accum_y.clear(); gyro_state_accum_z.clear(); - UAVObject::Metadata metadata; + // reset dirty state to forget previous unsaved runs + m_dirty = false; - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - Q_ASSERT(gyroState); - initialGyroStateMdata = gyroState->getMetadata(); - metadata = initialGyroStateMdata; - UAVObject::SetFlightTelemetryUpdateMode(metadata, UAVObject::UPDATEMODE_PERIODIC); - metadata.flightTelemetryUpdatePeriod = 100; - gyroState->setMetadata(metadata); - - UAVObject::Metadata gyroSensorMetadata; - GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); - Q_ASSERT(gyroSensor); - initialGyroSensorMdata = gyroSensor->getMetadata(); - gyroSensorMetadata = initialGyroSensorMdata; - UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC); - gyroSensorMetadata.flightTelemetryUpdatePeriod = 100; - gyroSensor->setMetadata(gyroSensorMetadata); + started(); + progressChanged(0); + displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); + displayInstructions(tr("Calibrating the gyroscopes. Keep the vehicle steady...")); // Now connect to the accels and mag updates, gather for 100 samples collectingData = true; @@ -112,15 +110,10 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); - Q_UNUSED(lock); - switch (obj->getObjID()) { case GyroState::OBJID: { - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - Q_ASSERT(gyroState); GyroState::DataFields gyroStateData = gyroState->getData(); - gyro_state_accum_x.append(gyroStateData.x); gyro_state_accum_y.append(gyroStateData.y); gyro_state_accum_z.append(gyroStateData.z); @@ -128,10 +121,7 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) } case GyroSensor::OBJID: { - GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); - Q_ASSERT(gyroSensor); GyroSensor::DataFields gyroSensorData = gyroSensor->getData(); - gyro_accum_x.append(gyroSensorData.x); gyro_accum_y.append(gyroSensorData.y); gyro_accum_z.append(gyroSensorData.z); @@ -149,61 +139,48 @@ void GyroBiasCalibrationModel::getSample(UAVObject *obj) if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) && collectingData == true) { collectingData = false; + m_dirty = true; - GyroState *gyroState = GyroState::GetInstance(getObjectManager()); - disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); - Q_ASSERT(gyroState); - - GyroSensor *gyroSensor = GyroSensor::GetInstance(getObjectManager()); - Q_ASSERT(gyroSensor); disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); - enableAllCalibrations(); - - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - Q_ASSERT(revoCalibration); - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); - Q_ASSERT(accelGyroSettings); - - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); - - revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_TRUE; - // Update biases based on collected data - // check whether the board does supports gyroSensor(updates were received) - if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) { - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z); - } else { - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y); - accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z); - } - - revoCalibration->setData(revoCalibrationData); - revoCalibration->updated(); - accelGyroSettings->setData(accelGyroSettingsData); - accelGyroSettings->updated(); - - AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); - Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); - attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE; - attitudeSettings->setData(attitudeSettingsData); - attitudeSettings->updated(); - - gyroState->setMetadata(initialGyroStateMdata); - gyroSensor->setMetadata(initialGyroSensorMdata); - - displayInstructions(tr("Gyroscope calibration computed succesfully."), false); - displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY); + gyroState->setMetadata(memento.gyroStateMetadata); + gyroSensor->setMetadata(memento.gyroSensorMetadata); + revoCalibration->setData(memento.revoCalibrationData); + attitudeSettings->setData(memento.attitudeSettingsData); // Recall saved board rotation recallBoardRotation(); + + stopped(); + displayInstructions(tr("Gyroscope calibration completed successfully."), WizardModel::Success); + displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY); } } +void GyroBiasCalibrationModel::save() +{ + if (!m_dirty) { + return; + } + // Update biases based on collected data + AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + + // check whether the board does supports gyroSensor (updates were received) + if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) { + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z); + } else { + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y); + accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z); + } + + accelGyroSettings->setData(accelGyroSettingsData); + + m_dirty = false; +} UAVObjectManager *GyroBiasCalibrationModel::getObjectManager() { diff --git a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h index 14fa6c0eb..e979ca621 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h @@ -29,34 +29,61 @@ #ifndef GYROBIASCALIBRATIONMODEL_H #define GYROBIASCALIBRATIONMODEL_H -#include +#include "wizardmodel.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" + +#include +#include +#include +#include +#include + +#include + namespace OpenPilot { class GyroBiasCalibrationModel : public QObject { Q_OBJECT + public: explicit GyroBiasCalibrationModel(QObject *parent = 0); + bool dirty() + { + return m_dirty; + } + signals: - void displayVisualHelp(QString elementID); - void displayInstructions(QString instructions, bool replace); - void disableAllCalibrations(); - void enableAllCalibrations(); + void started(); + void stopped(); void storeAndClearBoardRotation(); void recallBoardRotation(); void progressChanged(int value); + void displayVisualHelp(QString elementID); + void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info); + public slots: - // Slots for gyro bias zero void start(); + void save(); + private slots: void getSample(UAVObject *obj); private: + typedef struct { + UAVObject::Metadata gyroStateMetadata; + UAVObject::Metadata gyroSensorMetadata; + RevoCalibration::DataFields revoCalibrationData; + AttitudeSettings::DataFields attitudeSettingsData; + } Memento; + QMutex sensorsUpdateLock; bool collectingData; + bool m_dirty; + + Memento memento; QList gyro_accum_x; QList gyro_accum_y; @@ -64,9 +91,16 @@ private: QList gyro_state_accum_x; QList gyro_state_accum_y; QList gyro_state_accum_z; - UAVObject::Metadata initialGyroStateMdata; - UAVObject::Metadata initialGyroSensorMdata; + + // convenience pointers + GyroState *gyroState; + GyroSensor *gyroSensor; + RevoCalibration *revoCalibration; + AttitudeSettings *attitudeSettings; + AccelGyroSettings *accelGyroSettings; + UAVObjectManager *getObjectManager(); }; } + #endif // GYROBIASCALIBRATIONMODEL_H diff --git a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp index fd46259de..b9dd59631 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp @@ -26,55 +26,58 @@ */ #include "levelcalibrationmodel.h" -#include -#include #include "extensionsystem/pluginmanager.h" #include "calibration/calibrationuiutils.h" +#include +#include + static const int LEVEL_SAMPLES = 100; + namespace OpenPilot { LevelCalibrationModel::LevelCalibrationModel(QObject *parent) : - QObject(parent) -{} + QObject(parent), m_dirty(false) +{ + attitudeState = AttitudeState::GetInstance(getObjectManager()); + Q_ASSERT(attitudeState); + + attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); + Q_ASSERT(attitudeSettings); +} -/******* Level calibration *******/ /** * Starts an accelerometer bias calibration. */ void LevelCalibrationModel::start() { - // Store and reset board rotation before calibration starts - - disableAllCalibrations(); - progressChanged(0); - - rot_data_pitch = 0; - rot_data_roll = 0; - UAVObject::Metadata mdata; - - AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); - Q_ASSERT(attitudeState); - initialAttitudeStateMdata = attitudeState->getMetadata(); - mdata = initialAttitudeStateMdata; + memento.attitudeStateMdata = attitudeState->getMetadata(); + UAVObject::Metadata mdata = attitudeState->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; attitudeState->setMetadata(mdata); - /* Show instructions and enable controls */ - displayInstructions(tr("Place horizontally and click Save Position button..."), true); - displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); - disableAllCalibrations(); - savePositionEnabledChanged(true); + rot_data_pitch = 0; + rot_data_roll = 0; + + // reset dirty state to forget previous unsaved runs + m_dirty = false; + position = 0; + + started(); + + // Show instructions and enable controls + progressChanged(0); + displayInstructions(tr("Place horizontally and press Save Position..."), WizardModel::Prompt); + displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED); + savePositionEnabledChanged(true); } void LevelCalibrationModel::savePosition() { QMutexLocker lock(&sensorsUpdateLock); - Q_UNUSED(lock); - savePositionEnabledChanged(false); rot_accum_pitch.clear(); @@ -82,11 +85,9 @@ void LevelCalibrationModel::savePosition() collectingData = true; - AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); - Q_ASSERT(attitudeState); connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); - displayInstructions(tr("Hold..."), false); + displayInstructions(tr("Hold...")); } /** @@ -96,13 +97,9 @@ void LevelCalibrationModel::getSample(UAVObject *obj) { QMutexLocker lock(&sensorsUpdateLock); - Q_UNUSED(lock); - switch (obj->getObjID()) { case AttitudeState::OBJID: { - AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); - Q_ASSERT(attitudeState); AttitudeState::DataFields attitudeStateData = attitudeState->getData(); rot_accum_roll.append(attitudeStateData.Roll); rot_accum_pitch.append(attitudeStateData.Pitch); @@ -130,11 +127,9 @@ void LevelCalibrationModel::getSample(UAVObject *obj) rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll); - displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and click Save Position button..."), false); + displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and press Save Position..."), WizardModel::Prompt); displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD); - disableAllCalibrations(); - savePositionEnabledChanged(true); break; case 2: @@ -142,21 +137,24 @@ void LevelCalibrationModel::getSample(UAVObject *obj) rot_data_pitch /= 2; rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll); rot_data_roll /= 2; - attitudeState->setMetadata(initialAttitudeStateMdata); - compute(); - enableAllCalibrations(); + + attitudeState->setMetadata(memento.attitudeStateMdata); + + m_dirty = true; + + stopped(); displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY); - displayInstructions(tr("Board leveling computed successfully."), false); + displayInstructions(tr("Board level calibration completed successfully."), WizardModel::Success); break; } } } -void LevelCalibrationModel::compute() -{ - enableAllCalibrations(); - AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); - Q_ASSERT(attitudeSettings); +void LevelCalibrationModel::save() +{ + if (!m_dirty) { + return; + } AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); // Update the biases based on collected data @@ -165,8 +163,10 @@ void LevelCalibrationModel::compute() attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll; attitudeSettings->setData(attitudeSettingsData); - attitudeSettings->updated(); + + m_dirty = false; } + UAVObjectManager *LevelCalibrationModel::getObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h index 07ce6ccc0..e79a1bb35 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h @@ -28,47 +28,67 @@ #ifndef LEVELCALIBRATIONMODEL_H #define LEVELCALIBRATIONMODEL_H +#include "wizardmodel.h" +#include "calibration/calibrationutils.h" +#include +#include + #include #include #include -#include "calibration/calibrationutils.h" -#include -#include -#include -#include -#include namespace OpenPilot { class LevelCalibrationModel : public QObject { Q_OBJECT + public: explicit LevelCalibrationModel(QObject *parent = 0); + bool dirty() + { + return m_dirty; + } + signals: - void displayVisualHelp(QString elementID); - void displayInstructions(QString instructions, bool replace); - void disableAllCalibrations(); - void enableAllCalibrations(); + void started(); + void stopped(); void savePositionEnabledChanged(bool state); void progressChanged(int value); + void displayVisualHelp(QString elementID); + void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info); + public slots: - // Slots for calibrating the mags void start(); void savePosition(); + void save(); + private slots: void getSample(UAVObject *obj); - void compute(); + private: + typedef struct { + UAVObject::Metadata attitudeStateMdata; + } Memento; + QMutex sensorsUpdateLock; int position; + bool collectingData; + bool m_dirty; + + Memento memento; QList rot_accum_roll; QList rot_accum_pitch; double rot_data_roll; double rot_data_pitch; - UAVObject::Metadata initialAttitudeStateMdata; + + // convenience pointers + AttitudeState *attitudeState; + AttitudeSettings *attitudeSettings; + UAVObjectManager *getObjectManager(); }; } + #endif // LEVELCALIBRATIONMODEL_H diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp index 444a16d05..e5da80250 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp @@ -26,64 +26,83 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "sixpointcalibrationmodel.h" -#include #include "extensionsystem/pluginmanager.h" -#include -#include "math.h" #include "calibration/calibrationuiutils.h" + +#include "math.h" +#include #include "QDebug" + #define POINT_SAMPLE_SIZE 50 #define GRAVITY 9.81f -#define sign(x) ((x < 0) ? -1 : 1) +#define sign(x) ((x < 0) ? -1 : 1) #define FITTING_USING_CONTINOUS_ACQUISITION + +#define IS_NAN(v) (!(v == v)) + namespace OpenPilot { SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) : QObject(parent), + calibratingMag(false), + calibratingAccel(false), calibrationStepsMag(), calibrationStepsAccelOnly(), currentSteps(0), position(-1), - calibratingMag(false), - calibratingAccel(false), - collectingData(false) + collectingData(false), + m_dirty(false) { calibrationStepsMag.clear(); calibrationStepsMag << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally, nose pointing north and click Save Position button...")) + tr("Place horizontally, nose pointing north and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down, right side west and click Save Position button...")) + tr("Place with nose down, right side west and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down, nose west and click Save Position button...")) + tr("Place right side down, nose west and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down, nose east and click Save Position button...")) + tr("Place upside down, nose east and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up, left side north and click Save Position button...")) + tr("Place with nose up, left side north and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down, nose south and click Save Position button...")); + tr("Place with left side down, nose south and press Save Position...")); calibrationStepsAccelOnly.clear(); calibrationStepsAccelOnly << CalibrationStep(CALIBRATION_HELPER_IMAGE_NED, - tr("Place horizontally and click Save Position button...")) + tr("Place horizontally and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_DWN, - tr("Place with nose down and click Save Position button...")) + tr("Place with nose down and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_WDS, - tr("Place right side down and click Save Position button...")) + tr("Place right side down and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_ENU, - tr("Place upside down and click Save Position button...")) + tr("Place upside down and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_USE, - tr("Place with nose up and click Save Position button...")) + tr("Place with nose up and press Save Position...")) << CalibrationStep(CALIBRATION_HELPER_IMAGE_SUW, - tr("Place with left side down and click Save Position button...")); -} + tr("Place with left side down and press Save Position...")); -/********** Six point calibration **************/ + revoCalibration = RevoCalibration::GetInstance(getObjectManager()); + Q_ASSERT(revoCalibration); + + accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); + Q_ASSERT(accelGyroSettings); + + accelState = AccelState::GetInstance(getObjectManager()); + Q_ASSERT(accelState); + + magState = MagState::GetInstance(getObjectManager()); + Q_ASSERT(magState); + + homeLocation = HomeLocation::GetInstance(getObjectManager()); + Q_ASSERT(homeLocation); +} void SixPointCalibrationModel::magStart() { start(false, true); } + void SixPointCalibrationModel::accelStart() { start(true, false); @@ -99,39 +118,25 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) { calibratingAccel = calibrateAccel; calibratingMag = calibrateMag; - // Store and reset board rotation before calibration starts - storeAndClearBoardRotation(); - if (calibrateMag) { - currentSteps = &calibrationStepsMag; - } else { - currentSteps = &calibrationStepsAccelOnly; - } - - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); - - Q_ASSERT(revoCalibration); - Q_ASSERT(homeLocation); - RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); - savedSettings.revoCalibration = revoCalibration->getData(); - HomeLocation::DataFields homeLocationData = homeLocation->getData(); - AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); - savedSettings.accelGyroSettings = accelGyroSettings->getData(); + started(); // check if Homelocation is set + HomeLocation::DataFields homeLocationData = homeLocation->getData(); if (!homeLocationData.Set) { - QMessageBox msgBox; - msgBox.setInformativeText(tr("

HomeLocation not SET.

Please set your HomeLocation and try again. Aborting calibration!

")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.setIcon(QMessageBox::Information); - msgBox.exec(); + displayInstructions(tr("Home location not set, please set your home location and retry."), WizardModel::Warn); + displayInstructions(tr("Aborting calibration!"), WizardModel::Failure); + stopped(); return; } + // Store and reset board rotation before calibration starts + storeAndClearBoardRotation(); + // Calibration accel + AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + memento.accelGyroSettingsData = accelGyroSettingsData; + accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = 1; accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = 1; accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] = 1; @@ -139,11 +144,12 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0; accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0; - accel_accum_x.clear(); - accel_accum_y.clear(); - accel_accum_z.clear(); + accelGyroSettings->setData(accelGyroSettingsData); // Calibration mag + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + memento.revoCalibrationData = revoCalibrationData; + // Reset the transformation matrix to identity for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) { revoCalibrationData.mag_transform[i] = 0; @@ -156,11 +162,9 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = 0; // Disable adaptive mag nulling - initialMagCorrectionRate = revoCalibrationData.MagBiasNullingRate; revoCalibrationData.MagBiasNullingRate = 0; revoCalibration->setData(revoCalibrationData); - accelGyroSettings->setData(accelGyroSettingsData); QThread::usleep(100000); @@ -168,46 +172,49 @@ void SixPointCalibrationModel::start(bool calibrateAccel, bool calibrateMag) mag_accum_y.clear(); mag_accum_z.clear(); - UAVObject::Metadata mdata; - - /* Need to get as many accel updates as possible */ - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); - initialAccelStateMdata = accelState->getMetadata(); + mag_fit_x.clear(); + mag_fit_y.clear(); + mag_fit_z.clear(); + // Need to get as many accel updates as possible + memento.accelStateMetadata = accelState->getMetadata(); if (calibrateAccel) { - mdata = initialAccelStateMdata; + UAVObject::Metadata mdata = accelState->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; accelState->setMetadata(mdata); } - /* Need to get as many mag updates as possible */ - MagState *mag = MagState::GetInstance(getObjectManager()); - Q_ASSERT(mag); - initialMagStateMdata = mag->getMetadata(); + // Need to get as many mag updates as possible + memento.magStateMetadata = magState->getMetadata(); if (calibrateMag) { - mdata = initialMagStateMdata; + UAVObject::Metadata mdata = magState->getMetadata(); UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); mdata.flightTelemetryUpdatePeriod = 100; - mag->setMetadata(mdata); + magState->setMetadata(mdata); } - /* Show instructions and enable controls */ - displayInstructions((*currentSteps)[0].instructions, true); - showHelp((*currentSteps)[0].visualHelp); + // reset dirty state to forget previous unsaved runs + m_dirty = false; + + if (calibrateMag) { + currentSteps = &calibrationStepsMag; + } else { + currentSteps = &calibrationStepsAccelOnly; + } - disableAllCalibrations(); - savePositionEnabledChanged(true); position = 0; - mag_fit_x.clear(); - mag_fit_y.clear(); - mag_fit_z.clear(); + + // Show instructions and enable controls + progressChanged(0); + displayInstructions((*currentSteps)[0].instructions, WizardModel::Prompt); + showHelp((*currentSteps)[0].visualHelp); + savePositionEnabledChanged(true); } /** * Saves the data from the aircraft in one of six positions. - * This is called when they click "save position" and starts + * This is called when they press "save position" and starts * averaging data for this position. */ void SixPointCalibrationModel::savePositionData() @@ -225,25 +232,20 @@ void SixPointCalibrationModel::savePositionData() collectingData = true; - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); - MagState *mag = MagState::GetInstance(getObjectManager()); - Q_ASSERT(mag); - if (calibratingMag) { #ifdef FITTING_USING_CONTINOUS_ACQUISITION // Mag samples are acquired during the whole calibration session, to be used for ellipsoid fit. if (!position) { - connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); + connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); } #endif // FITTING_USING_CONTINOUS_ACQUISITION - connect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + connect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); } if (calibratingAccel) { connect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); } - displayInstructions(tr("Hold..."), false); + displayInstructions(tr("Hold...")); } /** @@ -258,16 +260,12 @@ void SixPointCalibrationModel::getSample(UAVObject *obj) // This is necessary to prevent a race condition on disconnect signal and another update if (collectingData == true) { if (obj->getObjID() == AccelState::OBJID) { - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); AccelState::DataFields accelStateData = accelState->getData(); accel_accum_x.append(accelStateData.x); accel_accum_y.append(accelStateData.y); accel_accum_z.append(accelStateData.z); } else if (obj->getObjID() == MagState::OBJID) { - MagState *mag = MagState::GetInstance(getObjectManager()); - Q_ASSERT(mag); - MagState::DataFields magData = mag->getData(); + MagState::DataFields magData = magState->getData(); mag_accum_x.append(magData.x); mag_accum_y.append(magData.y); mag_accum_z.append(magData.z); @@ -281,27 +279,35 @@ void SixPointCalibrationModel::getSample(UAVObject *obj) } } - if ((!calibratingAccel || (accel_accum_x.size() >= POINT_SAMPLE_SIZE)) && - (!calibratingMag || (mag_accum_x.size() >= POINT_SAMPLE_SIZE / 10)) && - (collectingData == true)) { + bool done = true; + float progress = 0; + if (calibratingAccel) { + done = (accel_accum_x.size() >= POINT_SAMPLE_SIZE); + progress = (float)accel_accum_x.size() / (float)POINT_SAMPLE_SIZE; + } + if (calibratingMag) { + done = (mag_accum_x.size() >= POINT_SAMPLE_SIZE / 10); + progress = (float)mag_accum_x.size() / (float)(POINT_SAMPLE_SIZE / 10); + } + + progressChanged(progress * 100); + + if (collectingData && done) { collectingData = false; savePositionEnabledChanged(true); // Store the mean for this position for the accel - AccelState *accelState = AccelState::GetInstance(getObjectManager()); - Q_ASSERT(accelState); if (calibratingAccel) { disconnect(accelState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); accel_data_x[position] = CalibrationUtils::listMean(accel_accum_x); accel_data_y[position] = CalibrationUtils::listMean(accel_accum_y); accel_data_z[position] = CalibrationUtils::listMean(accel_accum_z); } + // Store the mean for this position for the mag - MagState *mag = MagState::GetInstance(getObjectManager()); - Q_ASSERT(mag); if (calibratingMag) { - disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); mag_data_x[position] = CalibrationUtils::listMean(mag_accum_x); mag_data_y[position] = CalibrationUtils::listMean(mag_accum_y); mag_data_z[position] = CalibrationUtils::listMean(mag_accum_z); @@ -309,26 +315,30 @@ void SixPointCalibrationModel::getSample(UAVObject *obj) position = (position + 1) % 6; if (position != 0) { - displayInstructions((*currentSteps)[position].instructions, false); + // move to next step + displayInstructions((*currentSteps)[position].instructions, WizardModel::Prompt); showHelp((*currentSteps)[position].visualHelp); } else { + // done... #ifdef FITTING_USING_CONTINOUS_ACQUISITION if (calibratingMag) { - disconnect(mag, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); + disconnect(magState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(continouslyGetMagSamples(UAVObject *))); } #endif // FITTING_USING_CONTINOUS_ACQUISITION - compute(calibratingMag, calibratingAccel); - savePositionEnabledChanged(false); + compute(); - enableAllCalibrations(); - showHelp(CALIBRATION_HELPER_IMAGE_EMPTY); - /* Cleanup original settings */ - accelState->setMetadata(initialAccelStateMdata); - - mag->setMetadata(initialMagStateMdata); + // Restore original settings + accelState->setMetadata(memento.accelStateMetadata); + magState->setMetadata(memento.magStateMetadata); + revoCalibration->setData(memento.revoCalibrationData); + accelGyroSettings->setData(memento.accelGyroSettingsData); // Recall saved board rotation recallBoardRotation(); + + stopped(); + showHelp(CALIBRATION_HELPER_IMAGE_EMPTY); + savePositionEnabledChanged(false); } } } @@ -338,35 +348,29 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj) QMutexLocker lock(&sensorsUpdateLock); if (obj->getObjID() == MagState::OBJID) { - MagState *mag = MagState::GetInstance(getObjectManager()); - Q_ASSERT(mag); - MagState::DataFields magData = mag->getData(); - mag_fit_x.append(magData.x); - mag_fit_y.append(magData.y); - mag_fit_z.append(magData.z); + MagState::DataFields magStateData = magState->getData(); + mag_fit_x.append(magStateData.x); + mag_fit_y.append(magStateData.y); + mag_fit_z.append(magStateData.z); } } /** - * Computes the scale and bias for the magnetomer and (compile option) - * for the accel once all the data has been collected in 6 positions. + * Computes the scale and bias for the magnetomer or for the accel. + * Called once all the data has been collected in 6 positions. */ -void SixPointCalibrationModel::compute(bool mag, bool accel) +void SixPointCalibrationModel::compute() { double S[3], b[3]; double Be_length; - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager()); - RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager()); - HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); - Q_ASSERT(revoCalibration); - Q_ASSERT(homeLocation); - AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + HomeLocation::DataFields homeLocationData = homeLocation->getData(); // Calibration accel - if (accel) { + if (calibratingAccel) { OpenPilot::CalibrationUtils::SixPointInConstFieldCal(homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b); accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] = fabs(S[0]); accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] = fabs(S[1]); @@ -378,7 +382,7 @@ void SixPointCalibrationModel::compute(bool mag, bool accel) } // Calibration mag - if (mag) { + if (calibratingMag) { Be_length = sqrt(pow(homeLocationData.Be[0], 2) + pow(homeLocationData.Be[1], 2) + pow(homeLocationData.Be[2], 2)); int vectSize = mag_fit_x.count(); Eigen::VectorXf samples_x(vectSize); @@ -421,74 +425,85 @@ void SixPointCalibrationModel::compute(bool mag, bool accel) revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] = result.Bias.coeff(2); } // Restore the previous setting - revoCalibrationData.MagBiasNullingRate = initialMagCorrectionRate; - - - bool good_calibration = true; + revoCalibrationData.MagBiasNullingRate = memento.revoCalibrationData.MagBiasNullingRate;; // Check the mag calibration is good - if (mag) { - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]; + bool good_calibration = true; + if (calibratingMag) { + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]); - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2]; + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C0]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C1]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R1C2]); - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1]; - good_calibration &= revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2] == - revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2]; + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C0]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C1]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R2C2]); - good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X] == - revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]; - good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y] == - revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]; - good_calibration &= revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z] == - revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]; + good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]); + good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]); } // Check the accel calibration is good - if (accel) { - good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X] == - accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X]; - good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y] == - accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y]; - good_calibration &= accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z] == - accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z]; - good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] == - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]; - good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] == - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]; - good_calibration &= accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] == - accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]; + if (calibratingAccel) { + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_X]); + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Y]); + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_scale[AccelGyroSettings::ACCEL_SCALE_Z]); + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X]); + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y]); + good_calibration &= !IS_NAN(accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z]); } if (good_calibration) { - if (mag) { - revoCalibration->setData(revoCalibrationData); - } else { - revoCalibration->setData(savedSettings.revoCalibration); + m_dirty = true; + if (calibratingMag) { + result.revoCalibrationData = revoCalibrationData; + displayInstructions(tr("Magnetometer calibration completed successfully."), WizardModel::Success); + } + if (calibratingAccel) { + result.accelGyroSettingsData = accelGyroSettingsData; + displayInstructions(tr("Accelerometer calibration completed successfully."), WizardModel::Success); + } + } else { + progressChanged(0); + displayInstructions(tr("Calibration failed! Please review the help and retry."), WizardModel::Failure); + } + // set to run again + position = -1; +} + +void SixPointCalibrationModel::save() +{ + if (!m_dirty) { + return; + } + if (calibratingMag) { + RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); + + for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_R2C2; i++) { + revoCalibrationData.mag_transform[i] = result.revoCalibrationData.mag_transform[i]; + } + for (int i = 0; i < 3; i++) { + revoCalibrationData.mag_bias[i] = result.revoCalibrationData.mag_bias[i]; } - if (accel) { - accelGyroSettings->setData(accelGyroSettingsData); - } else { - accelGyroSettings->setData(savedSettings.accelGyroSettings); - } - displayInstructions(tr("Sensor scale and bias computed succesfully."), true); - } else { - displayInstructions(tr("Bad calibration. Please review the instructions and repeat."), true); + revoCalibration->setData(revoCalibrationData); } - position = -1; // set to run again + if (calibratingAccel) { + AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData(); + + for (int i = 0; i < 3; i++) { + accelGyroSettingsData.accel_scale[i] = result.accelGyroSettingsData.accel_scale[i]; + accelGyroSettingsData.accel_bias[i] = result.accelGyroSettingsData.accel_bias[i]; + } + + accelGyroSettings->setData(accelGyroSettingsData); + } + + m_dirty = false; } + UAVObjectManager *SixPointCalibrationModel::getObjectManager() { ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); @@ -497,6 +512,7 @@ UAVObjectManager *SixPointCalibrationModel::getObjectManager() Q_ASSERT(objMngr); return objMngr; } + void SixPointCalibrationModel::showHelp(QString image) { if (image == CALIBRATION_HELPER_IMAGE_EMPTY) { diff --git a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h index 128ff884a..37c7b7d29 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h @@ -27,20 +27,53 @@ */ #ifndef SIXPOINTCALIBRATIONMODEL_H #define SIXPOINTCALIBRATIONMODEL_H -#include -#include -#include + +#include "wizardmodel.h" #include "calibration/calibrationutils.h" -#include #include #include #include #include #include + +#include +#include +#include +#include + namespace OpenPilot { class SixPointCalibrationModel : public QObject { Q_OBJECT +public: + explicit SixPointCalibrationModel(QObject *parent = 0); + + bool dirty() + { + return m_dirty; + } + +signals: + void started(); + void stopped(); + void storeAndClearBoardRotation(); + void recallBoardRotation(); + void savePositionEnabledChanged(bool state); + void progressChanged(int value); + void displayVisualHelp(QString elementID); + void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info); + +public slots: + void magStart(); + void accelStart(); + void savePositionData(); + void save(); + +private slots: + void getSample(UAVObject *obj); + void continouslyGetMagSamples(UAVObject *obj); + +private: class CalibrationStep { public: CalibrationStep(QString newVisualHelp, QString newInstructions) @@ -53,54 +86,36 @@ public: }; typedef struct { - RevoCalibration::DataFields revoCalibration; - AccelGyroSettings::DataFields accelGyroSettings; - } SavedSettings; -public: - explicit SixPointCalibrationModel(QObject *parent = 0); + UAVObject::Metadata accelStateMetadata; + UAVObject::Metadata magStateMetadata; + RevoCalibration::DataFields revoCalibrationData; + AccelGyroSettings::DataFields accelGyroSettingsData; + } Memento; -signals: - void displayVisualHelp(QString elementID); - void displayInstructions(QString instructions, bool replace); - void disableAllCalibrations(); - void enableAllCalibrations(); - void storeAndClearBoardRotation(); - void recallBoardRotation(); - void savePositionEnabledChanged(bool state); - -public slots: - // Slots for calibrating the mags - void magStart(); - void accelStart(); - void savePositionData(); -private slots: - void getSample(UAVObject *obj); - void continouslyGetMagSamples(UAVObject *obj); -private: - void start(bool calibrateAccel, bool calibrateMag); - UAVObjectManager *getObjectManager(); - QList calibrationStepsMag; - QList calibrationStepsAccelOnly; - QList *currentSteps; - UAVObject::Metadata initialAccelStateMdata; - UAVObject::Metadata initialMagStateMdata; - float initialMagCorrectionRate; - SavedSettings savedSettings; - - int position; + typedef struct { + RevoCalibration::DataFields revoCalibrationData; + AccelGyroSettings::DataFields accelGyroSettingsData; + } Result; bool calibratingMag; bool calibratingAccel; - double accel_data_x[6], accel_data_y[6], accel_data_z[6]; - double mag_data_x[6], mag_data_y[6], mag_data_z[6]; + QList calibrationStepsMag; + QList calibrationStepsAccelOnly; + QList *currentSteps; + int position; + + Memento memento; + Result result; + + bool collectingData; + bool m_dirty; QMutex sensorsUpdateLock; - // ! Computes the scale and bias of the mag based on collected data - void compute(bool mag, bool accel); + double accel_data_x[6], accel_data_y[6], accel_data_z[6]; + double mag_data_x[6], mag_data_y[6], mag_data_z[6]; - bool collectingData; QList accel_accum_x; QList accel_accum_y; QList accel_accum_z; @@ -110,7 +125,20 @@ private: QList mag_fit_x; QList mag_fit_y; QList mag_fit_z; + + // convenience pointers + RevoCalibration *revoCalibration; + AccelGyroSettings *accelGyroSettings; + AccelState *accelState; + MagState *magState; + HomeLocation *homeLocation; + + void start(bool calibrateAccel, bool calibrateMag); + // Computes the scale and bias of the mag based on collected data + void compute(); void showHelp(QString image); + UAVObjectManager *getObjectManager(); }; } + #endif // SIXPOINTCALIBRATIONMODEL_H diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/boardsetuptransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/boardsetuptransition.h index 4e0e0e4cb..609a36337 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/boardsetuptransition.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/boardsetuptransition.h @@ -29,13 +29,15 @@ #ifndef BOARDSETUPTRANSITION_H #define BOARDSETUPTRANSITION_H +#include "thermalcalibrationhelper.h" + #include #include -#include "thermalcalibrationhelper.h" namespace OpenPilot { class BoardSetupTransition : public QSignalTransition { Q_OBJECT + public: BoardSetupTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState) : QSignalTransition(helper, SIGNAL(setupBoardCompleted(bool))), @@ -66,11 +68,14 @@ public: { Q_UNUSED(e); } + public slots: void enterState() { + m_helper->addInstructions(tr("Configuring board for calibration."), WizardModel::Debug); m_helper->setupBoard(); } + private: ThermalCalibrationHelper *m_helper; }; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h index d08510182..1ce4a56cf 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/compensationcalculationtransition.h @@ -34,9 +34,11 @@ #include "../wizardstate.h" #include "thermalcalibrationhelper.h" + namespace OpenPilot { class CompensationCalculationTransition : public QSignalTransition { Q_OBJECT + public: CompensationCalculationTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState) : QSignalTransition(helper, SIGNAL(calculationCompleted())), @@ -52,19 +54,23 @@ public: Q_UNUSED(e); QString nextStateName; if (m_helper->calibrationSuccessful()) { - nextStateName = tr("Calibration completed succesfully"); + m_helper->setProgressMax(100); + m_helper->setProgress(100); + m_helper->addInstructions(tr("Thermal calibration completed successfully."), WizardModel::Success); } else { - nextStateName = tr("Calibration failed! Please read the instructions and retry"); + m_helper->setProgressMax(100); + m_helper->setProgress(0); + m_helper->addInstructions(tr("Calibration failed! Please review the help and retry."), WizardModel::Failure); } - static_cast(targetState())->setStepName(nextStateName); } - public slots: void enterState() { + m_helper->addInstructions("Calculating calibration data.", WizardModel::Debug); m_helper->calculate(); } + private: ThermalCalibrationHelper *m_helper; }; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h index 35fc44a93..f4a4a2456 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/dataacquisitiontransition.h @@ -29,13 +29,15 @@ #ifndef DATAACQUISITIONTRANSITION_H #define DATAACQUISITIONTRANSITION_H +#include "thermalcalibrationhelper.h" + #include #include -#include "thermalcalibrationhelper.h" namespace OpenPilot { class DataAcquisitionTransition : public QSignalTransition { Q_OBJECT + public: DataAcquisitionTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState) : QSignalTransition(helper, SIGNAL(collectionCompleted())), @@ -55,8 +57,17 @@ 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->addInstructions(tr("Acquisition will run until the rate of temperature change is less than %1°C/min.").arg(ThermalCalibrationHelper::TargetGradient, 4, 'f', 2)); + m_helper->addInstructions(tr("For the calibration to be valid, the temperature span during acquisition must be greater than %1°C.").arg(ThermalCalibrationHelper::TargetTempDelta, 4, 'f', 2)); + m_helper->addInstructions(tr("Estimating acquisition duration...")); + m_helper->initAcquisition(); } + private: ThermalCalibrationHelper *m_helper; }; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/settingshandlingtransitions.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/settingshandlingtransitions.h index 247c3999e..d64bfd313 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/settingshandlingtransitions.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/settingshandlingtransitions.h @@ -28,13 +28,16 @@ #ifndef SETTINGSHANDLINGTRANSITIONS_H #define SETTINGSHANDLINGTRANSITIONS_H + +#include "thermalcalibrationhelper.h" + #include #include -#include "thermalcalibrationhelper.h" namespace OpenPilot { class BoardStatusSaveTransition : public QSignalTransition { Q_OBJECT + public: BoardStatusSaveTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState) : QSignalTransition(helper, SIGNAL(statusSaveCompleted(bool))), @@ -65,18 +68,21 @@ public: { Q_UNUSED(e); } + public slots: void enterState() { + m_helper->addInstructions(tr("Saving initial settings."), WizardModel::Debug); m_helper->statusSave(); } + private: ThermalCalibrationHelper *m_helper; }; - class BoardStatusRestoreTransition : public QSignalTransition { Q_OBJECT + public: BoardStatusRestoreTransition(ThermalCalibrationHelper *helper, QState *currentState, QState *targetState) : QSignalTransition(helper, SIGNAL(statusRestoreCompleted(bool))), @@ -102,12 +108,15 @@ public: } return false; } + public slots: void enterState() { + m_helper->addInstructions(tr("Restoring board configuration."), WizardModel::Debug); m_helper->endAcquisition(); m_helper->statusRestore(); } + private: ThermalCalibrationHelper *m_helper; }; diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp index a871cdf01..1190fb537 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.cpp @@ -31,17 +31,34 @@ #include #include "version_info/version_info.h" +#include + +// uncomment to simulate board warming up (no need to put it in the fridge...) +// #define SIMULATE + namespace OpenPilot { ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) : QObject(parent) { m_tempdir.reset(new QTemporaryDir()); - m_boardInitialSettings = thermalCalibrationBoardSettings(); - m_boardInitialSettings.statusSaved = false; - m_results = thermalCalibrationResults(); + + m_memento = Memento(); + m_memento.statusSaved = false; + + m_results = Results(); m_results.accelCalibrated = false; - m_results.baroCalibrated = false; m_results.gyroCalibrated = false; + m_results.baroCalibrated = false; + + 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()); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); TelemetryManager *telMngr = pm->getObject(); @@ -54,30 +71,12 @@ ThermalCalibrationHelper::ThermalCalibrationHelper(QObject *parent) : */ bool ThermalCalibrationHelper::setupBoardForCalibration() { - qDebug() << "setupBoardForCalibration"; - - UAVObjectManager *objManager = getObjectManager(); - Q_ASSERT(objManager); - - // accelSensor Meta - AccelSensor *accelSensor = AccelSensor::GetInstance(objManager); - Q_ASSERT(accelSensor); setMetadataForCalibration(accelSensor); - - // gyroSensor Meta - GyroSensor *gyroSensor = GyroSensor::GetInstance(objManager); - Q_ASSERT(gyroSensor); setMetadataForCalibration(gyroSensor); - - // baroSensor Meta - BaroSensor *baroSensor = BaroSensor::GetInstance(objManager); - Q_ASSERT(baroSensor); setMetadataForCalibration(baroSensor); // 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++) { data.accel_temp_coeff[i] = 0.0f; } @@ -92,8 +91,6 @@ bool ThermalCalibrationHelper::setupBoardForCalibration() accelGyroSettings->setData(data); // clean any correction before calibrating - RevoSettings *revoSettings = RevoSettings::GetInstance(objManager); - Q_ASSERT(revoSettings); RevoSettings::DataFields revoSettingsData = revoSettings->getData(); for (uint i = 0; i < RevoSettings::BAROTEMPCORRECTIONPOLYNOMIAL_NUMELEM; i++) { revoSettingsData.BaroTempCorrectionPolynomial[i] = 0.0f; @@ -113,43 +110,18 @@ bool ThermalCalibrationHelper::setupBoardForCalibration() bool ThermalCalibrationHelper::saveBoardInitialSettings() { // Store current board status: - qDebug() << "Save initial settings"; + 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(); - UAVObjectManager *objManager = getObjectManager(); - Q_ASSERT(objManager); - // accelSensor Meta - AccelSensor *accelSensor = AccelSensor::GetInstance(objManager); - Q_ASSERT(accelSensor); - m_boardInitialSettings.accelSensorMeta = accelSensor->getMetadata(); - // gyroSensor Meta - GyroSensor *gyroSensor = GyroSensor::GetInstance(objManager); - Q_ASSERT(gyroSensor); - m_boardInitialSettings.gyroSensorMeta = gyroSensor->getMetadata(); - - // baroSensor Meta - BaroSensor *baroSensor = BaroSensor::GetInstance(objManager); - Q_ASSERT(baroSensor); - m_boardInitialSettings.baroensorMeta = baroSensor->getMetadata(); - - // accelGyroSettings data - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager); - Q_ASSERT(accelGyroSettings); - m_boardInitialSettings.accelGyroSettings = accelGyroSettings->getData(); - - // revoSettings data - RevoSettings *revoSettings = RevoSettings::GetInstance(objManager); - Q_ASSERT(revoSettings); - m_boardInitialSettings.revoSettings = revoSettings->getData(); - - // 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 - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager); - Q_ASSERT(accelGyroSettings); - m_boardInitialSettings.accelGyroSettings = accelGyroSettings->getData(); + m_memento.accelGyroSettings = accelGyroSettings->getData(); */ - m_boardInitialSettings.statusSaved = true; + m_memento.statusSaved = true; return true; } @@ -159,47 +131,23 @@ bool ThermalCalibrationHelper::saveBoardInitialSettings() */ bool ThermalCalibrationHelper::restoreInitialSettings() { - if (!m_boardInitialSettings.statusSaved) { + if (!m_memento.statusSaved) { return false; } - // restore initial board status - UAVObjectManager *objManager = getObjectManager(); - Q_ASSERT(objManager); - // accelSensor Meta - AccelSensor *accelSensor = AccelSensor::GetInstance(objManager); - Q_ASSERT(accelSensor); - accelSensor->setMetadata(m_boardInitialSettings.accelSensorMeta); - - // 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); + accelSensor->setMetadata(m_memento.accelSensorMeta); + gyroSensor->setMetadata(m_memento.gyroSensorMeta); + baroSensor->setMetadata(m_memento.baroensorMeta); + accelGyroSettings->setData(m_memento.accelGyroSettings); + revoSettings->setData(m_memento.revoSettings); return true; } - /* Methods called from transitions */ void ThermalCalibrationHelper::setupBoard() { - setProcessPercentage(ProcessPercentageSetupBoard); if (setupBoardForCalibration()) { emit setupBoardCompleted(true); } else { @@ -219,7 +167,6 @@ void ThermalCalibrationHelper::statusRestore() void ThermalCalibrationHelper::statusSave() { - setProcessPercentage(ProcessPercentageSaveSettings); // prevent saving multiple times if (!isBoardInitialSettingsSaved() && saveBoardInitialSettings()) { emit statusSaveCompleted(true); @@ -230,25 +177,35 @@ void ThermalCalibrationHelper::statusSave() void ThermalCalibrationHelper::initAcquisition() { - setProcessPercentage(ProcessPercentageBaseAcquisition); QMutexLocker lock(&sensorsUpdateLock); - m_targetduration = 0; - m_gradient = 0.0f; - m_initialGradient = m_gradient; - m_forceStopAcquisition = false; + // 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; - BaroSensor *baroSensor = BaroSensor::GetInstance(getObjectManager()); - Q_ASSERT(baroSensor); - m_lastCheckpointTemp = baroSensor->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_rangeReached = false; + + m_forceStopAcquisition = false; + m_acquiring = true; + + createDebugLog(); connectUAVOs(); } @@ -256,76 +213,94 @@ void ThermalCalibrationHelper::collectSample(UAVObject *sample) { QMutexLocker lock(&sensorsUpdateLock); + if (!m_acquiring) { + return; + } + switch (sample->getObjID()) { case AccelSensor::OBJID: - { - AccelSensor *reading = AccelSensor::GetInstance(getObjectManager()); - Q_ASSERT(reading); - m_accelSamples.append(reading->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_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; - break; - } case GyroSensor::OBJID: - { - GyroSensor *reading = GyroSensor::GetInstance(getObjectManager()); - Q_ASSERT(reading); - m_gyroSamples.append(reading->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_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; break; - } + case BaroSensor::OBJID: { - BaroSensor *reading = BaroSensor::GetInstance(getObjectManager()); - Q_ASSERT(reading); - m_baroSamples.append(reading->getData()); - 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_temperature = reading->getTemperature(); - updateTemp(m_temperature); + float temp = getTemperature(); + 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; + // must be done last as this call might end acquisition and close the debug log file + updateTemperature(temp); break; } + case MagSensor::OBJID: - { - MagSensor *reading = MagSensor::GetInstance(getObjectManager()); - Q_ASSERT(reading); - m_magSamples.append(reading->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_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(); - } + qDebug() << "Unexpected object" << sample->getObjID(); } } +float ThermalCalibrationHelper::getTemperature() +{ +#ifdef SIMULATE + float t = m_startTime.msecsTo(QTime::currentTime()) / 1000.0f; + // Simulate a temperature rise using Newton's law of cooling + // See http://en.wikipedia.org/wiki/Newton%27s_law_of_cooling#Newton.27s_law_of_cooling + // Initial temp : 20 + // Final temp : 40 + // 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); + +#else + return baroSensor->getTemperature(); + +#endif +} + +void ThermalCalibrationHelper::endAcquisition() +{ + disconnectUAVOs(); +} + void ThermalCalibrationHelper::cleanup() { + m_acquiring = false; disconnectUAVOs(); - m_debugStream.flush(); - m_debugFile.close(); + closeDebugLog(); } - void ThermalCalibrationHelper::calculate() { - setProcessPercentage(ProcessPercentageBaseCalculation); + // baro int count = m_baroSamples.count(); Eigen::VectorXf datax(count); Eigen::VectorXf datay(1); @@ -337,11 +312,19 @@ 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) { + 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(); - setProcessPercentage(processPercentage() + 2); + m_results.baroTempMin = datat.array().minCoeff(); + m_results.baroTempMax = datat.array().maxCoeff(); + + // gyro count = m_gyroSamples.count(); datax.resize(count); datay.resize(count); @@ -355,12 +338,20 @@ 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) { + 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. /* - setProcessPercentage(processPercentage() + 2); count = m_accelSamples.count(); datax.resize(count); datay.resize(count); @@ -368,55 +359,66 @@ 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; - copyResultToSettings(); emit calculationCompleted(); closeDebugLog(); } - /* 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()); + // 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. You may now end acquisition or continue.")); + } + 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 gradientChanged(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; } @@ -426,63 +428,47 @@ void ThermalCalibrationHelper::updateTemp(float temp) if (m_initialGradient < .1 && m_gradient > .1) { m_initialGradient = m_gradient; } - if (m_gradient < TargetGradient || m_forceStopAcquisition) { - emit collectionCompleted(); - } if (m_targetduration != 0) { - int tmp = ((ProcessPercentageBaseCalculation - ProcessPercentageBaseAcquisition) - * elapsed) / m_targetduration; - tmp = tmp > ProcessPercentageBaseCalculation - 5 ? ProcessPercentageBaseCalculation - 5 : tmp; - setProcessPercentage(tmp); - } else if (m_gradient > .1 && m_initialGradient / 2.0f > m_gradient) { - qDebug() << "M_gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient; + 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; - if (m_debugFile.isOpen()) { - m_debugStream << "INFO::Trace gradient " << m_gradient << " Elapsed" << elapsed << " m_initialGradient" << m_initialGradient - << " target:" << m_targetduration << endl; - } + + 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(); } } } -void ThermalCalibrationHelper::endAcquisition() -{ - disconnectUAVOs(); -} - void ThermalCalibrationHelper::connectUAVOs() { - createDebugLog(); - AccelSensor *accel = AccelSensor::GetInstance(getObjectManager()); - - connect(accel, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); - - GyroSensor *gyro = GyroSensor::GetInstance(getObjectManager()); - connect(gyro, 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 *))); + connect(accelSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); + connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); + connect(baroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); + connect(magSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); } void ThermalCalibrationHelper::disconnectUAVOs() { - AccelSensor *accel = AccelSensor::GetInstance(getObjectManager()); - - disconnect(accel, 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 *))); + disconnect(accelSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(collectSample(UAVObject *))); + disconnect(gyroSensor, 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 *))); } void ThermalCalibrationHelper::createDebugLog() @@ -506,18 +492,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; } } @@ -532,27 +520,17 @@ void ThermalCalibrationHelper::closeDebugLog() void ThermalCalibrationHelper::copyResultToSettings() { - UAVObjectManager *objManager = getObjectManager(); - - Q_ASSERT(objManager); - if (calibrationSuccessful()) { - RevoSettings *revosettings = RevoSettings::GetInstance(objManager); - Q_ASSERT(revosettings); - RevoSettings::DataFields revosettingsdata = revosettings->getData(); - revosettingsdata.BaroTempCorrectionPolynomial[0] = m_results.baro[0]; - revosettingsdata.BaroTempCorrectionPolynomial[1] = m_results.baro[1]; - revosettingsdata.BaroTempCorrectionPolynomial[2] = m_results.baro[2]; - revosettingsdata.BaroTempCorrectionPolynomial[3] = m_results.baro[3]; - revosettingsdata.BaroTempCorrectionExtent[0] = m_results.baroTempMin; - revosettingsdata.BaroTempCorrectionExtent[1] = m_results.baroTempMax; - revosettings->setData(revosettingsdata); - revosettings->updated(); - - AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(objManager); - Q_ASSERT(accelGyroSettings); - AccelGyroSettings::DataFields data = accelGyroSettings->getData(); + RevoSettings::DataFields revoSettingsData = revoSettings->getData(); + revoSettingsData.BaroTempCorrectionPolynomial[0] = m_results.baro[0]; + revoSettingsData.BaroTempCorrectionPolynomial[1] = m_results.baro[1]; + revoSettingsData.BaroTempCorrectionPolynomial[2] = m_results.baro[2]; + revoSettingsData.BaroTempCorrectionPolynomial[3] = m_results.baro[3]; + revoSettingsData.BaroTempCorrectionExtent[0] = m_results.baroTempMin; + revoSettingsData.BaroTempCorrectionExtent[1] = m_results.baroTempMax; + revoSettings->setData(revoSettingsData); + 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]; @@ -571,7 +549,6 @@ void ThermalCalibrationHelper::copyResultToSettings() data.temp_calibrated_extent[1] = m_results.accelGyroTempMax; accelGyroSettings->setData(data); - accelGyroSettings->updated(); } } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h index 4552b5e41..0e0136606 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationhelper.h @@ -47,11 +47,13 @@ #include #include "accelgyrosettings.h" - // Calibration data #include #include #include + +#include "../wizardmodel.h" + namespace OpenPilot { typedef struct { // this is not needed for revo, but should for CC/CC3D @@ -62,7 +64,7 @@ typedef struct { UAVObject::Metadata accelSensorMeta; UAVObject::Metadata baroensorMeta; bool statusSaved; -} thermalCalibrationBoardSettings; +} Memento; typedef struct { bool baroCalibrated; @@ -84,11 +86,17 @@ typedef struct { float baroTempMax; float accelGyroTempMin; float accelGyroTempMax; -} thermalCalibrationResults; +} Results; + class ThermalCalibrationHelper : public QObject { Q_OBJECT + public: + const static float TargetGradient = 0.20f; + const static float TargetTempDelta = 10.0f; + explicit ThermalCalibrationHelper(QObject *parent = 0); + float temperature() { return m_temperature; @@ -99,28 +107,56 @@ public: return m_gradient; } + float range() + { + return fabs(m_maxTemperature - m_minTemperature); + } + int processPercentage() { - return m_processPercentage; + return m_progress; } + void endAcquisition(); bool calibrationSuccessful() { - return m_results.baroCalibrated && - ((m_results.baroTempMax - m_results.baroTempMin) > 10.0f); + 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 statusRestoreCompleted(bool succesful); void statusSaveCompleted(bool succesful); void setupBoardCompleted(bool succesful); - void temperatureChanged(float value); - void gradientChanged(float value); - void processPercentageChanged(int percentage); void collectionCompleted(); void calculationCompleted(); - void abort(); + + 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: /** @@ -152,18 +188,30 @@ public slots: void calculate(); void collectSample(UAVObject *sample); - void setProcessPercentage(int value) + void setProgress(int value) { - if (m_processPercentage != value) { - m_processPercentage = value; - emit processPercentageChanged(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) + { + emit instructionsAdded(text, type); + } void cleanup(); private: - void updateTemp(float temp); + float getTemperature(); + void updateTemperature(float temp); + void connectUAVOs(); void disconnectUAVOs(); @@ -172,7 +220,6 @@ private: QScopedPointer m_tempdir; void createDebugLog(); void closeDebugLog(); - void copyResultToSettings(); QMutex sensorsUpdateLock; @@ -181,23 +228,36 @@ private: QList m_baroSamples; QList 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; - 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.20f; + int m_targetduration; - int m_processPercentage; + + bool m_rangeReached; + + int m_progress; + int m_progressMax; + + // convenience pointers + AccelSensor *accelSensor; + GyroSensor *gyroSensor; + BaroSensor *baroSensor; + MagSensor *magSensor; + AccelGyroSettings *accelGyroSettings; + RevoSettings *revoSettings; /* board settings save/restore */ bool setupBoardForCalibration(); @@ -205,14 +265,14 @@ private: bool restoreInitialSettings(); bool isBoardInitialSettingsSaved() { - return m_boardInitialSettings.statusSaved; + return m_memento.statusSaved; } void clearBoardInitialSettingsSaved() { - m_boardInitialSettings.statusSaved = false; + m_memento.statusSaved = false; } - thermalCalibrationBoardSettings m_boardInitialSettings; - thermalCalibrationResults m_results; + Memento m_memento; + Results m_results; void setMetadataForCalibration(UAVDataObject *uavo); UAVObjectManager *getObjectManager(); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp index 02921236e..92b470365 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.cpp @@ -38,50 +38,53 @@ ThermalCalibrationModel::ThermalCalibrationModel(QObject *parent) : m_startEnabled(false), m_cancelEnabled(false), m_endEnabled(false), - m_initDone(false) + m_dirty(false) { m_helper.reset(new ThermalCalibrationHelper()); - m_readyState = new WizardState(tr("Start"), this), - m_workingState = new WizardState(NULL, this); - m_saveSettingState = new WizardState(tr("Saving initial settings"), m_workingState); + m_readyState = new WizardState("Ready", this), + m_workingState = new WizardState("Working", this); + + m_saveSettingState = new WizardState("Storing Settings", m_workingState); m_workingState->setInitialState(m_saveSettingState); - m_setupState = new WizardState(tr("Setup board for calibration"), m_workingState); + m_setupState = new WizardState("SetupBoard", m_workingState); - m_acquisitionState = new WizardState(tr("*** Please Wait *** Samples acquisition, this can take several minutes"), m_workingState); - m_restoreState = new WizardState(tr("Restore board settings"), m_workingState); - m_calculateState = new WizardState(tr("Calculate calibration data"), m_workingState); + m_acquisitionState = new WizardState("Acquiring", m_workingState); + m_restoreState = new WizardState("Restoring Settings", m_workingState); + m_calculateState = new WizardState("Calculating", m_workingState); - m_abortState = new WizardState(tr("Canceled"), this); + m_abortState = new WizardState("Canceled", this); + + m_completedState = new WizardState("Completed", this); - // note: step name for this state is changed by CompensationCalculationTransition based on result - m_completedState = new WizardState(NULL, this); setTransitions(); - connect(m_helper.data(), SIGNAL(gradientChanged(float)), this, SLOT(setTemperatureGradient(float))); connect(m_helper.data(), SIGNAL(temperatureChanged(float)), this, SLOT(setTemperature(float))); - connect(m_helper.data(), SIGNAL(processPercentageChanged(int)), this, SLOT(setProgress(int))); - connect(m_readyState, SIGNAL(entered()), this, SLOT(wizardReady())); - connect(m_readyState, SIGNAL(exited()), this, SLOT(wizardStarted())); - connect(m_completedState, SIGNAL(entered()), this, SLOT(wizardReady())); - connect(m_completedState, SIGNAL(exited()), this, SLOT(wizardStarted())); - this->setInitialState(m_readyState); + 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))); + connect(m_readyState, SIGNAL(entered()), this, SLOT(stopWizard())); + connect(m_readyState, SIGNAL(exited()), this, SLOT(startWizard())); + connect(m_completedState, SIGNAL(entered()), this, SLOT(stopWizard())); + connect(m_completedState, SIGNAL(exited()), this, SLOT(startWizard())); + + setInitialState(m_readyState); m_steps << m_readyState << m_saveSettingState << m_setupState << m_acquisitionState << m_restoreState << m_calculateState; } + void ThermalCalibrationModel::init() { - if (!m_initDone) { - m_initDone = true; - setStartEnabled(true); - setEndEnabled(false); - setCancelEnabled(false); - start(); - setTemperature(0); - setTemperatureGradient(0); - emit instructionsChanged(instructions()); - } + setStartEnabled(true); + setEndEnabled(false); + setCancelEnabled(false); + setTemperature(NAN); + setTemperatureGradient(NAN); + setTemperatureRange(NAN); + start(); } void ThermalCalibrationModel::stepChanged(WizardState *state) @@ -92,10 +95,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 @@ -112,6 +115,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 310c5024a..79556d3eb 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/thermal/thermalcalibrationmodel.h @@ -30,23 +30,24 @@ #define THERMALCALIBRATIONMODEL_H #include "thermalcalibrationhelper.h" - +#include "../wizardstate.h" +#include "../wizardmodel.h" #include #include #include -#include "../wizardstate.h" -#include "../wizardmodel.h" + namespace OpenPilot { class ThermalCalibrationModel : public WizardModel { - Q_PROPERTY(bool startEnable READ startEnabled NOTIFY startEnabledChanged) + Q_OBJECT Q_PROPERTY(bool startEnable READ startEnabled NOTIFY startEnabledChanged) Q_PROPERTY(bool endEnable READ endEnabled NOTIFY endEnabledChanged) Q_PROPERTY(bool cancelEnable READ cancelEnabled NOTIFY cancelEnabledChanged) - - Q_PROPERTY(QString temperature READ temperature NOTIFY temperatureChanged) - Q_PROPERTY(QString temperatureGradient READ temperatureGradient NOTIFY temperatureGradientChanged) + 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_OBJECT + Q_PROPERTY(int progressMax READ progressMax WRITE setProgressMax NOTIFY progressMaxChanged) + public: explicit ThermalCalibrationModel(QObject *parent = 0); @@ -67,84 +68,95 @@ public: void setStartEnabled(bool status) { - if (m_startEnabled != status) { - m_startEnabled = status; - emit startEnabledChanged(status); - } + m_startEnabled = status; + emit startEnabledChanged(status); } void setEndEnabled(bool status) { - if (m_endEnabled != status) { - m_endEnabled = status; - emit endEnabledChanged(status); - } + m_endEnabled = status; + emit endEnabledChanged(status); } void setCancelEnabled(bool status) { - if (m_cancelEnabled != status) { - m_cancelEnabled = status; - emit cancelEnabledChanged(status); - } + m_cancelEnabled = status; + emit cancelEnabledChanged(status); + } + + float temperature() + { + return m_helper->temperature(); + } + + float temperatureGradient() + { + return m_helper->gradient(); + } + + float temperatureRange() + { + return m_helper->range(); + } + + bool dirty() + { + return m_dirty; } public slots: + void setTemperature(float temperature) + { + emit temperatureChanged(temperature); + } + + void setTemperatureGradient(float temperatureGradient) + { + emit temperatureGradientChanged(temperatureGradient); + } + + void setTemperatureRange(float temperatureRange) + { + emit temperatureRangeChanged(temperatureRange); + + if (m_helper->range() >= ThermalCalibrationHelper::TargetTempDelta) { + setEndEnabled(true); + } + } + int progress() { return m_progress; } - QString temperature() + void setProgress(int value) { - return m_temperature; + m_progress = value; + emit progressChanged(value); } - QString temperatureGradient() + int progressMax() { - return m_temperatureGradient; + return m_progressMax; } - void setTemperature(float status) + void setProgressMax(int value) { - QString tmp = QString("%1").arg(status, 5, 'f', 2); - - if (m_temperature != tmp) { - m_temperature = tmp; - emit temperatureChanged(tmp); - } - } - - void setTemperatureGradient(float status) - { - QString tmp = QString("%1").arg(status, 5, 'f', 2); - - if (m_temperatureGradient != tmp) { - m_temperatureGradient = tmp; - emit temperatureGradientChanged(tmp); - } - } - - void setProgress(int status) - { - m_progress = status; - emit progressChanged(status); - if (this->currentState()) { - setInstructions(this->currentState()->stepName()); - } + m_progressMax = value; + emit progressMaxChanged(value); } private: + QScopedPointer m_helper; + bool m_startEnabled; bool m_cancelEnabled; bool m_endEnabled; - bool m_initDone; - int m_progress; - QString m_temperature; - QString m_temperatureGradient; + bool m_dirty; - QScopedPointer m_helper; + int m_progress; + int m_progressMax; // Start from here WizardState *m_readyState; @@ -165,7 +177,7 @@ private: WizardState *m_finalizeState; // revert board settings if something goes wrong WizardState *m_abortState; - // just the same as readystate, but it is reached after havign completed the calibration + // just the same as ready state, but it is reached after having completed the calibration WizardState *m_completedState; void setTransitions(); @@ -174,41 +186,58 @@ signals: void endEnabledChanged(bool state); void cancelEnabledChanged(bool state); - void temperatureChanged(QString status); - void temperatureGradientChanged(QString status); + void wizardStarted(); + void wizardStopped(); + + void temperatureChanged(float temperature); + void temperatureGradientChanged(float temperatureGradient); + void temperatureRangeChanged(float temperatureRange); + void progressChanged(int value); + void progressMaxChanged(int value); void next(); void previous(); void abort(); + public slots: void stepChanged(WizardState *state); void init(); void btnStart() { + init(); emit next(); } - void btnEnd() { m_helper->stopAcquisition(); } - void btnAbort() { emit abort(); } - void wizardReady() + void startWizard() { + m_dirty = false; + setStartEnabled(false); + setEndEnabled(false); + setCancelEnabled(true); + wizardStarted(); + } + void stopWizard() + { + m_dirty = m_helper->calibrationSuccessful(); setStartEnabled(true); setEndEnabled(false); setCancelEnabled(false); + wizardStopped(); } - void wizardStarted() + void save() { - setStartEnabled(false); - setEndEnabled(true); - setCancelEnabled(true); + if (m_dirty) { + m_dirty = false; + m_helper->copyResultToSettings(); + } } }; } diff --git a/ground/openpilotgcs/src/plugins/config/calibration/wizardmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/wizardmodel.h index d07fc6885..25ac0424d 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/wizardmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/wizardmodel.h @@ -28,16 +28,20 @@ #ifndef WIZARDMODEL_H #define WIZARDMODEL_H -#include -#include #include "wizardstate.h" #include +#include +#include + class WizardModel : public QStateMachine { Q_OBJECT Q_PROPERTY(QQmlListProperty steps READ steps CONSTANT) - Q_PROPERTY(QString instructions READ instructions NOTIFY instructionsChanged) + // Q_PROPERTY(QString instructions READ instructions NOTIFY instructionsChanged) Q_PROPERTY(WizardState * currentState READ currentState NOTIFY currentStateChanged) + public: + enum MessageType { Debug, Info, Prompt, Warn, Success, Failure }; + explicit WizardModel(QObject *parent = 0); QQmlListProperty steps() @@ -45,25 +49,29 @@ public: return QQmlListProperty(this, m_steps); } - QString instructions() + QString &instructions() { return m_instructions; } - void setInstructions(QString instructions) - { - m_instructions = instructions; - emit instructionsChanged(instructions); - } WizardState *currentState(); + +public slots: + void addInstructions(QString text, WizardModel::MessageType type = WizardModel::Info) + { + m_instructions = text; + emit instructionsAdded(text, type); + } + protected: QList m_steps; + private: QString m_instructions; + signals: - void instructionsChanged(QString status); + void instructionsAdded(QString text, WizardModel::MessageType type = WizardModel::Info); void currentStateChanged(WizardState *status); -public slots: }; #endif // WIZARDMODEL_H diff --git a/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.cpp b/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.cpp index c94cdb50d..136a46095 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.cpp +++ b/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.cpp @@ -27,6 +27,7 @@ */ #include "wizardstate.h" #include "QDebug" + WizardState::WizardState(QString name, QState *parent) : QState(parent) { @@ -41,12 +42,6 @@ void WizardState::setCompletion(qint8 completion) emit completionChanged(); } -void WizardState::setStepName(QString name) -{ - m_stepName = name; - emit stepNameChanged(); -} - void WizardState::onEntry(QEvent *event) { Q_UNUSED(event); diff --git a/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.h b/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.h index 55f8f267e..6d4d9352a 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/wizardstate.h @@ -34,9 +34,11 @@ class WizardState : public QState { Q_OBJECT Q_PROPERTY(bool isActive READ isActive NOTIFY isActiveChanged) Q_PROPERTY(bool isDone READ isDone NOTIFY isDoneChanged) Q_PROPERTY(qint8 completion READ completion NOTIFY completionChanged) - Q_PROPERTY(QString stepName READ stepName WRITE setStepName NOTIFY stepNameChanged) + Q_PROPERTY(QString stepName READ stepName) + public: explicit WizardState(QString name, QState *parent = 0); + bool isActive() { return m_active; @@ -57,17 +59,18 @@ public: return m_stepName; } - void setStepName(QString name); void setCompletion(qint8 completion); virtual void onEntry(QEvent *event) Q_DECL_OVERRIDE; virtual void onExit(QEvent *event) Q_DECL_OVERRIDE; + signals: void isActiveChanged(); void isDoneChanged(); - void stepNameChanged(); void completionChanged(); + public slots: void clean(); + private: void setIsDone(bool done); bool m_done; diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index e36b0fda7..54e1d1a9b 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -26,20 +26,6 @@ */ #include "configrevowidget.h" -#include "math.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include @@ -52,14 +38,29 @@ #include "assertions.h" #include "calibration.h" #include "calibration/calibrationutils.h" -#define sign(x) ((x < 0) ? -1 : 1) + +#include "math.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +// #define DEBUG // Uncomment this to enable 6 point calibration on the accels #define NOISE_SAMPLES 50 - -// ***************** - class Thread : public QThread { public: static void usleep(unsigned long usecs) @@ -68,87 +69,110 @@ public: } }; -// ***************** - ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent), m_ui(new Ui_RevoSensorsWidget()), isBoardRotationStored(false) { m_ui->setupUi(this); + m_ui->tabWidget->setCurrentIndex(0); - // Initialization of the Paper plane widget + addApplySaveButtons(m_ui->revoCalSettingsSaveRAM, m_ui->revoCalSettingsSaveSD); + + // Initialization of the visual help m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this)); m_ui->calibrationVisualHelp->setRenderHint(QPainter::HighQualityAntialiasing, true); m_ui->calibrationVisualHelp->setRenderHint(QPainter::SmoothPixmapTransform, true); + m_ui->calibrationVisualHelp->setBackgroundBrush(QBrush(QColor(51, 51, 51))); displayVisualHelp("empty"); + // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // will be dealing with some null pointers - addUAVObject("RevoCalibration"); addUAVObject("HomeLocation"); - addUAVObject("AttitudeSettings"); - addUAVObject("RevoSettings"); - addUAVObject("AccelGyroSettings"); autoLoadWidgets(); - // connect the thermalCalibration model to UI - m_thermalCalibrationModel = new OpenPilot::ThermalCalibrationModel(this); + // accel calibration + m_accelCalibrationModel = new OpenPilot::SixPointCalibrationModel(this); + connect(m_ui->accelStart, SIGNAL(clicked()), m_accelCalibrationModel, SLOT(accelStart())); + connect(m_ui->accelSavePos, SIGNAL(clicked()), m_accelCalibrationModel, SLOT(savePositionData())); - 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())); + connect(m_accelCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations())); + connect(m_accelCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations())); + connect(m_accelCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation())); + connect(m_accelCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation())); + connect(m_accelCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)), + this, SLOT(addInstructions(QString, WizardModel::MessageType))); + connect(m_accelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString))); + connect(m_accelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->accelSavePos, SLOT(setEnabled(bool))); + connect(m_accelCalibrationModel, SIGNAL(progressChanged(int)), m_ui->accelProgress, SLOT(setValue(int))); + m_ui->accelSavePos->setEnabled(false); - connect(m_thermalCalibrationModel, SIGNAL(startEnabledChanged(bool)), m_ui->ThermalBiasStart, SLOT(setEnabled(bool))); - connect(m_thermalCalibrationModel, SIGNAL(endEnabledChanged(bool)), m_ui->ThermalBiasEnd, SLOT(setEnabled(bool))); - connect(m_thermalCalibrationModel, SIGNAL(cancelEnabledChanged(bool)), m_ui->ThermalBiasCancel, SLOT(setEnabled(bool))); + // mag calibration + m_magCalibrationModel = new OpenPilot::SixPointCalibrationModel(this); + connect(m_ui->magStart, SIGNAL(clicked()), m_magCalibrationModel, SLOT(magStart())); + connect(m_ui->magSavePos, SIGNAL(clicked()), m_magCalibrationModel, SLOT(savePositionData())); - connect(m_thermalCalibrationModel, SIGNAL(instructionsChanged(QString)), m_ui->label_thermalDescription, SLOT(setText(QString))); - connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(QString)), m_ui->textTemperature, SLOT(setText(QString))); - connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(QString)), m_ui->textThermalGradient, SLOT(setText(QString))); - connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int))); - // note: init for m_thermalCalibrationModel is done in showEvent to prevent cases wiht "Start" button not enabled due to some itming issue. - - m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this); - // six point calibrations - connect(m_ui->sixPointsStartAccel, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(accelStart())); - connect(m_ui->sixPointsStartMag, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(magStart())); - connect(m_ui->sixPointsSave, SIGNAL(clicked()), m_sixPointCalibrationModel, SLOT(savePositionData())); - - connect(m_sixPointCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations())); - connect(m_sixPointCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations())); - connect(m_sixPointCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation())); - connect(m_sixPointCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation())); - connect(m_sixPointCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool))); - connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString))); - connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool))); + connect(m_magCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations())); + connect(m_magCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations())); + connect(m_magCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation())); + connect(m_magCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation())); + connect(m_magCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)), + this, SLOT(addInstructions(QString, WizardModel::MessageType))); + connect(m_magCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString))); + connect(m_magCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->magSavePos, SLOT(setEnabled(bool))); + connect(m_magCalibrationModel, SIGNAL(progressChanged(int)), m_ui->magProgress, SLOT(setValue(int))); + m_ui->magSavePos->setEnabled(false); + // board level calibration m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel(this); - // level calibration connect(m_ui->boardLevelStart, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(start())); connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), m_levelCalibrationModel, SLOT(savePosition())); - connect(m_levelCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations())); - connect(m_levelCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations())); - connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool))); + connect(m_levelCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations())); + connect(m_levelCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations())); + connect(m_levelCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)), + this, SLOT(addInstructions(QString, WizardModel::MessageType))); connect(m_levelCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString))); - connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->boardLevelSavePos, SLOT(setEnabled(bool))); - connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->boardLevelProgress, SLOT(setValue(int))); + connect(m_levelCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), m_ui->boardLevelSavePos, SLOT(setEnabled(bool))); + connect(m_levelCalibrationModel, SIGNAL(progressChanged(int)), m_ui->boardLevelProgress, SLOT(setValue(int))); + m_ui->boardLevelSavePos->setEnabled(false); - // Connect the signals // gyro zero calibration m_gyroBiasCalibrationModel = new OpenPilot::GyroBiasCalibrationModel(this); connect(m_ui->gyroBiasStart, SIGNAL(clicked()), m_gyroBiasCalibrationModel, SLOT(start())); - connect(m_gyroBiasCalibrationModel, SIGNAL(progressChanged(int)), this->m_ui->gyroBiasProgress, SLOT(setValue(int))); + connect(m_gyroBiasCalibrationModel, SIGNAL(progressChanged(int)), m_ui->gyroBiasProgress, SLOT(setValue(int))); - connect(m_gyroBiasCalibrationModel, SIGNAL(disableAllCalibrations()), this, SLOT(disableAllCalibrations())); - connect(m_gyroBiasCalibrationModel, SIGNAL(enableAllCalibrations()), this, SLOT(enableAllCalibrations())); + connect(m_gyroBiasCalibrationModel, SIGNAL(started()), this, SLOT(disableAllCalibrations())); + connect(m_gyroBiasCalibrationModel, SIGNAL(stopped()), this, SLOT(enableAllCalibrations())); connect(m_gyroBiasCalibrationModel, SIGNAL(storeAndClearBoardRotation()), this, SLOT(storeAndClearBoardRotation())); connect(m_gyroBiasCalibrationModel, SIGNAL(recallBoardRotation()), this, SLOT(recallBoardRotation())); - connect(m_gyroBiasCalibrationModel, SIGNAL(displayInstructions(QString, bool)), this, SLOT(displayInstructions(QString, bool))); + connect(m_gyroBiasCalibrationModel, SIGNAL(displayInstructions(QString, WizardModel::MessageType)), + this, SLOT(addInstructions(QString, WizardModel::MessageType))); connect(m_gyroBiasCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString))); + // thermal calibration + m_thermalCalibrationModel = new OpenPilot::ThermalCalibrationModel(this); + 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())); + connect(m_thermalCalibrationModel, SIGNAL(startEnabledChanged(bool)), m_ui->thermalBiasStart, SLOT(setEnabled(bool))); + connect(m_thermalCalibrationModel, SIGNAL(endEnabledChanged(bool)), m_ui->thermalBiasEnd, SLOT(setEnabled(bool))); + connect(m_thermalCalibrationModel, SIGNAL(cancelEnabledChanged(bool)), m_ui->thermalBiasCancel, SLOT(setEnabled(bool))); + connect(m_thermalCalibrationModel, SIGNAL(wizardStarted()), this, SLOT(disableAllCalibrations())); + connect(m_thermalCalibrationModel, SIGNAL(wizardStopped()), this, SLOT(enableAllCalibrations())); + + connect(m_thermalCalibrationModel, SIGNAL(instructionsAdded(QString, WizardModel::MessageType)), + 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_thermalCalibrationModel->init(); + + // home location connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation())); addWidgetBinding("RevoSettings", "FusionAlgorithm", m_ui->FusionAlgorithm); @@ -160,8 +184,9 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : populateWidgets(); refreshWidgetsValues(); - m_ui->tabWidget->setCurrentIndex(0); enableAllCalibrations(); + + forceConnectedState(); } ConfigRevoWidget::~ConfigRevoWidget() @@ -169,20 +194,22 @@ ConfigRevoWidget::~ConfigRevoWidget() // Do nothing } - void ConfigRevoWidget::showEvent(QShowEvent *event) { Q_UNUSED(event); - m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); - m_thermalCalibrationModel->init(); + updateVisualHelp(); } void ConfigRevoWidget::resizeEvent(QResizeEvent *event) { Q_UNUSED(event); - m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + updateVisualHelp(); } +void ConfigRevoWidget::updateVisualHelp() +{ + m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatio); +} void ConfigRevoWidget::storeAndClearBoardRotation() { @@ -229,20 +256,77 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID) QPixmap pixmap = QPixmap(":/configgadget/images/calibration/" + elementID + ".png"); m_ui->calibrationVisualHelp->scene()->addPixmap(pixmap); m_ui->calibrationVisualHelp->setSceneRect(pixmap.rect()); - m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::IgnoreAspectRatio); + updateVisualHelp(); } -void ConfigRevoWidget::displayInstructions(QString instructions, bool replace) +void ConfigRevoWidget::clearInstructions() { - if (replace || instructions.isNull()) { - m_ui->calibrationInstructions->clear(); + m_ui->calibrationInstructions->clear(); +} + +void ConfigRevoWidget::addInstructions(QString text, WizardModel::MessageType type) +{ + QString msg; + + switch (type) { + case WizardModel::Debug: +#ifdef DEBUG + msg = QString("%1").arg(text); +#endif + break; + case WizardModel::Info: + msg = QString("%1").arg(text); + break; + case WizardModel::Prompt: + msg = QString("%1").arg(text); + break; + case WizardModel::Warn: + msg = QString("%1").arg(text); + break; + case WizardModel::Success: + msg = QString("%1").arg(text); + break; + case WizardModel::Failure: + msg = QString("%1").arg(text); + break; + default: + break; } - if (!instructions.isNull()) { - m_ui->calibrationInstructions->append(instructions); + if (!msg.isEmpty()) { + m_ui->calibrationInstructions->append(msg); } } -/********** UI Functions *************/ +static QString format(float v) +{ + QString str; + + if (!std::isnan(v)) { + // format as ##.## + str = QString("%1").arg(v, 5, 'f', 2, ' '); + str = str.replace(" ", " "); + } else { + str = "--.--"; + } + // use a fixed width font + QString style("font-family:courier new,monospace;"); + return QString("%2").arg(style).arg(str); +} + +void ConfigRevoWidget::displayTemperature(float temperature) +{ + 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("Sampled range: %1°C").arg(format(temperatureRange))); +} /** * Called by the ConfigTaskWidget parent when RevoCalibration is updated @@ -252,9 +336,6 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) { ConfigTaskWidget::refreshWidgetsValues(object); - - m_ui->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); - m_ui->isSetCheckBox->setEnabled(false); HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); @@ -265,6 +346,27 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) m_ui->beBox->setText(beStr); } +void ConfigRevoWidget::updateObjectsFromWidgets() +{ + ConfigTaskWidget::updateObjectsFromWidgets(); + + if (m_accelCalibrationModel->dirty()) { + m_accelCalibrationModel->save(); + } + if (m_magCalibrationModel->dirty()) { + m_magCalibrationModel->save(); + } + if (m_levelCalibrationModel->dirty()) { + m_levelCalibrationModel->save(); + } + if (m_gyroBiasCalibrationModel->dirty()) { + m_gyroBiasCalibrationModel->save(); + } + if (m_thermalCalibrationModel->dirty()) { + m_thermalCalibrationModel->save(); + } +} + void ConfigRevoWidget::clearHomeLocation() { HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); @@ -284,18 +386,27 @@ void ConfigRevoWidget::clearHomeLocation() void ConfigRevoWidget::disableAllCalibrations() { - m_ui->sixPointsStartAccel->setEnabled(false); - m_ui->sixPointsStartMag->setEnabled(false); + clearInstructions(); + + m_ui->accelStart->setEnabled(false); + m_ui->magStart->setEnabled(false); m_ui->boardLevelStart->setEnabled(false); m_ui->gyroBiasStart->setEnabled(false); - m_ui->ThermalBiasStart->setEnabled(false); + m_ui->thermalBiasStart->setEnabled(false); } void ConfigRevoWidget::enableAllCalibrations() { - m_ui->sixPointsStartAccel->setEnabled(true); - m_ui->sixPointsStartMag->setEnabled(true); + // TODO this logic should not be here and should use a signal instead + // need to check if ConfigTaskWidget has support for this kind of use cases + if (m_accelCalibrationModel->dirty() || m_magCalibrationModel->dirty() || m_levelCalibrationModel->dirty() + || m_gyroBiasCalibrationModel->dirty() || m_thermalCalibrationModel->dirty()) { + widgetsContentsChanged(); + } + + m_ui->accelStart->setEnabled(true); + m_ui->magStart->setEnabled(true); m_ui->boardLevelStart->setEnabled(true); m_ui->gyroBiasStart->setEnabled(true); - m_ui->ThermalBiasStart->setEnabled(true); + m_ui->thermalBiasStart->setEnabled(true); } diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 62f1e48fb..5b19aa548 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -29,22 +29,22 @@ #include "ui_revosensors.h" #include "configtaskwidget.h" -#include "../uavobjectwidgetutils/configtaskwidget.h" #include "extensionsystem/pluginmanager.h" #include "uavobjectmanager.h" #include "uavobject.h" +#include "calibration/thermal/thermalcalibrationmodel.h" +#include "calibration/sixpointcalibrationmodel.h" +#include "calibration/levelcalibrationmodel.h" +#include "calibration/gyrobiascalibrationmodel.h" + #include #include #include #include #include #include -#include "calibration/thermal/thermalcalibrationmodel.h" -#include "calibration/sixpointcalibrationmodel.h" -#include "calibration/levelcalibrationmodel.h" -#include "calibration/gyrobiascalibrationmodel.h" -class Ui_Widget; +class Ui_Widget; class ConfigRevoWidget : public ConfigTaskWidget { Q_OBJECT @@ -54,10 +54,11 @@ public: ~ConfigRevoWidget(); private: - OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel; - OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel; + OpenPilot::SixPointCalibrationModel *m_accelCalibrationModel; + OpenPilot::SixPointCalibrationModel *m_magCalibrationModel; OpenPilot::LevelCalibrationModel *m_levelCalibrationModel; OpenPilot::GyroBiasCalibrationModel *m_gyroBiasCalibrationModel; + OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel; Ui_RevoSensorsWidget *m_ui; @@ -66,13 +67,18 @@ private: bool isBoardRotationStored; private slots: - void displayVisualHelp(QString elementID); void storeAndClearBoardRotation(); void recallBoardRotation(); - void displayInstructions(QString instructions = QString(), bool replace = false); + void displayVisualHelp(QString elementID); + void clearInstructions(); + void addInstructions(QString text, WizardModel::MessageType type = WizardModel::Info); + 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); + virtual void updateObjectsFromWidgets(); // Slot for clearing home location void clearHomeLocation(); @@ -80,6 +86,8 @@ private slots: void disableAllCalibrations(); void enableAllCalibrations(); + void updateVisualHelp(); + protected: void showEvent(QShowEvent *event); void resizeEvent(QResizeEvent *event); diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index b53d7ba6d..2b9654beb 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -7,7 +7,7 @@ 0 0 796 - 828 + 591 @@ -117,7 +117,7 @@ 0 0 772 - 751 + 514 @@ -542,8 +542,8 @@ 0 0 - 772 - 751 + 724 + 497 @@ -2044,8 +2044,8 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread 0 0 - 772 - 751 + 407 + 138 @@ -2234,7 +2234,7 @@ Set to 0 to disable (recommended for soaring fixed wings). - + :/core/images/helpicon.svg:/core/images/helpicon.svg diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 7d75bea2d..d7f2538ed 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -7,7 +7,7 @@ 0 0 834 - 772 + 652 @@ -49,8 +49,8 @@ 0 0 - 812 - 692 + 810 + 575 @@ -1761,6 +1761,9 @@ + + 4 + @@ -1776,7 +1779,19 @@ + + + 0 + 0 + + + + 0 + 0 + + + 25 25 diff --git a/ground/openpilotgcs/src/plugins/config/revosensors.ui b/ground/openpilotgcs/src/plugins/config/revosensors.ui index a93d14032..c563269b4 100644 --- a/ground/openpilotgcs/src/plugins/config/revosensors.ui +++ b/ground/openpilotgcs/src/plugins/config/revosensors.ui @@ -6,8 +6,8 @@ 0 0 - 836 - 605 + 1014 + 725 @@ -22,7 +22,7 @@ 0 - + true @@ -31,356 +31,95 @@ - 0 + 6 - 0 + 12 - 0 + 12 - 0 + 12 - 0 + 12 - - - 12 - - - 12 - - - 12 - - - 12 - - + + 6 - - - - - - Qt::ScrollBarAlwaysOff - - - Qt::ScrollBarAlwaysOff - - - - - - - - - - - - 10 - 75 - true - - - - Calibration status - - - Qt::ScrollBarAlwaysOff - - - QAbstractScrollArea::AdjustToContents - - - - - - - Calibration instructions - - - Qt::ScrollBarAlwaysOff - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Ubuntu'; font-size:9pt; font-weight:400; font-style:normal;"> -<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> -<tr> -<td style="border: none;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:16pt; font-weight:600; font-style:italic;">Help</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Following steps #1, #2 and #3 are necessary. Step #4 is optional, it may helps you achieve the best possible results.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'MS Shell Dlg 2'; font-size:8pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#1: Multi-Point calibration</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This calibration will compute the scale for Magnetometer or Accelerometer sensors. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Press &quot;Calibrate Mag&quot; or &quot;Calibrate Accel&quot; to begin calibration, and follow the instructions which will be displayed here. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">For optimal calibration perform the Accel calibration with the board not mounted to the craft. in this way you can accurately level the board on your desk/table during the process. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">Magnetometer calibration need to be performed inside your plane/copter to account for metal/magnetic stuffs on board.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 1:</span><span style=" font-size:11pt;"> Your </span><span style=" font-size:11pt; font-weight:600;">HomeLocation must be set first</span><span style=" font-size:11pt;">, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt; font-weight:600; font-style:italic;">Note 2:</span><span style=" font-size:11pt;"> There is no need to point exactly at South/North/West/East. They are just used to easily tells the user how to point the plane/craft. </span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">You can simply assume that North is in front of you, right is East etc. and perform the calibration this way.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#2: Board level calibration</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press Start below and do not move your airframe at all until the end of the calibration.</span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#3: Gyro Bias calculation</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This step allows to calibrate the gyro measured value when the board is steady. To perform the calibration leave the board/aircraft completely steady and click start. </span></p> -<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:14pt; font-weight:600; font-style:italic;">#4: Thermal Calibration</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">The calibration will compute sensors bias variations at different temperatures while the board warms up.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">This allow a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:11pt;">To perform this calibration leave the board to cool down at room temperature in the coldest places available. after 15-20 minutes attach the usb connector to the board and Click the Calibrate button leaving the board steady. Wait until completed</span></p> -<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:11pt;"><br /></p></td></tr></table></body></html> - - - - - - - - - #1: Accelerometer/Magnetometer calibration + + 0 + + + 0 + + + 0 + + + 0 + + + + + + 0 + 0 + + + + QFrame::NoFrame + + + 0 + + + Qt::ScrollBarAlwaysOff + + + Qt::ScrollBarAlwaysOff + + + QAbstractScrollArea::AdjustToContents + + + false - - - - - false - - - Launch accelerometer range and bias calibration. - - - Calibrate Accel - - - - - - - false - - - Launch magnetometer range and bias calibration. - - - Calibrate Mag - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - false - - - Save settings (only enabled when calibration is running) - - - Save Position - - - - - - - - #2: Board level calibration + + + + 6 - - - - - false - - - Start - - - - - - - 0 - - - true - - - - - - - false - - - Save Position - - - - - - - - - - #3: Gyro bias calibration + + 0 - - - - - false - - - Start - - - - - - - 0 - - - true - - - - - - - - - - #4*: Thermal calibration + + 0 - - - + + 0 + + + 0 + + + + + Accelerometer calibration + + - - - - 50 - false - - - - Temperature - - - - - - - - 50 - true - false - - - - 0.00 - - - - - - - - 50 - false - - - - °C - Temperature rise - - - - - - - - 50 - true - false - - - - 0.5 - - - - - - - - 50 - false - - - - °C/min - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - 50 - false - - - - this contains the current status of the thermal calibration wizard - - - - - - - + - false + true + + + Launch accelerometer range and bias calibration. Start @@ -388,7 +127,85 @@ p, li { white-space: pre-wrap; } - + + + 0 + + + + + + + true + + + Save settings (only enabled when calibration is running) + + + Save Position + + + + + + + + + + Magnetometer calibration + + + + + + true + + + Launch magnetometer range and bias calibration. + + + Start + + + + + + + 0 + + + + + + + true + + + Save Position + + + + + + + + + + Board level calibration + + + + + + true + + + Start + + + + + 0 @@ -398,35 +215,257 @@ p, li { white-space: pre-wrap; } - + - false + true - End - - - - - - - false - - - Cancel + Save Position - - - + + + + + + Gyro bias calibration + + + + + + true + + + + 0 + 0 + + + + Start + + + + + + + + 300 + 0 + + + + + 16777215 + 16777215 + + + + 0 + + + true + + + + + + + + + + Thermal calibration + + + + + + + + true + + + Start + + + + + + + + 300 + 0 + + + + + 16777215 + 16777215 + + + + 0 + + + true + + + + + + + true + + + End + + + + + + + true + + + Cancel + + + + + + + + + 12 + + + 0 + + + 0 + + + + + + 0 + 0 + + + + + 50 + false + + + + <temperature> + + + + + + + + 0 + 0 + + + + <gradient> + + + + + + + <range> + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + + + + + + + 0 + 0 + + + + + 10 + 50 + false + + + + Calibration status + + + QFrame::StyledPanel + + + Qt::ScrollBarAlwaysOn + + + Qt::ScrollBarAlwaysOff + + + QAbstractScrollArea::AdjustIgnored + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:10pt; font-weight:400; font-style:normal;"> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-weight:600;"><br /></p></body></html> + + + Qt::NoTextInteraction + + + + + + + + Qt::Vertical + + + + 20 + 40 + + + + - + true @@ -911,17 +950,72 @@ A setting of 0.00 disables the filter. + + + Help + + + + + + Calibration instructions + + + Qt::ScrollBarAlwaysOff + + + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'MS Shell Dlg 2'; font-size:8.25pt; font-weight:400; font-style:normal;"> +<table border="0" style="-qt-table-type: root; margin-top:4px; margin-bottom:4px; margin-left:4px; margin-right:4px;"> +<tr> +<td style="border: none;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:14pt; font-weight:600; font-style:italic;">Help</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Steps 1, 2 and 3 are necessary.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Step 4 is optional but may help achieve the best possible results.</span></p> +<p align="center" style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600; font-style:italic;">Step 1: Accelerometer and Magnetometer calibration</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">These calibrations will compute the scale for Magnetometer and Accelerometer sensors. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Press </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Start</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> to begin calibration and follow the instructions which will be displayed. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">For optimal calibration, perform the acceleration calibration with the board not mounted in the craft.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">In this way you can accurately level the board on your desk/table during the process. </span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Magnetometer calibration needs to be performed inside your plane/copter to account for metal/magnetic elements on board.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Note 1</span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">:</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> </span><span style=" font-family:'Ubuntu'; font-size:11pt; text-decoration: underline;">Your HomeLocation must be set first</span><span style=" font-family:'Ubuntu'; font-size:11pt;">, including the local magnetic field vector (Be) and acceleration due to gravity (g_e).</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">Note 2</span><span style=" font-family:'Ubuntu'; font-size:11pt; font-weight:600;">:</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> There is no need to point exactly at South/North/West/East. These are just used to easily tell the user how to point the plane/craft. </span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">You can simply assume that North is in front of you, right is East, etc., and perform the calibration this way.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600; font-style:italic;">Step 2: Board level calibration</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This step will ensure that board leveling is accurate. Place your airframe as horizontally as possible (use a spirit level if necessary), then press </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Start</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> and do not move your airframe at all until the end of the calibration.</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600; font-style:italic;">Step 3: Gyro bias calculation</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This step allows to calibrate the gyro measured value when the board is steady. To perform the calibration leave the board/aircraft completely steady and pres </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Start</span><span style=" font-family:'Ubuntu'; font-size:11pt;">. </span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-family:'Ubuntu'; font-size:11pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:12pt; font-weight:600; font-style:italic;">Step 4: Thermal calibration</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:8pt;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">The calibration will compute sensors bias variations at different temperatures while the board warms up.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">This allows a certain amount of correction of those bias variations against temperature changes. It does improve both altitude hold and yaw performances.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">To perform this calibration leave the board to cool down at room temperature in the coldest places available.</span></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-family:'Ubuntu'; font-size:11pt;">After 15-20 minutes, attach the usb connector to the board and press </span><span style=" font-family:'Ubuntu'; font-size:11pt; font-style:italic;">Start,</span><span style=" font-family:'Ubuntu'; font-size:11pt;"> leaving the board steady. Wait until completed.</span></p></td></tr></table></body></html> + + + Qt::LinksAccessibleByKeyboard|Qt::LinksAccessibleByMouse|Qt::TextBrowserInteraction|Qt::TextSelectableByKeyboard|Qt::TextSelectableByMouse + + + + + - - - - Telemetry link not established. - - - @@ -930,7 +1024,7 @@ A setting of 0.00 disables the filter. 40 - 20 + 5 @@ -945,8 +1039,8 @@ A setting of 0.00 disables the filter. - 32 - 32 + 25 + 25 @@ -963,8 +1057,8 @@ A setting of 0.00 disables the filter. - 32 - 32 + 25 + 25