MadgwickSimulator.h

#pragma once
#include "DLLInfo.h"
#include <NDEVR/Model.h>
#include <NDEVR/GaussianRN.h>
#include <NDEVR/MadgwickOrientationSensor.h>
#include <NDEVR/ApplicationOption.h>
#include <NDEVR/Resource.h>
#include "IMUNoiseSimulator.h"
namespace NDEVR
{
    class TranslatedString;
    class PipeInfo;
    class QTModelManager;
    class DesignObjectLookup;
    class MadgwickOptions
    {
    public:
        fltp08 simulation_multiplier = 1.0; 
        fltp08 gyro_input_range = 480.0; 
        fltp08 beta = 0.08; 
        fltp08 acceleration_multiplier = 5.0; 
        fltp08 gyro_frequency = 250.0; 
        Ray<3, fltp08> sensor_misalignment = Ray<3, fltp08>(0.0); 
        bool use_gyro = true; 
        bool use_accelerometer = true; 
        bool use_madgwick = true; 
    };

    struct SimulationDataPoint
    {
        Ray<3, fltp08> raw_gyro; 
        Ray<3, fltp08> raw_acceleromater; 
        Vector<4, fltp08> madgwick_quaternion; 
        Time time; 
    };

    class HARDWARE_API MadgwickSimulator : public Device
    {
    public:
        MadgwickSimulator(DesignObjectLookup* manager, const Model& model,  LogPtr log);
        static constexpr StringView TypeName() { return "madgwick_simulator"; }
        MadgwickOrientationSensor* orientationSensor();
        Time simulatedTime() const;
        Time simulationStartTime() const;
        IMUNoiseSimulator& noiseModel() { return m_noise_simulator; }
        virtual void update(Time time) override;
        Ray<3, fltp08> gyroData(const Matrix<fltp08>& target_matrix, Matrix<fltp08>& last_gyro_matrix);
        Ray<3, fltp08> accelerationData(const Matrix<fltp08>& target_matrix);
        Ray<3, Angle<fltp08>> currentError() const;
        void setMovement(const Vector<2, fltp08>& movement);
        virtual TimeSpan updateInterval() const override;
        void updateText(const TranslatedString& name, Model& model);
        void setMadgwickOptions(const MadgwickOptions& options);
        MadgwickOptions madgwickOptions() const;
        void setRecording(bool is_recording);
        void saveStats(File file) const;
        void readFile(const File& file);
        void reset();
        MadgwickOrientationSensor* createOrientationSensor();
        void createCalculatedModel();
        Model referenceModel() const { return getChild(0); }
        Model referenceTransformModel() const { return getChild(0).getChild(0); }
        void setActualDataPoints(const Buffer<SimulationDataPoint>& points);
        void setOrientationSensor(MadgwickOrientationSensor* sensor) { m_orientation_sensor = sensor; }
        const Buffer<SimulationDataPoint>& actualData() const { return m_actual_data_points; }
    public:
        static ApplicationOption<fltp08> gyro_noise; 
        static ApplicationOption<fltp08> accelerometer_noise; 
        static Resource<String> model_location; 
    protected:
        void init();
        void setupChild(const TranslatedString& name, Model& model);
        IMUNoiseSimulator m_noise_simulator; 
        MadgwickOrientationSensor* m_orientation_sensor = nullptr; 
        MadgwickOptions m_madgwick_options; 
        Time m_simulated_last_update_time = Constant<Time>::Invalid; 
        Time m_last_text_update_time = Time(0); 
        Time m_last_real_time = Constant<Time>::Invalid; 
        TimeSpan m_gyro_td = TimeSpan(1/250.0); 
        Vector<2, fltp08> m_accelerated_movement = Vector<2, fltp08>(0.0); 
        Vector<2, fltp08> m_movement = Vector<2, fltp08>(0.0); 
        Vector<3, fltp08> m_axis_accelerometer_offset = Vector<3, fltp08>(0.0); 
        Vector<3, fltp08> m_axis_gyro_offset = Vector<3, fltp08>(0.0); 
        Matrix<fltp08> m_last_gyro_matrix = Constant<Matrix<fltp08>>::Invalid; 
        DesignObjectLookup* m_manager; 
        Buffer<SimulationDataPoint> m_data_points; 
        Buffer<SimulationDataPoint> m_actual_data_points; 
        uint04 m_actual_data_location = Constant<uint04>::Invalid; 
        bool m_is_recording = false; 
    };
}