From 2249510c52d0bc3927e8a3a48725fe28ee302819 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 11 Apr 2014 01:45:36 +0200 Subject: [PATCH] OP-975 Move board level calibration to a reusable class --- .../calibration/levelcalibrationmodel.cpp | 176 ++++++++++++++++++ .../calibration/levelcalibrationmodel.h | 75 ++++++++ .../src/plugins/config/config.pro | 6 +- .../src/plugins/config/configrevowidget.cpp | 149 ++------------- .../src/plugins/config/configrevowidget.h | 17 +- 5 files changed, 272 insertions(+), 151 deletions(-) create mode 100644 ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp create mode 100644 ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h diff --git a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp new file mode 100644 index 000000000..96972215d --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.cpp @@ -0,0 +1,176 @@ +/** + ****************************************************************************** + * + * @file levelcalibrationmodel.cpp + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @addtogroup board level calibration + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Telemetry configuration panel + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "levelcalibrationmodel.h" +#include +#include +#include "extensionsystem/pluginmanager.h" + +static const int LEVEL_SAMPLES = 100; +namespace OpenPilot { +LevelCalibrationModel::LevelCalibrationModel(QObject *parent) : + QObject(parent) +{ +} + + +/******* 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; + UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); + mdata.flightTelemetryUpdatePeriod = 100; + attitudeState->setMetadata(mdata); + + /* Show instructions and enable controls */ + displayInstructions("Place horizontally and click save position...", true); + displayVisualHelp("plane-horizontal"); + disableAllCalibrations(); + savePositionEnabledChanged(true); + position = 0; +} + +void LevelCalibrationModel::savePosition() +{ + QMutexLocker lock(&sensorsUpdateLock); + + Q_UNUSED(lock); + + savePositionEnabledChanged(false); + + rot_accum_pitch.clear(); + rot_accum_roll.clear(); + + collectingData = true; + + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + Q_ASSERT(attitudeState); + connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + + displayInstructions("Hold...",false); +} + +/** + Updates the accel bias raw values + */ +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); + break; + } + default: + Q_ASSERT(0); + } + + // Work out the progress based on whichever has less + double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES; + progressChanged(p1 * 100); + + if (rot_accum_roll.size() >= LEVEL_SAMPLES && + collectingData == true) { + collectingData = false; + + AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); + + disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *))); + + position++; + switch (position) { + case 1: + rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); + rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll); + + displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true); + displayVisualHelp("plane-horizontal-rotated"); + + disableAllCalibrations(); + + savePositionEnabledChanged(true); + break; + case 2: + rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); + rot_data_pitch /= 2; + rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll); + rot_data_roll /= 2; + attitudeState->setMetadata(initialAttitudeStateMdata); + compute(); + enableAllCalibrations(); + break; + } + } +} +void LevelCalibrationModel::compute() +{ + enableAllCalibrations(); + + AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); + Q_ASSERT(attitudeSettings); + AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); + + // Update the biases based on collected data + // "rotate" the board in the opposite direction as the calculated offset + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch; + attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll; + + attitudeSettings->setData(attitudeSettingsData); + attitudeSettings->updated(); +} +UAVObjectManager *LevelCalibrationModel::getObjectManager() +{ + ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); + UAVObjectManager *objMngr = pm->getObject(); + + Q_ASSERT(objMngr); + return objMngr; +} +} diff --git a/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h new file mode 100644 index 000000000..cc4c79428 --- /dev/null +++ b/ground/openpilotgcs/src/plugins/config/calibration/levelcalibrationmodel.h @@ -0,0 +1,75 @@ +/** + ****************************************************************************** + * + * @file levelcalibrationmodel.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @addtogroup board level calibration + * @{ + * @addtogroup ConfigPlugin Config Plugin + * @{ + * @brief Telemetry configuration panel + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef LEVELCALIBRATIONMODEL_H +#define LEVELCALIBRATIONMODEL_H + +#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); + +signals: + void displayVisualHelp(QString elementID); + void displayInstructions(QString instructions, bool replace); + void disableAllCalibrations(); + void enableAllCalibrations(); + void savePositionEnabledChanged(bool state); + void progressChanged(int value); +public slots: + // Slots for calibrating the mags + void start(); + void savePosition(); +private slots: + void getSample(UAVObject *obj); + void compute(); +private: + QMutex sensorsUpdateLock; + int position; + bool collectingData; + + QList rot_accum_roll; + QList rot_accum_pitch; + double rot_data_roll; + double rot_data_pitch; + UAVObject::Metadata initialAttitudeStateMdata; + UAVObjectManager *getObjectManager(); +}; +} +#endif // LEVELCALIBRATIONMODEL_H diff --git a/ground/openpilotgcs/src/plugins/config/config.pro b/ground/openpilotgcs/src/plugins/config/config.pro index 087af1088..3534ecfd2 100644 --- a/ground/openpilotgcs/src/plugins/config/config.pro +++ b/ground/openpilotgcs/src/plugins/config/config.pro @@ -52,7 +52,8 @@ HEADERS += configplugin.h \ calibration/thermal/dataacquisitiontransition.h \ calibration/thermal/settingshandlingtransitions.h \ calibration/thermal/compensationcalculationtransition.h \ - calibration/sixpointcalibrationmodel.h + calibration/sixpointcalibrationmodel.h \ + calibration/levelcalibrationmodel.h SOURCES += configplugin.cpp \ configgadgetwidget.cpp \ @@ -88,7 +89,8 @@ SOURCES += configplugin.cpp \ calibration/thermal/thermalcalibration.cpp \ calibration/thermal/thermalcalibrationhelper.cpp \ calibration/thermal/thermalcalibrationmodel.cpp \ - calibration/sixpointcalibrationmodel.cpp + calibration/sixpointcalibrationmodel.cpp \ + calibration/levelcalibrationmodel.cpp FORMS += airframe.ui \ airframe_ccpm.ui \ diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp index 9ffc9e3fc..ec08ee1ec 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.cpp @@ -56,8 +56,8 @@ #define sign(x) ((x < 0) ? -1 : 1) // Uncomment this to enable 6 point calibration on the accels -#define SAMPLE_SIZE 40 -const double ConfigRevoWidget::maxVarValue = 0.1; +#define NOISE_SAMPLES 50 + // ***************** @@ -75,7 +75,6 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : ConfigTaskWidget(parent), m_ui(new Ui_RevoSensorsWidget()), collectingData(false), - position(-1), isBoardRotationStored(false) { m_ui->setupUi(this); @@ -128,9 +127,17 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) : // gyro zero calibration connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart())); + m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel(); // level calibration - connect(m_ui->boardLevelStart, SIGNAL(clicked()), this, SLOT(levelCalibrationStart())); - connect(m_ui->boardLevelSavePos, SIGNAL(clicked()), this, SLOT(levelCalibrationSavePosition())); + 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(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_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation())); @@ -242,9 +249,9 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj) // Work out the progress based on whichever has less double p1 = (double)gyro_accum_x.size() / (double)NOISE_SAMPLES; double p2 = (double)gyro_accum_y.size() / (double)NOISE_SAMPLES; - m_ui->gyroBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 50); + m_ui->gyroBiasProgress->setValue(((p1 < p2) ? p1 : p2) * 100); - if (gyro_accum_y.size() >= 2 * NOISE_SAMPLES && + if (gyro_accum_y.size() >= NOISE_SAMPLES && collectingData == true) { collectingData = false; @@ -289,134 +296,6 @@ void ConfigRevoWidget::gyroBiasCalibrationGetSample(UAVObject *obj) } -/******* Level calibration *******/ -/** - * Starts an accelerometer bias calibration. - */ -void ConfigRevoWidget::levelCalibrationStart() -{ - // Store and reset board rotation before calibration starts - - disableAllCalibrations(); - m_ui->boardLevelProgress->setValue(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; - UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC); - mdata.flightTelemetryUpdatePeriod = 100; - attitudeState->setMetadata(mdata); - - /* Show instructions and enable controls */ - displayInstructions("Place horizontally and click save position...", true); - displayVisualHelp("plane-horizontal"); - disableAllCalibrations(); - m_ui->boardLevelSavePos->setEnabled(true); - position = 0; -} - -void ConfigRevoWidget::levelCalibrationSavePosition() -{ - QMutexLocker lock(&sensorsUpdateLock); - - Q_UNUSED(lock); - - m_ui->boardLevelSavePos->setEnabled(false); - - rot_accum_pitch.clear(); - rot_accum_roll.clear(); - - collectingData = true; - - AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); - Q_ASSERT(attitudeState); - connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); - - displayInstructions("Hold..."); -} - -/** - Updates the accel bias raw values - */ -void ConfigRevoWidget::levelCalibrationGetSample(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); - break; - } - default: - Q_ASSERT(0); - } - - // Work out the progress based on whichever has less - double p1 = (double)rot_accum_roll.size() / (double)NOISE_SAMPLES; - m_ui->boardLevelProgress->setValue(p1 * 100); - - if (rot_accum_roll.size() >= NOISE_SAMPLES && - collectingData == true) { - collectingData = false; - - AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager()); - - disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(UAVObject *))); - - position++; - switch (position) { - case 1: - rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); - rot_data_roll = OpenPilot::CalibrationUtils::listMean(rot_accum_roll); - - displayInstructions("Leave horizontally, rotate 180° along yaw axis and click save position...", true); - displayVisualHelp("plane-horizontal-rotated"); - - disableAllCalibrations(); - - m_ui->boardLevelSavePos->setEnabled(true); - break; - case 2: - rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch); - rot_data_pitch /= 2; - rot_data_roll += OpenPilot::CalibrationUtils::listMean(rot_accum_roll); - rot_data_roll /= 2; - attitudeState->setMetadata(initialAttitudeStateMdata); - levelCalibrationCompute(); - enableAllCalibrations(); - break; - } - } -} -void ConfigRevoWidget::levelCalibrationCompute() -{ - enableAllCalibrations(); - - AttitudeSettings *attitudeSettings = AttitudeSettings::GetInstance(getObjectManager()); - Q_ASSERT(attitudeSettings); - AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData(); - - // Update the biases based on collected data - // "rotate" the board in the opposite direction as the calculated offset - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH] -= rot_data_pitch; - attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL] -= rot_data_roll; - - attitudeSettings->setData(attitudeSettingsData); - attitudeSettings->updated(); -} - void ConfigRevoWidget::storeAndClearBoardRotation() { if (!isBoardRotationStored) { diff --git a/ground/openpilotgcs/src/plugins/config/configrevowidget.h b/ground/openpilotgcs/src/plugins/config/configrevowidget.h index 8e9fcd08b..f256076c0 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevowidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevowidget.h @@ -41,7 +41,7 @@ #include #include "calibration/thermal/thermalcalibrationmodel.h" #include "calibration/sixpointcalibrationmodel.h" - +#include "calibration/levelcalibrationmodel.h" class Ui_Widget; @@ -55,11 +55,12 @@ public: private: OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel; OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel; + OpenPilot::LevelCalibrationModel *m_levelCalibrationModel; + Ui_RevoSensorsWidget *m_ui; QMutex sensorsUpdateLock; int phaseCounter; - const static double maxVarValue; const static int calibrationDelay = 10; bool collectingData; @@ -68,20 +69,8 @@ private: QList gyro_accum_y; QList gyro_accum_z; - QList rot_accum_roll; - QList rot_accum_pitch; - - - double rot_data_roll; - double rot_data_pitch; - - - UAVObject::Metadata initialAttitudeStateMdata; UAVObject::Metadata initialGyroStateMdata; - int position; - - static const int NOISE_SAMPLES = 50; // Board rotation store/recall qint16 storedBoardRotation[3];