IMUCalibrator.h

#pragma once
#include "DLLInfo.h"
#include <NDEVR/Device.h>
#include <NDEVR/Matrix.h>
#include <NDEVR/Buffer.h>
#include <NDEVR/INIInterface.h>
#include <NDEVR/TranslatedString.h>
#include <NDEVR/CalibrationData.h>
#include <NDEVR/CalibrationPosition.h>
#include <NDEVR/ApplicationResource.h>
namespace NDEVR
{
    class CalibrationData;
    class Model;
    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;
        void setStabilitySpan(const TimeSpan& span) { m_time_to_stable = span; }
        Vertex<3, fltp08> gyroBias() const;
        void setMaxRotationThreshold(const Ray<3, fltp08>& rotation_threshold) { m_rotation_threshold = rotation_threshold;}
        void setMaxAccelerometerThreshold(const Ray<3, fltp08>& rotation_threshold) { m_accelerometer_threshold = rotation_threshold; }
        void setMaxNormalDeviation(const Ray<3, fltp08>& dev) { m_max_norm = dev; }
        Matrix<fltp08> accelerometerTransform() const;
        virtual TranslatedString message() const;
        void setCalibrationData(const CalibrationData& data);
        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>& positions);
        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<CalibrationReading>& accData();
        const Buffer<CalibrationReading>& accData() const;
        virtual void finishCalibration();
        uint04 stepIndex() const { return m_calibration_idx; }
        Ray<3, fltp08> gravityTarget() const;
        fltp08 stdDev(fltp08& mean_vector_length) const;
        void finishStep();
        static ApplicationOption<TimeSpan> default_stability_span; 
        static ApplicationOption<uint04> default_sample_count; 
        static ApplicationOption<fltp08> std_dev_filter; 
    signals:
        void stepFinishedSignal();
    private:
        CalibrationData m_calibration_data; 
        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_accelerometer_threshold = Ray<3, fltp08>(0.02); 
        Ray<3, fltp08> m_max_norm = Vector<3, fltp08>(0.5); 
        TimeSpan m_time_to_stable; 
        Time m_step_start_time = Constant<Time>::Invalid; 
        fltp04 m_percent = 1.0f; 
        uint04 m_samples_to_collect; 
        uint04 m_calibration_idx = 0; 
        uint04 m_valid_step_samples = 0U; 
        Status m_state = Status::e_idle; 
        Time m_calibration_time = Constant<Time>::Invalid; 
        bool m_is_moving = false; 
        bool m_is_in_norm = false; 
    };
}