ground/gcs/src/plugins/config/calibration/sixpointcalibrationmodel.h

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       sixpointcalibrationmodel.h
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  *
   7  * @brief      Six point calibration for Magnetometer and Accelerometer
   8  * @see        The GNU Public License (GPL) Version 3
   9  * @defgroup
  10  * @{
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28 #ifndef SIXPOINTCALIBRATIONMODEL_H
  29 #define SIXPOINTCALIBRATIONMODEL_H
  30
  31 #include "wizardmodel.h"
  32 #include "calibration/calibrationutils.h"
  33 #include <revocalibration.h>
  34
  35 #include <auxmagsettings.h>
  36 #include <accelgyrosettings.h>
  37 #include <homelocation.h>
  38 #include <accelstate.h>
  39 #include <magsensor.h>
  40 #include <auxmagsensor.h>
  41
  42 #include <QMutex>
  43 #include <QObject>
  44 #include <QList>
  45 #include <QString>
  46
  47 namespace OpenPilot {
  48 class SixPointCalibrationModel : public QObject {
  49     Q_OBJECT
  50
  51 public:
  52     explicit SixPointCalibrationModel(QObject *parent = 0);
  53
  54     bool dirty()
  55     {
  56         return m_dirty;
  57     }
  58
  59 signals:
  60     void started();
  61     void stopped();
  62     void storeAndClearBoardRotation();
  63     void recallBoardRotation();
  64     void savePositionEnabledChanged(bool state);
  65     void progressChanged(int value);
  66     void displayVisualHelp(QString elementID);
  67     void displayInstructions(QString text, WizardModel::MessageType type = WizardModel::Info);
  68
  69 public slots:
  70     void magStart();
  71     void accelStart();
  72     void savePositionData();
  73     void save();
  74
  75 private slots:
  76     void getSample(UAVObject *obj);
  77     void continouslyGetMagSamples(UAVObject *obj);
  78
  79 private:
  80     class CalibrationStep {
  81 public:
  82         CalibrationStep(QString newVisualHelp, QString newInstructions)
  83         {
  84             visualHelp   = newVisualHelp;
  85             instructions = newInstructions;
  86         }
  87         QString visualHelp;
  88         QString instructions;
  89     };
  90
  91     typedef struct {
  92         UAVObject::Metadata accelStateMetadata;
  93         UAVObject::Metadata magSensorMetadata;
  94         UAVObject::Metadata auxMagSensorMetadata;
  95         AuxMagSettings::DataFields    auxMagSettings;
  96         RevoCalibration::DataFields   revoCalibrationData;
  97         AccelGyroSettings::DataFields accelGyroSettingsData;
  98     } Memento;
  99
 100     typedef struct {
 101         RevoCalibration::DataFields   revoCalibrationData;
 102         AuxMagSettings::DataFields    auxMagSettingsData;
 103         AccelGyroSettings::DataFields accelGyroSettingsData;
 104     } Result;
 105
 106     bool calibratingMag;
 107     bool calibratingAuxMag;
 108     bool calibratingAccel;
 109
 110     QList<CalibrationStep> calibrationStepsMag;
 111     QList<CalibrationStep> calibrationStepsAccelOnly;
 112     QList<CalibrationStep> *currentSteps;
 113     int position;
 114
 115     Memento memento;
 116     Result result;
 117
 118     bool collectingData;
 119     bool m_dirty;
 120
 121     QMutex sensorsUpdateLock;
 122
 123     double accel_data_x[6], accel_data_y[6], accel_data_z[6];
 124
 125     QList<double> accel_accum_x;
 126     QList<double> accel_accum_y;
 127     QList<double> accel_accum_z;
 128
 129     QList<double> mag_accum_x;
 130     QList<double> mag_accum_y;
 131     QList<double> mag_accum_z;
 132     QList<float> mag_fit_x;
 133     QList<float> mag_fit_y;
 134     QList<float> mag_fit_z;
 135
 136     QList<double> aux_mag_accum_x;
 137     QList<double> aux_mag_accum_y;
 138     QList<double> aux_mag_accum_z;
 139     QList<float> aux_mag_fit_x;
 140     QList<float> aux_mag_fit_y;
 141     QList<float> aux_mag_fit_z;
 142
 143     // convenience pointers
 144     RevoCalibration *revoCalibration;
 145     AccelGyroSettings *accelGyroSettings;
 146     AuxMagSettings *auxMagSettings;
 147     AccelState *accelState;
 148     MagSensor *magSensor;
 149     AuxMagSensor *auxMagSensor;
 150     HomeLocation *homeLocation;
 151
 152     void start(bool calibrateAccel, bool calibrateMag);
 153     // Computes the scale and bias of the mag based on collected data
 154     void compute();
 155     void showHelp(QString image);
 156     UAVObjectManager *getObjectManager();
 157     void calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
 158 };
 159 }
 160
 161 #endif // SIXPOINTCALIBRATIONMODEL_H