IMUCalibrator.h

Go to the documentation of this file.

#pragma once
#include <NDEVR/Device.h>
#include <NDEVR/Matrix.h>
#include <NDEVR/Buffer.h>
#include <NDEVR/INIInterface.h>
#include <NDEVR/TranslatedString.h>
 
namespace NDEVR
{
    /**--------------------------------------------------------------------------------------------------
    \brief Represents a location used for calibrating an IMU. Described by the expected gravity vector for
        that location.
    **/
    struct CalibrationPosition
    {
        CalibrationPosition(const TranslatedString& location_title, const String& image, const Ray<3, fltp08>& gravity)
            : gravity(gravity)
            , location_title(location_title)
            , location_icon(image)
        {}
        Ray<3, fltp08> gravity;
        Buffer<std::pair<Time, Ray<3, fltp08>>> acc_data;
        TranslatedString location_title;
        String location_icon;
        void clearData()
        {
            acc_data.clear();
        }
    };
    /**--------------------------------------------------------------------------------------------------
    \brief Logic and storage for IMU calibration. An IMU calibration works by moving the sensor into a
        series of predefined locations and using this motion to calibrated the accelerometer and gyro.
    **/
    class HARDWARE_API IMUCalibrator : public Device, public INIInterface
    {
        Q_OBJECT
        enum Status
        {
              e_idle
            , e_rotate_to_position
            , e_wait_to_stable
            , e_collect_data
        };
    public:
        IMUCalibrator(const UUID& id, const Model& model);
        IMUCalibrator(const File& location, const Model& model);
        bool isActive() const;
        virtual fltp04 percent() const;
        Status status() const;
        void setSamplesToCollect(uint04 samples);
        Time calibrationTime() const;
        Vertex<3, fltp08> gyroBias() const;
        void setMaxRotationThreshold(const Ray<3, fltp08>& rotation_threshold) { m_rotation_threshold = rotation_threshold;}
        void setMaxNormalDeviation(const Ray<3, fltp08>& dev) { m_max_norm = dev; }
        Matrix<fltp08> accelerometerTransform() const;
        virtual TranslatedString message() const;
        virtual String iconID() const;
        void onReading(const Ray<3, fltp08>& accel, const Ray<3, fltp08>& gyro, Time t);
        bool canCalibrate() const;
        void setCalibrationPositions(const Buffer<CalibrationPosition>& postions);
        const Buffer<CalibrationPosition>& calibrationPositions() const { return m_calibration_positions; };
        void startCalibration();
        void cancelCalibration();
        Ray<3, fltp08> expectedGravity() const;
        virtual void getINI(INIFactory& factory) override;
        Buffer<std::pair<Time, Ray<3, fltp08>>>& gyroData();
        Buffer<std::pair<Time, Ray<3, fltp08>>>& accData();
        virtual void finishCalibration();
        uint04 stepIndex() const { return m_calibration_idx; }
        Ray<3, fltp08> gravityTarget() const;
    signals:
        void stepFinishedSignal();
    private:
        Matrix<fltp08> m_accelerometer_calibration = Constant<Matrix<fltp08>>::Invalid;
        Buffer<std::pair<Time, Ray<3, fltp08>>> m_collected_gyro_data;
        Buffer<CalibrationPosition> m_calibration_positions;
        Ray<3, fltp08> m_gyro_bias = Ray<3, fltp08>(0.0);
        Ray<3, fltp08> m_rotation_threshold = Ray<3, fltp08>(0.1);
        Ray<3, fltp08> m_max_norm = Vector<3, fltp08>(0.5);
        TimeSpan m_time_to_stable = TimeSpan(3.0);
        Time m_step_start_time = Constant<Time>::Invalid;
        fltp04 m_percent = 1.0f;
        uint04 m_samples_to_collect = 1000;
        uint04 m_calibration_idx = 0; 
        Status m_state = Status::e_idle;
        Time m_calibration_time = Constant<Time>::Invalid;
        bool m_is_moving = false;
        bool m_is_in_norm = false;
    };
}