1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-15 07:29:15 +01:00

OP-975 Move gyro zero bias calibration to a reusable class

This commit is contained in:
Alessio Morale 2014-04-11 05:41:39 +02:00
parent 2249510c52
commit 9aef8b0edf
5 changed files with 263 additions and 150 deletions

View File

@ -0,0 +1,172 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.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 <attitudestate.h>
#include <attitudesettings.h>
#include "extensionsystem/pluginmanager.h"
#include <gyrostate.h>
#include <revocalibration.h>
#include <accelgyrosettings.h>
#include "calibration/gyrobiascalibrationmodel.h"
#include "calibration/calibrationutils.h"
static const int LEVEL_SAMPLES = 100;
#include "gyrobiascalibrationmodel.h"
namespace OpenPilot {
GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) :
QObject(parent),
collectingData(false)
{
}
/******* 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();
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();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
UAVObject::Metadata mdata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
}
/**
Updates the gyro bias raw values
*/
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_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
break;
}
default:
Q_ASSERT(0);
}
// Work out the progress based on whichever has less
double p1 = (double)gyro_accum_x.size() / (double)LEVEL_SAMPLES;
double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES;
progressChanged(((p1 < p2) ? p1 : p2) * 100);
if (gyro_accum_y.size() >= LEVEL_SAMPLES &&
collectingData == true) {
collectingData = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
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
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);
// Recall saved board rotation
recallBoardRotation();
}
}
UAVObjectManager *GyroBiasCalibrationModel::getObjectManager()
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
Q_ASSERT(objMngr);
return objMngr;
}
}

View File

@ -0,0 +1,69 @@
/**
******************************************************************************
*
* @file gyrobiascalibrationmodel.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
*/
#ifndef GYROBIASCALIBRATIONMODEL_H
#define GYROBIASCALIBRATIONMODEL_H
#include <QObject>
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
namespace OpenPilot {
class GyroBiasCalibrationModel : public QObject
{
Q_OBJECT
public:
explicit GyroBiasCalibrationModel(QObject *parent = 0);
signals:
void displayVisualHelp(QString elementID);
void displayInstructions(QString instructions, bool replace);
void disableAllCalibrations();
void enableAllCalibrations();
void storeAndClearBoardRotation();
void recallBoardRotation();
void progressChanged(int value);
public slots:
// Slots for gyro bias zero
void start();
private slots:
void getSample(UAVObject *obj);
private:
QMutex sensorsUpdateLock;
bool collectingData;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
UAVObject::Metadata initialGyroStateMdata;
UAVObjectManager *getObjectManager();
};
}
#endif // GYROBIASCALIBRATIONMODEL_H

View File

@ -53,7 +53,8 @@ HEADERS += configplugin.h \
calibration/thermal/settingshandlingtransitions.h \
calibration/thermal/compensationcalculationtransition.h \
calibration/sixpointcalibrationmodel.h \
calibration/levelcalibrationmodel.h
calibration/levelcalibrationmodel.h \
calibration/gyrobiascalibrationmodel.h
SOURCES += configplugin.cpp \
configgadgetwidget.cpp \
@ -90,7 +91,8 @@ SOURCES += configplugin.cpp \
calibration/thermal/thermalcalibrationhelper.cpp \
calibration/thermal/thermalcalibrationmodel.cpp \
calibration/sixpointcalibrationmodel.cpp \
calibration/levelcalibrationmodel.cpp
calibration/levelcalibrationmodel.cpp \
calibration/gyrobiascalibrationmodel.cpp
FORMS += airframe.ui \
airframe_ccpm.ui \

View File

@ -47,7 +47,7 @@
#include <accelgyrosettings.h>
#include <homelocation.h>
#include <accelstate.h>
#include <gyrostate.h>
#include <magstate.h>
#include "assertions.h"
@ -74,7 +74,6 @@ public:
ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
ConfigTaskWidget(parent),
m_ui(new Ui_RevoSensorsWidget()),
collectingData(false),
isBoardRotationStored(false)
{
m_ui->setupUi(this);
@ -123,11 +122,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
connect(m_sixPointCalibrationModel, SIGNAL(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_sixPointCalibrationModel, SIGNAL(savePositionEnabledChanged(bool)), this->m_ui->sixPointsSave, SLOT(setEnabled(bool)));
// Connect the signals
// gyro zero calibration
connect(m_ui->gyroBiasStart, SIGNAL(clicked()), this, SLOT(gyroCalibrationStart()));
m_levelCalibrationModel = new OpenPilot::LevelCalibrationModel();
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()));
@ -139,6 +134,20 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
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 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(disableAllCalibrations()), this, SLOT(disableAllCalibrations()));
connect(m_gyroBiasCalibrationModel, SIGNAL(enableAllCalibrations()), 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(displayVisualHelp(QString)), this, SLOT(displayVisualHelp(QString)));
connect(m_ui->hlClearButton, SIGNAL(clicked()), this, SLOT(clearHomeLocation()));
@ -177,124 +186,6 @@ void ConfigRevoWidget::resizeEvent(QResizeEvent *event)
m_ui->calibrationVisualHelp->fitInView(m_ui->calibrationVisualHelp->scene()->sceneRect(), Qt::KeepAspectRatioByExpanding);
}
/******* gyro bias zero ******/
void ConfigRevoWidget::gyroCalibrationStart()
{
// Store and reset board rotation before calibration starts
isBoardRotationStored = false;
storeAndClearBoardRotation();
disableAllCalibrations();
m_ui->gyroBiasProgress->setValue(0);
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
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();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
attitudeSettings->setData(attitudeSettingsData);
attitudeSettings->updated();
gyro_accum_x.clear();
gyro_accum_y.clear();
gyro_accum_z.clear();
UAVObject::Metadata mdata;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
Q_ASSERT(gyroState);
initialGyroStateMdata = gyroState->getMetadata();
mdata = initialGyroStateMdata;
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
mdata.flightTelemetryUpdatePeriod = 100;
gyroState->setMetadata(mdata);
// Now connect to the accels and mag updates, gather for 100 samples
collectingData = true;
connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroBiasCalibrationGetSample(UAVObject *)));
}
/**
Updates the gyro bias raw values
*/
void ConfigRevoWidget::gyroBiasCalibrationGetSample(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_accum_x.append(gyroStateData.x);
gyro_accum_y.append(gyroStateData.y);
gyro_accum_z.append(gyroStateData.z);
break;
}
default:
Q_ASSERT(0);
}
// 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) * 100);
if (gyro_accum_y.size() >= NOISE_SAMPLES &&
collectingData == true) {
collectingData = false;
GyroState *gyroState = GyroState::GetInstance(getObjectManager());
disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(levelCalibrationGetSample(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
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);
// Recall saved board rotation
recallBoardRotation();
}
}
void ConfigRevoWidget::storeAndClearBoardRotation()
{

View File

@ -42,6 +42,7 @@
#include "calibration/thermal/thermalcalibrationmodel.h"
#include "calibration/sixpointcalibrationmodel.h"
#include "calibration/levelcalibrationmodel.h"
#include "calibration/gyrobiascalibrationmodel.h"
class Ui_Widget;
@ -56,21 +57,9 @@ private:
OpenPilot::SixPointCalibrationModel *m_sixPointCalibrationModel;
OpenPilot::ThermalCalibrationModel *m_thermalCalibrationModel;
OpenPilot::LevelCalibrationModel *m_levelCalibrationModel;
OpenPilot::GyroBiasCalibrationModel *m_gyroBiasCalibrationModel;
Ui_RevoSensorsWidget *m_ui;
QMutex sensorsUpdateLock;
int phaseCounter;
const static int calibrationDelay = 10;
bool collectingData;
QList<double> gyro_accum_x;
QList<double> gyro_accum_y;
QList<double> gyro_accum_z;
UAVObject::Metadata initialGyroStateMdata;
// Board rotation store/recall
qint16 storedBoardRotation[3];
@ -85,16 +74,6 @@ private slots:
// ! Overriden method from the configTaskWidget to update UI
virtual void refreshWidgetsValues(UAVObject *object = NULL);
// Slots for calibrating the accel and gyro
void levelCalibrationStart();
void levelCalibrationSavePosition();
void levelCalibrationCompute();
void levelCalibrationGetSample(UAVObject *);
// Slots for gyro bias zero
void gyroCalibrationStart();
void gyroBiasCalibrationGetSample(UAVObject *obj);
// Slot for clearing home location
void clearHomeLocation();