mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-09 20:46:07 +01:00
235 lines
9.6 KiB
C++
235 lines
9.6 KiB
C++
/**
|
|
******************************************************************************
|
|
*
|
|
* @file biascalibrationutil.cpp
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
|
* @addtogroup
|
|
* @{
|
|
* @addtogroup BiasCalibrationUtil
|
|
* @{
|
|
* @brief
|
|
*****************************************************************************/
|
|
/*
|
|
* 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 "biascalibrationutil.h"
|
|
#include "extensionsystem/pluginmanager.h"
|
|
#include "uavobjectmanager.h"
|
|
#include "attitudesettings.h"
|
|
#include "accelstate.h"
|
|
#include "gyrostate.h"
|
|
#include "qdebug.h"
|
|
#include "revocalibration.h"
|
|
#include "accelgyrosettings.h"
|
|
|
|
|
|
BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
|
|
m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount),
|
|
m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate)
|
|
{}
|
|
|
|
BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate,
|
|
long gyroMeasurementCount, long gyroMeasurementRate) : QObject(),
|
|
m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount),
|
|
m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate)
|
|
{}
|
|
|
|
void BiasCalibrationUtil::start()
|
|
{
|
|
if (!m_isMeasuring) {
|
|
startMeasurement();
|
|
|
|
// Set up timeout timer
|
|
connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
|
|
// Set timeout to max time of measurement + 10 secs
|
|
m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000);
|
|
}
|
|
}
|
|
|
|
void BiasCalibrationUtil::abort()
|
|
{
|
|
if (m_isMeasuring) {
|
|
stopMeasurement();
|
|
}
|
|
}
|
|
|
|
void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj)
|
|
{
|
|
Q_UNUSED(obj);
|
|
|
|
if (m_receivedGyroUpdates < m_gyroMeasurementCount) {
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
|
|
Q_ASSERT(uavObjectManager);
|
|
|
|
GyroState *gyroState = GyroState::GetInstance(uavObjectManager);
|
|
GyroState::DataFields gyroStateData = gyroState->getData();
|
|
|
|
m_gyroX += gyroStateData.x;
|
|
m_gyroY += gyroStateData.y;
|
|
m_gyroZ += gyroStateData.z;
|
|
|
|
m_receivedGyroUpdates++;
|
|
emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
|
|
} else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
|
|
m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
|
|
stopMeasurement();
|
|
}
|
|
}
|
|
|
|
void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj)
|
|
{
|
|
Q_UNUSED(obj);
|
|
|
|
if (m_receivedAccelUpdates < m_accelMeasurementCount) {
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
|
|
Q_ASSERT(uavObjectManager);
|
|
|
|
AccelState *accelState = AccelState::GetInstance(uavObjectManager);
|
|
AccelState::DataFields AccelStateData = accelState->getData();
|
|
|
|
m_accelerometerX += AccelStateData.x;
|
|
m_accelerometerY += AccelStateData.y;
|
|
m_accelerometerZ += AccelStateData.z;
|
|
|
|
m_receivedAccelUpdates++;
|
|
emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
|
|
} else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
|
|
m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
|
|
stopMeasurement();
|
|
}
|
|
}
|
|
|
|
void BiasCalibrationUtil::timeout()
|
|
{
|
|
stopMeasurement();
|
|
emit timeout(tr("Calibration timed out before receiving required updates."));
|
|
}
|
|
|
|
void BiasCalibrationUtil::startMeasurement()
|
|
{
|
|
m_isMeasuring = true;
|
|
|
|
// Reset variables
|
|
m_receivedAccelUpdates = 0;
|
|
m_accelerometerX = 0;
|
|
m_accelerometerY = 0;
|
|
m_accelerometerZ = 0;
|
|
|
|
m_receivedGyroUpdates = 0;
|
|
m_gyroX = 0;
|
|
m_gyroY = 0;
|
|
m_gyroZ = 0;
|
|
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
|
|
Q_ASSERT(uavObjectManager);
|
|
|
|
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
|
|
Q_ASSERT(revolutionCalibration);
|
|
AccelGyroSettings *accelGyroSettings = AccelGyroSettings::GetInstance(uavObjectManager);
|
|
Q_ASSERT(accelGyroSettings);
|
|
|
|
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
|
|
AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
|
|
// Disable gyro bias correction to see raw data
|
|
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
|
|
|
|
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
|
|
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
|
|
|
|
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
|
|
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
|
|
accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
|
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] = 0;
|
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] = 0;
|
|
accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] = 0;
|
|
int i;
|
|
for (i = 0; i < 5; i++) {
|
|
RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData);
|
|
AccelGyroSettings::GetInstance(uavObjectManager)->setData(accelGyroSettingsData);
|
|
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
|
|
}
|
|
// Set up to receive updates for accels
|
|
UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
|
|
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
|
|
|
|
// Set update period for accels
|
|
m_previousAccelMetaData = uavObject->getMetadata();
|
|
UAVObject::Metadata newMetaData = m_previousAccelMetaData;
|
|
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
|
|
newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
|
|
|
|
for (i = 0; i < 5; i++) {
|
|
uavObject->setMetadata(newMetaData);
|
|
}
|
|
// Set up to receive updates from gyros
|
|
uavObject = GyroState::GetInstance(uavObjectManager);
|
|
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
|
|
|
|
// Set update period for gyros
|
|
m_previousGyroMetaData = uavObject->getMetadata();
|
|
newMetaData = m_previousGyroMetaData;
|
|
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
|
|
newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
|
|
for (i = 0; i < 5; i++) {
|
|
uavObject->setMetadata(newMetaData);
|
|
}
|
|
}
|
|
|
|
void BiasCalibrationUtil::stopMeasurement()
|
|
{
|
|
qDebug() << "Sampling done, G =" << m_receivedGyroUpdates << "A =" << m_receivedAccelUpdates;
|
|
|
|
m_isMeasuring = false;
|
|
|
|
// Stop timeout timer
|
|
m_timeoutTimer.stop();
|
|
disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
|
|
|
|
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
|
|
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
|
|
Q_ASSERT(uavObjectManager);
|
|
|
|
// Stop listening for updates from accels
|
|
UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
|
|
disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
|
|
uavObject->setMetadata(m_previousAccelMetaData);
|
|
|
|
// Stop listening for updates from gyros
|
|
uavObject = GyroState::GetInstance(uavObjectManager);
|
|
disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
|
|
uavObject->setMetadata(m_previousGyroMetaData);
|
|
|
|
// Enable gyro bias correction again
|
|
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
|
|
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
|
|
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
|
|
|
|
accelGyroBias bias;
|
|
bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates;
|
|
bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates;
|
|
bias.m_accelerometerZBias = m_accelerometerZ / (double)m_receivedAccelUpdates;
|
|
|
|
bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates;
|
|
bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates;
|
|
bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates;
|
|
|
|
qDebug() << "Bias calculations finished";
|
|
emit done(bias);
|
|
}
|