IMUDevice.h

Go to the documentation of this file.

/*
 * IMU.h
 *
 *  Created on: Mar 16, 2014
 *      Author: Dr.-Ing. David Israel González Aguirre
 *      Mail:   david.gonzalez@kit.edu
 */
 
#pragma once
 
#include "Includes.h"
#include "IMUHelpers.h"
#include "IMUEvent.h"
#include "IMUState.h"
 
#ifdef _IMU_USE_XSENS_DEVICE_
 
#include "Xsens/Xsens.h"
#include "Xsens/XsensMTiModule.h"
 
#endif
 
#define _IMU_DEVICE_DEFAUL_CONNECTION_ std::string("/dev/ttyUSB0")
#define _IMU_DEVICE_DEFAUL_FREQUENCY_ IMU::CIMUDevice::eSamplingFrequency_120HZ
#define _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_ 0.5f
#define _IMU_DEVICE_DEFAUL_NORMALIZED_THREAD_PRIORITY_ 0.125f
 
namespace IMU
{
    //Forward definition
    class IIMUEventDispatcher;
 
    /*!
     \class CIMUDevice
     \ingroup IMU
     \brief This class contains the the devices module and the thread for read the measurements.
 
     CIMUDevice encapsulates the device details for the rest of the library and applications.
     This also includes a thread which is in charge of generate the IMU events
     */
    class CIMUDevice
    {
    public:
 
        /*!
         \brief Enum specifying the running thread policy.
         */
        enum ThreadPolicyType
        {
            eRealTime = SCHED_FIFO, eRoundRobinPriorityBased = SCHED_RR, eBatch = SCHED_BATCH, eIdle = SCHED_IDLE
        };
 
        /*!
         \brief Enum specifying the supported sampling frequencies.
         */
        enum SamplingFrequency
        {
            eSamplingFrequency_10HZ = 0x2D00,
            eSamplingFrequency_12HZ = 0x2580,
            eSamplingFrequency_15HZ = 0x1E00,
            eSamplingFrequency_16HZ = 0x1C20,
            eSamplingFrequency_18HZ = 0x1900,
            eSamplingFrequency_20HZ = 0x1680,
            eSamplingFrequency_24HZ = 0x12C0,
            eSamplingFrequency_25HZ = 0x1200,
            eSamplingFrequency_30HZ = 0x0F00,
            eSamplingFrequency_32HZ = 0x0E10,
            eSamplingFrequency_36HZ = 0x0C80,
            eSamplingFrequency_40HZ = 0x0B40,
            eSamplingFrequency_45HZ = 0x0A00,
            eSamplingFrequency_48HZ = 0x0960,
            eSamplingFrequency_50HZ = 0x0900,
            eSamplingFrequency_60HZ = 0x0780,
            eSamplingFrequency_64HZ = 0x0708,
            eSamplingFrequency_72HZ = 0x0640,
            eSamplingFrequency_75HZ = 0x0600,
            eSamplingFrequency_80HZ = 0x05A0,
            eSamplingFrequency_90HZ = 0x0500,
            eSamplingFrequency_96HZ = 0x04B0,
            eSamplingFrequency_100HZ = 0x0480,
            eSamplingFrequency_120HZ = 0x03C0,
            eSamplingFrequency_128HZ = 0x0384,
            eSamplingFrequency_144HZ = 0x0320,
            eSamplingFrequency_150HZ = 0x0300,
            eSamplingFrequency_160HZ = 0x02D0,
            eSamplingFrequency_180HZ = 0x0280,
            eSamplingFrequency_192HZ = 0x0258,
            eSamplingFrequency_200HZ = 0x0240,
            eSamplingFrequency_225HZ = 0x0200,
            eSamplingFrequency_240HZ = 0x01E0,
            eSamplingFrequency_256HZ = 0x01C2,
            eSamplingFrequency_288HZ = 0x0190,
            eSamplingFrequency_300HZ = 0x0180,
            eSamplingFrequency_320HZ = 0x0168,
            eSamplingFrequency_360HZ = 0x0140,
            eSamplingFrequency_384HZ = 0x012C,
            eSamplingFrequency_400HZ = 0x0120,
            eSamplingFrequency_450HZ = 0x0100,
            eSamplingFrequency_480HZ = 0x00F0,
            eSamplingFrequency_512HZ = 0x00E1
        };
 
        enum FusionStrategy
        {
            eNoFusion, eMeanFusion, eGaussianFusion
        };
 
        /*!
         \brief The default constructor.
         The default constructor sets all member variables to zero, i.e. after construction no valid device nor thread are represented.
         */
        CIMUDevice();
 
        /*!
         \brief The destructor.
         */
        virtual ~CIMUDevice();
 
        uint64_t GetDeviceId() const;
 
        inline IMUState GetIMUState() const
        {
 
#ifdef _IMU_USE_XSENS_DEVICE_
 
            return m_XsensMTiFrame.m_IMUState;
 
#endif
 
        }
 
        bool Connect(const std::string& PortName, const SamplingFrequency Frequency);
 
        bool Start(const bool Blocking = true);
 
        bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority);
 
        void Stop(const bool Blocking = true);
 
        bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion);
 
        bool IsActive() const;
 
        bool RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
 
        bool UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher);
 
        inline const timeval& GetReferenceTimeStamp() const
        {
            return m_ReferenceTimeStamp;
        }
 
    protected:
 
        bool LoadCurrentState();
 
        bool MeanFuseCurrentState();
        void IncorporateCurrentStateMeanFusion();
        void MeanFusion();
 
        bool GaussianFuseCurrentState();
        void IncorporateCurrentStateGaussianFusion();
        void GaussianFusion();
 
        bool IntegrateCurrentState();
        bool IntegrateWithOutFusion();
        bool IntegrateWithFusion();
 
    private:
 
        void UnregisterEventDispatchers();
 
        bool InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency);
        void FinalizeModuleDevice();
        void ShouldYield();
        bool DispatchCylcle();
        void SendEvent(const CIMUEvent& Event);
        void SetReferenceTimeStamps();
 
        static void* ThreadLoop(void* pData);
 
        uint64_t m_DeviceId;
        SamplingFrequency m_SamplingFrequency;
        int m_PeriodMicroSeconds;
        FusionStrategy m_FusionStrategy;
        int m_SamplesPerFusion;
        int m_CollectedFusionSamples;
        volatile bool m_IsActive;
        volatile bool m_IsDispatching;
        bool m_IsInitialized;
        pthread_t m_pInternalThreadHandel;
        pthread_mutex_t m_IsActiveMutex;
        pthread_mutex_t m_IsDispatchingMutex;
        pthread_mutex_t m_EventDispatchersMutex;
        pthread_mutex_t m_DeviceMutex;
        std::set<IIMUEventDispatcher*> m_IMUEventDispatchers;
        timeval m_ReferenceTimeStamp;
        timeval m_LastFrameTimeStamp;
 
        IMUState::PhysicalData m_FusedPhysicalData;
        IMUState m_FusedIMUState;
        IMUState m_IntegratedIMUState;
 
#ifdef _IMU_USE_XSENS_DEVICE_
 
        bool InitializeXsensDevice(const std::string& PortName, const SamplingFrequency Frequency);
        void FinalizeXsensModuleDevice();
        void DestroyXsensModuleDevice();
 
        Xsens::CXsensMTiModule* m_pXsensMTiModule;
        Xsens::XsensMTiFrame m_XsensMTiFrame;
 
#endif
 
    };
}