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

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       levelcalibrationmodel.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 "levelcalibrationmodel.h"
  29 #include "extensionsystem/pluginmanager.h"
  30 #include "calibration/calibrationuiutils.h"
  31 #include "uavobjectmanager.h"
  32
  33 #include <attitudestate.h>
  34 #include <attitudesettings.h>
  35
  36 static const int LEVEL_SAMPLES = 100;
  37
  38 namespace OpenPilot {
  39 LevelCalibrationModel::LevelCalibrationModel(QObject *parent) :
  40     QObject(parent), m_dirty(false)
  41 {
  42     attitudeState    = AttitudeState::GetInstance(getObjectManager());
  43     Q_ASSERT(attitudeState);
  44
  45     attitudeSettings = AttitudeSettings::GetInstance(getObjectManager());
  46     Q_ASSERT(attitudeSettings);
  47 }
  48
  49
  50 /**
  51  * Starts an accelerometer bias calibration.
  52  */
  53 void LevelCalibrationModel::start()
  54 {
  55     memento.attitudeStateMdata = attitudeState->getMetadata();
  56     UAVObject::Metadata mdata = attitudeState->getMetadata();
  57     UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_PERIODIC);
  58     mdata.flightTelemetryUpdatePeriod = 100;
  59     attitudeState->setMetadata(mdata);
  60
  61     rot_data_pitch = 0;
  62     rot_data_roll  = 0;
  63
  64     // reset dirty state to forget previous unsaved runs
  65     m_dirty  = false;
  66
  67     position = 0;
  68
  69     started();
  70
  71     // Show instructions and enable controls
  72     progressChanged(0);
  73     displayInstructions(tr("Place horizontally and press Save Position..."), WizardModel::Prompt);
  74     displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_NED);
  75     savePositionEnabledChanged(true);
  76 }
  77
  78 void LevelCalibrationModel::savePosition()
  79 {
  80     QMutexLocker lock(&sensorsUpdateLock);
  81
  82     savePositionEnabledChanged(false);
  83
  84     rot_accum_pitch.clear();
  85     rot_accum_roll.clear();
  86
  87     collectingData = true;
  88
  89     connect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
  90
  91     displayInstructions(tr("Hold..."));
  92 }
  93
  94 /**
  95    Updates the accel bias raw values
  96  */
  97 void LevelCalibrationModel::getSample(UAVObject *obj)
  98 {
  99     QMutexLocker lock(&sensorsUpdateLock);
 100
 101     switch (obj->getObjID()) {
 102     case AttitudeState::OBJID:
 103     {
 104         AttitudeState::DataFields attitudeStateData = attitudeState->getData();
 105         rot_accum_roll.append(attitudeStateData.Roll);
 106         rot_accum_pitch.append(attitudeStateData.Pitch);
 107         break;
 108     }
 109     default:
 110         Q_ASSERT(0);
 111     }
 112
 113     // Work out the progress based on whichever has less
 114     double p1 = (double)rot_accum_roll.size() / (double)LEVEL_SAMPLES;
 115     progressChanged(p1 * 100);
 116
 117     if (rot_accum_roll.size() >= LEVEL_SAMPLES &&
 118         collectingData == true) {
 119         collectingData = false;
 120
 121         AttitudeState *attitudeState = AttitudeState::GetInstance(getObjectManager());
 122
 123         disconnect(attitudeState, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(getSample(UAVObject *)));
 124
 125         position++;
 126         switch (position) {
 127         case 1:
 128             rot_data_pitch = OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
 129             rot_data_roll  = OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
 130
 131             displayInstructions(tr("Leave horizontally, rotate 180° along yaw axis and press Save Position..."), WizardModel::Prompt);
 132             displayVisualHelp(CALIBRATION_HELPER_PLANE_PREFIX + CALIBRATION_HELPER_IMAGE_SWD);
 133
 134             savePositionEnabledChanged(true);
 135             break;
 136         case 2:
 137             rot_data_pitch += OpenPilot::CalibrationUtils::listMean(rot_accum_pitch);
 138             rot_data_pitch /= 2;
 139             rot_data_roll  += OpenPilot::CalibrationUtils::listMean(rot_accum_roll);
 140             rot_data_roll  /= 2;
 141
 142             attitudeState->setMetadata(memento.attitudeStateMdata);
 143
 144             m_dirty = true;
 145
 146             stopped();
 147             displayVisualHelp(CALIBRATION_HELPER_IMAGE_EMPTY);
 148             displayInstructions(tr("Board level calibration completed successfully."), WizardModel::Success);
 149             break;
 150         }
 151     }
 152 }
 153
 154 void LevelCalibrationModel::save()
 155 {
 156     if (!m_dirty) {
 157         return;
 158     }
 159     AttitudeSettings::DataFields attitudeSettingsData = attitudeSettings->getData();
 160
 161     // Update the biases based on collected data
 162     // "rotate" the board in the opposite direction as the calculated offset
 163     attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_PITCH] -= rot_data_pitch;
 164     attitudeSettingsData.BoardLevelTrim[AttitudeSettings::BOARDLEVELTRIM_ROLL]  -= rot_data_roll;
 165
 166     attitudeSettings->setData(attitudeSettingsData);
 167
 168     m_dirty = false;
 169 }
 170
 171 UAVObjectManager *LevelCalibrationModel::getObjectManager()
 172 {
 173     ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
 174     UAVObjectManager *objMngr = pm->getObject<UAVObjectManager>();
 175
 176     Q_ASSERT(objMngr);
 177     return objMngr;
 178 }
 179 }