MadgwickOrientationSensor.h

Go to the documentation of this file.

#pragma once
#include <NDEVR/OrientationSensor.h>
#include <NDEVR/QueueBuffer.h>
#include <NDEVR/Quaternion.h>
#include <NDEVR/IMU.h>
#include <mutex>
namespace NDEVR
{
    class IMUCalibrator;
    /**--------------------------------------------------------------------------------------------------
    \brief A single reading, typically used by a MadgwickOrientationSensor consisiting of input from all
        IMU sensors. These might be interpolated values.
    **/
    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;
    /**--------------------------------------------------------------------------------------------------
    \brief Used to calculate madgick orientation given sensor streams for gyro and optionally magnetometer(s) and accelerometer(s)
    \ingroup Hardware Magnetometer Orientation Gyro Accelerometer
    **/
    class HARDWARE_API MadgwickOrientationSensor : public OrientationSensor
    {
    public:
        MadgwickOrientationSensor(const Model& model, ProgressInfo* log, QObject* parent = nullptr);
        bool readyToSetupFrame() const;
    #if _WIN32
        bool collectRecentRecords(const Time& cutoff_time, Buffer<IMU::Point, uint04, ObjectAllocator<false>>& 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);
        bool supportsFixed() const override { return false; }
        bool supportsRodded() const override { return false; }
        void setRequestGyroCallback(const std::function<void()>& callback) { m_request_gyro_callback = callback; }
        // Function to calculate the change in angle of motion based on data from gyro
        void processGyro(Ray<3, fltp08> gyro_data, const Time& ts);
        void processAcceleration(Ray<3, fltp08> accel_data, const Time& ts);
        void processQuaternion(const Quaternion<fltp08>& quaternion, const Time& ts);
        bool requestGyroCalibration() 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);
    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 };
        // theta is the angle of camera rotation in x, y and z components
        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_accel = Constant<Vector<3, fltp08>>::Invalid;
        Vector<3, fltp08> m_magnet = Constant<Vector<3, fltp08>>::Invalid;
        Vector<3, fltp08> m_gyro_calibration = Vector<3, fltp08>(0.0);
        TimeSpan m_total_ts;
        Time m_start_calibration_time = Constant<Time>::Invalid;
        TimeSpan m_calibration_time = TimeSpan(5.0);
        fltp08 m_beta = 0.042;
        Angle<fltp08> yaw_offset = Constant<Angle<fltp08>>::Invalid;
        // Keeps the arrival time of previous gyro frame
        Time m_last_timestamp = Constant<Time>::Invalid;
        QueueBuffer<OrientationReading, uint04, ObjectAllocator<true, uint04>> m_historic_orientation;
        bool m_reset_gyro = true;
        bool m_store_readings = false;
        uint04 m_init_count = 0;
    };
}