From 934a11bd963ea60aad488756187a32f3c6aa0146 Mon Sep 17 00:00:00 2001 From: Philippe Renon Date: Fri, 30 May 2014 15:09:07 +0200 Subject: [PATCH] OP-1351 Apply and Save logic for gyro bias calibration now works as expected --- .../calibration/gyrobiascalibrationmodel.cpp | 163 ++++++++---------- .../calibration/gyrobiascalibrationmodel.h | 36 +++- .../src/plugins/config/configrevowidget.cpp | 76 ++++---- .../src/plugins/config/configrevowidget.h | 1 + 4 files changed, 147 insertions(+), 129 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp index 3c29a5451..5447e2177 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; 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(); - started(); - 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 vehicle steady...")); + 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 *))); - stopped(); - - 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 completed succesfully."), WizardModel::Success); - 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 succesfully."), 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 5ab397afd..e979ca621 100644 --- a/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h +++ b/ground/openpilotgcs/src/plugins/config/calibration/gyrobiascalibrationmodel.h @@ -34,6 +34,12 @@ #include "uavobjectmanager.h" #include "uavobject.h" +#include +#include +#include +#include +#include + #include namespace OpenPilot { @@ -43,27 +49,41 @@ class GyroBiasCalibrationModel : public QObject { public: explicit GyroBiasCalibrationModel(QObject *parent = 0); + bool dirty() + { + return m_dirty; + } signals: - void displayVisualHelp(QString elementID); - void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info); 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; @@ -71,8 +91,14 @@ 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(); }; } diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index c686a1e70..fe40aa800 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -26,6 +26,19 @@ */ #include "configrevowidget.h" +#include +#include +#include +#include +#include +#include + +#include + +#include "assertions.h" +#include "calibration.h" +#include "calibration/calibrationutils.h" + #include "math.h" #include #include @@ -40,26 +53,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include - -#include "assertions.h" -#include "calibration.h" -#include "calibration/calibrationutils.h" #define sign(x) ((x < 0) ? -1 : 1) // 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,14 +67,15 @@ public: } }; -// ***************** - ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent), m_ui(new Ui_RevoSensorsWidget()), isBoardRotationStored(false) { m_ui->setupUi(this); + m_ui->tabWidget->setCurrentIndex(0); + + addApplySaveButtons(m_ui->revoCalSettingsSaveRAM, m_ui->revoCalSettingsSaveSD); // Initialization of the visual help m_ui->calibrationVisualHelp->setScene(new QGraphicsScene(this)); @@ -86,11 +86,11 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues // will be dealing with some null pointers - addUAVObject("RevoCalibration"); +// addUAVObject("RevoCalibration"); addUAVObject("HomeLocation"); - addUAVObject("AttitudeSettings"); - addUAVObject("RevoSettings"); - addUAVObject("AccelGyroSettings"); +// addUAVObject("AttitudeSettings"); +// addUAVObject("RevoSettings"); +// addUAVObject("AccelGyroSettings"); autoLoadWidgets(); // thermal calibration @@ -114,7 +114,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : connect(m_thermalCalibrationModel, SIGNAL(temperatureChanged(float)), this, SLOT(displayTemperature(float))); connect(m_thermalCalibrationModel, SIGNAL(temperatureGradientChanged(float)), this, SLOT(displayTemperatureGradient(float))); connect(m_thermalCalibrationModel, SIGNAL(progressChanged(int)), m_ui->thermalBiasProgress, SLOT(setValue(int))); - // note: init for m_thermalCalibrationModel is done in showEvent to prevent cases with "Start" button not enabled due to some timing issue. // six point calibration m_sixPointCalibrationModel = new OpenPilot::SixPointCalibrationModel(this); @@ -169,8 +168,9 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : populateWidgets(); refreshWidgetsValues(); - m_ui->tabWidget->setCurrentIndex(0); enableAllCalibrations(); + + forceConnectedState(); } ConfigRevoWidget::~ConfigRevoWidget() @@ -246,17 +246,21 @@ void ConfigRevoWidget::displayVisualHelp(QString elementID) void ConfigRevoWidget::clearInstructions() { m_ui->calibrationInstructions->clear(); + // addInstructions(tr("Press any Start button to start a calibration step."), WizardModel::Prompt); } void ConfigRevoWidget::addInstructions(QString text, WizardModel::MessageType type) { if (!text.isNull()) { switch (type) { - case WizardModel::Failure: - text = QString("%1").arg(text); - break; case WizardModel::Prompt: - text = QString("%1").arg(text); + text = QString("%1").arg(text); + break; + case WizardModel::Success: + text = QString("%1").arg(text); + break; + case WizardModel::Failure: + text = QString("%1").arg(text); break; default: break; @@ -275,8 +279,6 @@ void ConfigRevoWidget::displayTemperatureGradient(float tempGradient) m_ui->temperatureGradientLabel->setText(tr("Temperature rise %1 °C/min").arg(tempGradient, 5, 'f', 2)); } -/********** UI Functions *************/ - /** * Called by the ConfigTaskWidget parent when RevoCalibration is updated * to update the UI @@ -285,9 +287,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()); @@ -298,6 +297,15 @@ void ConfigRevoWidget::refreshWidgetsValues(UAVObject *object) m_ui->beBox->setText(beStr); } +void ConfigRevoWidget::updateObjectsFromWidgets() +{ + ConfigTaskWidget::updateObjectsFromWidgets(); + + if (m_gyroBiasCalibrationModel->dirty()) { + m_gyroBiasCalibrationModel->save(); + } +} + void ConfigRevoWidget::clearHomeLocation() { HomeLocation *homeLocation = HomeLocation::GetInstance(getObjectManager()); @@ -318,6 +326,7 @@ void ConfigRevoWidget::clearHomeLocation() void ConfigRevoWidget::disableAllCalibrations() { clearInstructions(); + m_ui->sixPointsStartAccel->setEnabled(false); m_ui->sixPointsStartMag->setEnabled(false); m_ui->boardLevelStart->setEnabled(false); @@ -327,6 +336,11 @@ void ConfigRevoWidget::disableAllCalibrations() void ConfigRevoWidget::enableAllCalibrations() { + // TODO should use a signal from m_gyroBiasCalibrationModel instead + if (m_gyroBiasCalibrationModel->dirty()) { + widgetsContentsChanged(); + } + m_ui->sixPointsStartAccel->setEnabled(true); m_ui->sixPointsStartMag->setEnabled(true); m_ui->boardLevelStart->setEnabled(true); diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index f88737f0f..7d84aa5e9 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -75,6 +75,7 @@ private slots: // ! Overriden method from the configTaskWidget to update UI virtual void refreshWidgetsValues(UAVObject *object = NULL); + virtual void updateObjectsFromWidgets(); // Slot for clearing home location void clearHomeLocation();