MadgwickOrientationSensor.h

#pragma once
#include "DLLInfo.h"
#include <NDEVR/OrientationSensor.h>
#include <NDEVR/QueueBuffer.h>
#include <NDEVR/Quaternion.h>
#include <mutex>
namespace NDEVR
{
    namespace IMU
    {
        class Point;
    };
    class DesignObjectLookup;
    class IMUCalibrator;
    struct OrientationReading
    {
        OrientationReading()
        {}
        HARDWARE_API OrientationReading(Quaternion<fltp08> quaternion
            , Vector<3, fltp08> gyro
            , Vector<3, fltp08> accel
            , Vector<3, fltp08> magnet, const Time& time);
        Quaternion<fltp08> quaternion; 
        Vector<3, fltp08> gyro; 
        Vector<3, fltp08> accel; 
        Vector<3, fltp08> magnet; 
        Time time; 
    };
    struct RasterFrame;
    class HARDWARE_API MadgwickOrientationSensor : public OrientationSensor
    {
    public:
        MadgwickOrientationSensor(DesignObjectLookup* lookup, const Model& model,  LogPtr log, QObject* parent = nullptr);
        bool readyToSetupFrame(const Time& cutoff_time) const;
    #if _WIN32
        bool collectRecentRecords(const Time& cutoff_time, AlocatingAlignedBuffer<IMU::Point, 32>& imu_points, const Time& offset_time);
    #endif
        sint02 getPacketGyroState() const;
        void reset();
        void setGyroState(GyroState state);
        void setPacketGyroState(sint02 state);
        void setStoreReadings(bool store_readings);
        void setBeta(fltp08 beta) { m_beta = beta; }
        fltp08 beta() const { return m_beta; }
        bool supportsFixed() const override { return false; }
        bool supportsRodded() const override { return false; }
        void setRequestGyroCallback(const std::function<void()>& callback) { m_request_gyro_callback = callback; }
        void processGyro(Ray<3, fltp08> gyro_data, const Time& ts);
        void processAcceleration(Ray<3, fltp08> accel_data, const Time& ts);
        void processAccelerationAndGyro(Ray<3, fltp08> accel_data, Ray<3, fltp08> gyro_data, const Time& ts);
        void processMagnetometer(Ray<3, fltp08> accel_data, const Time& ts);
        void processQuaternion(const Quaternion<fltp08>& quaternion, const Time& ts);
        bool requestGyroAlignment(bool reference_previous) override;
        void processMadgwick(Time time);
        void updateFromQuaternion(Time time);
        Vector<3, Angle<fltp08>> angleFromQuaternion(const Quaternion<fltp08>& quaternion) const;
        Matrix<fltp08> orientationAt(const Time& time) const;
        Angle<fltp08> angularVelocity() const;
        void initialComputeMadgwick(Vector<3, fltp08> a, Vector<3, fltp08> m, Quaternion<fltp08>& q, Time time);
        static void updateMadgwick(Vector<3, fltp08> g, Vector<3, fltp08> a, Vector<3, fltp08> m, Quaternion<fltp08>& q, TimeSpan time_span, fltp08 beta);
        void finishAlignment();
    protected:
        std::function<void()> m_request_gyro_callback; 
        Buffer<Ray<3, fltp08>> v_accel_data_sync; 
        Buffer<Time> v_gyro_timestamp; 
        Buffer<Ray<3, fltp08>> v_gyro_data; 
        Time m_prev_accel_timestamp = Time(0); 
        Vector<3, fltp08> m_prev_accel_data = { 0,0,0 }; 
        std::mutex m_theta_mtx; 
        Quaternion<fltp08> m_quaternion = Quaternion<fltp08>(1.0, 0.0, 0.0, 0.0); 
        Vector<3, fltp08> m_gyro = Constant<Vector<3, fltp08>>::Invalid; 
        Vector<3, fltp08> m_gyro_calibration = Vector<3, fltp08>(0.0); 
        TimeSpan m_total_ts = TimeSpan(0.0); 
        Time m_start_calibration_time = Constant<Time>::Invalid; 
        TimeSpan m_calibration_period = TimeSpan(5.0); 
        fltp08 m_beta = 0.042; 
        Angle<fltp08> yaw_offset = Constant<Angle<fltp08>>::Invalid; 
        Time m_last_timestamp = Constant<Time>::Invalid; 
        QueueBuffer<OrientationReading> m_historic_orientation; 
        bool m_reset_gyro = true; 
        bool m_store_readings = false; 
        DesignObjectLookup* m_lookup = nullptr; 
        uint04 m_init_count = 0; 
    };
}