ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       biascalibrationutil.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   6  * @addtogroup
   7  * @{
   8  * @addtogroup BiasCalibrationUtil
   9  * @{
  10  * @brief
  11  *****************************************************************************/
  12 /*
  13  * This program is free software; you can redistribute it and/or modify
  14  * it under the terms of the GNU General Public License as published by
  15  * the Free Software Foundation; either version 3 of the License, or
  16  * (at your option) any later version.
  17  *
  18  * This program is distributed in the hope that it will be useful, but
  19  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  20  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  21  * for more details.
  22  *
  23  * You should have received a copy of the GNU General Public License along
  24  * with this program; if not, write to the Free Software Foundation, Inc.,
  25  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  26  */
  27
  28 #include "biascalibrationutil.h"
  29 #include "extensionsystem/pluginmanager.h"
  30 #include "uavobjectmanager.h"
  31 #include "attitudesettings.h"
  32 #include "accelstate.h"
  33 #include "gyrostate.h"
  34 #include "qdebug.h"
  35 #include "revocalibration.h"
  36 #include "accelgyrosettings.h"
  37
  38
  39 BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
  40     m_isMeasuring(false), m_accelMeasurementCount(measurementCount), m_gyroMeasurementCount(measurementCount),
  41     m_accelMeasurementRate(measurementRate), m_gyroMeasurementRate(measurementRate)
  42 {}
  43
  44 BiasCalibrationUtil::BiasCalibrationUtil(long accelMeasurementCount, long accelMeasurementRate,
  45                                          long gyroMeasurementCount, long gyroMeasurementRate) : QObject(),
  46     m_isMeasuring(false), m_accelMeasurementCount(accelMeasurementCount), m_gyroMeasurementCount(gyroMeasurementCount),
  47     m_accelMeasurementRate(accelMeasurementRate), m_gyroMeasurementRate(gyroMeasurementRate)
  48 {}
  49
  50 void BiasCalibrationUtil::start()
  51 {
  52     if (!m_isMeasuring) {
  53         startMeasurement();
  54
  55         // Set up timeout timer
  56         connect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
  57         // Set timeout to max time of measurement + 10 secs
  58         m_timeoutTimer.start(std::max(m_accelMeasurementCount * m_accelMeasurementRate, m_gyroMeasurementCount * m_gyroMeasurementRate) + 10000);
  59     }
  60 }
  61
  62 void BiasCalibrationUtil::abort()
  63 {
  64     if (m_isMeasuring) {
  65         stopMeasurement();
  66     }
  67 }
  68
  69 void BiasCalibrationUtil::gyroMeasurementsUpdated(UAVObject *obj)
  70 {
  71     Q_UNUSED(obj);
  72
  73     if (m_receivedGyroUpdates < m_gyroMeasurementCount) {
  74         ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  75         UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
  76         Q_ASSERT(uavObjectManager);
  77
  78         GyroState *gyroState = GyroState::GetInstance(uavObjectManager);
  79         GyroState::DataFields gyroStateData = gyroState->getData();
  80
  81         m_gyroX += gyroStateData.x;
  82         m_gyroY += gyroStateData.y;
  83         m_gyroZ += gyroStateData.z;
  84
  85         m_receivedGyroUpdates++;
  86         emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
  87     } else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
  88                m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
  89         stopMeasurement();
  90     }
  91 }
  92
  93 void BiasCalibrationUtil::accelMeasurementsUpdated(UAVObject *obj)
  94 {
  95     Q_UNUSED(obj);
  96
  97     if (m_receivedAccelUpdates < m_accelMeasurementCount) {
  98         ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
  99         UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
 100         Q_ASSERT(uavObjectManager);
 101
 102         AccelState *accelState = AccelState::GetInstance(uavObjectManager);
 103         AccelState::DataFields AccelStateData = accelState->getData();
 104
 105         m_accelerometerX += AccelStateData.x;
 106         m_accelerometerY += AccelStateData.y;
 107         m_accelerometerZ += AccelStateData.z;
 108
 109         m_receivedAccelUpdates++;
 110         emit progress(m_receivedGyroUpdates + m_receivedAccelUpdates, m_gyroMeasurementCount + m_accelMeasurementCount);
 111     } else if (m_receivedAccelUpdates >= m_accelMeasurementCount &&
 112                m_receivedGyroUpdates >= m_gyroMeasurementCount && m_isMeasuring) {
 113         stopMeasurement();
 114     }
 115 }
 116
 117 void BiasCalibrationUtil::timeout()
 118 {
 119     stopMeasurement();
 120     emit timeout(tr("Calibration timed out before receiving required updates."));
 121 }
 122
 123 void BiasCalibrationUtil::startMeasurement()
 124 {
 125     m_isMeasuring = true;
 126
 127     // Reset variables
 128     m_receivedAccelUpdates = 0;
 129     m_accelerometerX = 0;
 130     m_accelerometerY = 0;
 131     m_accelerometerZ = 0;
 132
 133     m_receivedGyroUpdates = 0;
 134     m_gyroX = 0;
 135     m_gyroY = 0;
 136     m_gyroZ = 0;
 137
 138     ExtensionSystem::PluginManager *pm     = ExtensionSystem::PluginManager::instance();
 139     UAVObjectManager *uavObjectManager     = pm->getObject<UAVObjectManager>();
 140     Q_ASSERT(uavObjectManager);
 141
 142     RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
 143     Q_ASSERT(revolutionCalibration);
 144     AccelGyroSettings *accelGyroSettings   = AccelGyroSettings::GetInstance(uavObjectManager);
 145     Q_ASSERT(accelGyroSettings);
 146
 147     RevoCalibration::DataFields revoCalibrationData     = revolutionCalibration->getData();
 148     AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
 149     // Disable gyro bias correction to see raw data
 150     AttitudeSettings::DataFields attitudeSettingsData   = AttitudeSettings::GetInstance(uavObjectManager)->getData();
 151
 152     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
 153     revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
 154
 155     accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_X] = 0;
 156     accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Y] = 0;
 157     accelGyroSettingsData.accel_bias[AccelGyroSettings::ACCEL_BIAS_Z] = 0;
 158     accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X]   = 0;
 159     accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y]   = 0;
 160     accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z]   = 0;
 161     int i;
 162     for (i = 0; i < 5; i++) {
 163         RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData);
 164         AccelGyroSettings::GetInstance(uavObjectManager)->setData(accelGyroSettingsData);
 165         AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
 166     }
 167     // Set up to receive updates for accels
 168     UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
 169     connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
 170
 171     // Set update period for accels
 172     m_previousAccelMetaData = uavObject->getMetadata();
 173     UAVObject::Metadata newMetaData = m_previousAccelMetaData;
 174     UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
 175     newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
 176
 177     for (i = 0; i < 5; i++) {
 178         uavObject->setMetadata(newMetaData);
 179     }
 180     // Set up to receive updates from gyros
 181     uavObject = GyroState::GetInstance(uavObjectManager);
 182     connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
 183
 184     // Set update period for gyros
 185     m_previousGyroMetaData = uavObject->getMetadata();
 186     newMetaData = m_previousGyroMetaData;
 187     UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
 188     newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
 189     for (i = 0; i < 5; i++) {
 190         uavObject->setMetadata(newMetaData);
 191     }
 192 }
 193
 194 void BiasCalibrationUtil::stopMeasurement()
 195 {
 196     qDebug() << "Sampling done, G =" << m_receivedGyroUpdates << "A =" << m_receivedAccelUpdates;
 197
 198     m_isMeasuring = false;
 199
 200     // Stop timeout timer
 201     m_timeoutTimer.stop();
 202     disconnect(&m_timeoutTimer, SIGNAL(timeout()), this, SLOT(timeout()));
 203
 204     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 205     UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
 206     Q_ASSERT(uavObjectManager);
 207
 208     // Stop listening for updates from accels
 209     UAVDataObject *uavObject = AccelState::GetInstance(uavObjectManager);
 210     disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
 211     uavObject->setMetadata(m_previousAccelMetaData);
 212
 213     // Stop listening for updates from gyros
 214     uavObject = GyroState::GetInstance(uavObjectManager);
 215     disconnect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
 216     uavObject->setMetadata(m_previousGyroMetaData);
 217
 218     // Enable gyro bias correction again
 219     AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
 220     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
 221     AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
 222
 223     accelGyroBias bias;
 224     bias.m_accelerometerXBias = m_accelerometerX / (double)m_receivedAccelUpdates;
 225     bias.m_accelerometerYBias = m_accelerometerY / (double)m_receivedAccelUpdates;
 226     bias.m_accelerometerZBias = m_accelerometerZ / (double)m_receivedAccelUpdates;
 227
 228     bias.m_gyroXBias = m_gyroX / (double)m_receivedGyroUpdates;
 229     bias.m_gyroYBias = m_gyroY / (double)m_receivedGyroUpdates;
 230     bias.m_gyroZBias = m_gyroZ / (double)m_receivedGyroUpdates;
 231
 232     qDebug() << "Bias calculations finished";
 233     emit done(bias);
 234 }