ground/gcs/src/plugins/config/calibration/gyrobiascalibrationmodel.cpp

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       gyrobiascalibrationmodel.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @addtogroup board level calibration
   7  * @{
   8  * @addtogroup ConfigPlugin Config Plugin
   9  * @{
  10  * @brief Telemetry configuration panel
  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 "calibration/gyrobiascalibrationmodel.h"
  29 #include "calibration/calibrationutils.h"
  30 #include "calibration/calibrationuiutils.h"
  31 #include "extensionsystem/pluginmanager.h"
  32
  33 static const int LEVEL_SAMPLES = 100;
  34
  35 namespace OpenPilot {
  36 GyroBiasCalibrationModel::GyroBiasCalibrationModel(QObject *parent) :
  37     QObject(parent), collectingData(false), m_dirty(false)
  38 {
  39     gyroState  = GyroState::GetInstance(getObjectManager());
  40     Q_ASSERT(gyroState);
  41
  42     gyroSensor = GyroSensor::GetInstance(getObjectManager());
  43     Q_ASSERT(gyroSensor);
  44
  45     revoCalibration   = RevoCalibration::GetInstance(getObjectManager());
  46     Q_ASSERT(revoCalibration);
  47
  48     attitudeSettings  = AttitudeSettings::GetInstance(getObjectManager());
  49     Q_ASSERT(attitudeSettings);
  50
  51     accelGyroSettings = AccelGyroSettings::GetInstance(getObjectManager());
  52     Q_ASSERT(accelGyroSettings);
  53 }
  54
  55
  56 void GyroBiasCalibrationModel::start()
  57 {
  58     // reset dirty state to forget previous unsaved runs
  59     m_dirty = false;
  60
  61     // configure board for calibration
  62     RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
  63     memento.revoCalibrationData = revoCalibrationData;
  64     revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
  65     revoCalibration->setData(revoCalibrationData);
  66
  67     AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
  68     memento.attitudeSettingsData = attitudeSettingsData;
  69     // Disable gyro bias correction while calibrating
  70     attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
  71     // Zero board rotation
  72     attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_YAW]     = 0;
  73     attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_ROLL]    = 0;
  74     attitudeSettingsData.BoardRotation[AttitudeSettings::BOARDROTATION_PITCH]   = 0;
  75     // Zero board level trim
  76     attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  = 0;
  77     attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] = 0;
  78
  79     attitudeSettings->setData(attitudeSettingsData);
  80
  81     UAVObject::Metadata gyroStateMetadata = gyroState->getMetadata();
  82     memento.gyroStateMetadata = gyroStateMetadata;
  83     UAVObject::SetFlightTelemetryUpdateMode(gyroStateMetadata, UAVObject::UPDATEMODE_PERIODIC);
  84     gyroStateMetadata.flightTelemetryUpdatePeriod = 100;
  85     gyroState->setMetadata(gyroStateMetadata);
  86
  87     UAVObject::Metadata gyroSensorMetadata = gyroSensor->getMetadata();
  88     memento.gyroSensorMetadata = gyroSensorMetadata;
  89     UAVObject::SetFlightTelemetryUpdateMode(gyroSensorMetadata, UAVObject::UPDATEMODE_PERIODIC);
  90     gyroSensorMetadata.flightTelemetryUpdatePeriod = 100;
  91     gyroSensor->setMetadata(gyroSensorMetadata);
  92
  93     gyro_accum_x.clear();
  94     gyro_accum_y.clear();
  95     gyro_accum_z.clear();
  96
  97     gyro_state_accum_x.clear();
  98     gyro_state_accum_y.clear();
  99     gyro_state_accum_z.clear();
 100
 101     started();
 102     progressChanged(0);
 103     displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
 104     displayInstructions(tr("Calibrating the gyroscopes. Keep the vehicle steady..."));
 105
 106     // Now connect to the accels and mag updates, gather for 100 samples
 107     collectingData = true;
 108     connect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
 109     connect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
 110 }
 111
 112 /**
 113    Updates the gyro bias raw values
 114  */
 115 void GyroBiasCalibrationModel::getSample(UAVObject *obj)
 116 {
 117     QMutexLocker lock(&sensorsUpdateLock);
 118
 119     switch (obj->getObjID()) {
 120     case GyroState::OBJID:
 121     {
 122         GyroState::DataFields gyroStateData = gyroState->getData();
 123         gyro_state_accum_x.append(gyroStateData.x);
 124         gyro_state_accum_y.append(gyroStateData.y);
 125         gyro_state_accum_z.append(gyroStateData.z);
 126         break;
 127     }
 128     case GyroSensor::OBJID:
 129     {
 130         GyroSensor::DataFields gyroSensorData = gyroSensor->getData();
 131         gyro_accum_x.append(gyroSensorData.x);
 132         gyro_accum_y.append(gyroSensorData.y);
 133         gyro_accum_z.append(gyroSensorData.z);
 134         break;
 135     }
 136     default:
 137         Q_ASSERT(0);
 138     }
 139
 140     // Work out the progress based on whichever has less
 141     double p1 = (double)gyro_state_accum_x.size() / (double)LEVEL_SAMPLES;
 142     double p2 = (double)gyro_accum_y.size() / (double)LEVEL_SAMPLES;
 143     progressChanged(((p1 > p2) ? p1 : p2) * 100);
 144
 145     if ((gyro_accum_y.size() >= LEVEL_SAMPLES || (gyro_accum_y.size() == 0 && gyro_state_accum_y.size() >= LEVEL_SAMPLES)) &&
 146         collectingData == true) {
 147         collectingData = false;
 148         m_dirty = true;
 149
 150         disconnect(gyroSensor, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
 151         disconnect(gyroState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
 152
 153         gyroState->setMetadata(memento.gyroStateMetadata);
 154         gyroSensor->setMetadata(memento.gyroSensorMetadata);
 155         revoCalibration->setData(memento.revoCalibrationData);
 156         attitudeSettings->setData(memento.attitudeSettingsData);
 157
 158         stopped();
 159         displayInstructions(tr("Gyroscope calibration completed successfully."), WizardModel::Success);
 160         displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
 161     }
 162 }
 163
 164 void GyroBiasCalibrationModel::save()
 165 {
 166     if (!m_dirty) {
 167         return;
 168     }
 169     // Update biases based on collected data
 170     AccelGyroSettings::DataFields accelGyroSettingsData = accelGyroSettings->getData();
 171
 172     // check whether the board does supports gyroSensor (updates were received)
 173     if (gyro_accum_x.count() < LEVEL_SAMPLES / 10) {
 174         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_x);
 175         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_y);
 176         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_state_accum_z);
 177     } else {
 178         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_X] += OpenPilot::CalibrationUtils::listMean(gyro_accum_x);
 179         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Y] += OpenPilot::CalibrationUtils::listMean(gyro_accum_y);
 180         accelGyroSettingsData.gyro_bias[AccelGyroSettings::GYRO_BIAS_Z] += OpenPilot::CalibrationUtils::listMean(gyro_accum_z);
 181     }
 182
 183     accelGyroSettings->setData(accelGyroSettingsData);
 184
 185     m_dirty = false;
 186 }
 187
 188 UAVObjectManager *GyroBiasCalibrationModel::getObjectManager()
 189 {
 190     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 191     UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
 192
 193     Q_ASSERT(objMngr);
 194     return objMngr;
 195 }
 196 }