calibration.h

Go to the documentation of this file.

/*
 * 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, see <http://www.gnu.org/licenses/>
 */
#ifndef CALIBRATION_H
#define CALIBRATION_H

#include <config/config_global.h>
#include <uavobjects/uavobjectmanager.h>
#include "uavobjectutil/uavobjectutilmanager.h"
#include <extensionsystem/pluginmanager.h>
#include <uavobjects/uavobject.h>
#include <config/tempcompcurve.h>

#include <QObject>
#include <QTimer>
#include <QString>

class CONFIG_EXPORT Calibration : public QObject
{
    Q_OBJECT

public:
    explicit Calibration();
    ~Calibration();

    void initialize(bool calibrateAccels, bool calibrateMags);

private:
    enum CALIBRATION_STATE {
        IDLE,
        LEVELING,
        YAW_ORIENTATION,
        SIX_POINT_WAIT1,
        SIX_POINT_COLLECT1,
        SIX_POINT_WAIT2,
        SIX_POINT_COLLECT2,
        SIX_POINT_WAIT3,
        SIX_POINT_COLLECT3,
        SIX_POINT_WAIT4,
        SIX_POINT_COLLECT4,
        SIX_POINT_WAIT5,
        SIX_POINT_COLLECT5,
        SIX_POINT_WAIT6,
        SIX_POINT_COLLECT6,
        GYRO_TEMP_CAL
    } calibration_state;

public slots:
    void doStartBiasAndLeveling();

    void doStartNoBiasLeveling();

    void doStartOrientation();

    void doStartSixPoint();

    void doCancelSixPoint();

    void doSaveSixPointPosition();

    void doStartTempCal();

    void doAcceptTempCal();

    void doCancelTempCalPoint();

    void configureTempCurves(TempCompCurve *x, TempCompCurve *y, TempCompCurve *z);

private slots:
    void dataUpdated(UAVObject *);

    void timeout();

public slots:
    void setTempCalRange(int r);

signals:
    void calibrationBusy(bool busy);
    void toggleControls(bool enable);

    void toggleSavePosition(bool enable);

    void updatePlane(int position);

    void showSixPointMessage(QString message);

    void showYawOrientationMessage(QString message);

    void showLevelingMessage(QString message);

    void yawOrientationProgressChanged(int);

    void levelingProgressChanged(int);

    void sixPointProgressChanged(int);

    void showTempCalMessage(QString message);

    void tempCalProgressChanged(int);

    void calibrationCompleted();

private:
    void setMetadata(QMap<QString, UAVObject::Metadata> metaList);

    void doStartLeveling();

    UAVObjectManager *getObjectManager();

    UAVObjectUtilManager *getObjectUtilManager();

    void assignUpdateRate(UAVObject *obj, quint32 updatePeriod);

    QTimer timer;

    bool calibrateMags;

    bool calibrateAccels;

    bool zeroVertical;

    double accelLength;

    QMap<QString, UAVObject::Metadata> originalMetaData;

    QMap<QString, UAVObject::Metadata> slowedDownMetaDataList;

    QList<double> gyro_accum_x;
    QList<double> gyro_accum_y;
    QList<double> gyro_accum_z;
    QList<double> gyro_accum_temp;
    QList<double> accel_accum_x;
    QList<double> accel_accum_y;
    QList<double> accel_accum_z;
    QList<double> mag_accum_x;
    QList<double> mag_accum_y;
    QList<double> mag_accum_z;

    double gyro_data_x[6], gyro_data_y[6], gyro_data_z[6];
    double accel_data_x[6], accel_data_y[6], accel_data_z[6];
    double mag_data_x[6], mag_data_y[6], mag_data_z[6];

    static const int NUM_SENSOR_UPDATES_LEVELING = 300;
    static const int NUM_SENSOR_UPDATES_YAW_ORIENTATION = 300;
    static const int NUM_SENSOR_UPDATES_SIX_POINT = 100;
    static const int SENSOR_UPDATE_PERIOD = 25;
    static const int NON_SENSOR_UPDATE_PERIOD = 0;

    double min_temperature_range;
    double boardRotationMatrix[3][3];
    double initialAccelsScale[3];
    double initialAccelsBias[3];
    double initialMagsScale[3];
    double initialMagsBias[3];

    TempCompCurve *xCurve;
    TempCompCurve *yCurve;
    TempCompCurve *zCurve;

protected:
    enum sensor_type { ACCEL, GYRO, MAG };

    void connectSensor(sensor_type sensor, bool connect);

    bool storeSixPointMeasurement(UAVObject *obj, int position);

    bool storeYawOrientationMeasurement(UAVObject *obj);

    bool storeLevelingMeasurement(UAVObject *obj);

    int computeScaleBias();

    int SixPointInConstFieldCal(double ConstMag, double x[6], double y[6], double z[6], double S[3],
                                double b[3]);

    void rotate_vector(double R[3][3], const double vec[3], double vec_out[3], bool transpose);

    void Euler2R(double rpy[3], double Rbe[3][3]);

    static inline double listMean(QList<double> list)
    {
        return std::accumulate(list.begin(), list.end(), 0.0) / list.size();
    }

    void resetSensorCalibrationToOriginalValues();

    bool storeTempCalMeasurement(UAVObject *obj);

    int computeTempCal();

    void updateTempCompCalibrationDisplay();
};

#endif // CALIBRATION_H