IIMUEventDispatcher.cpp

Go to the documentation of this file.

/*
 * IIMUEventDispatcher.cpp
 *
 *  Created on: Mar 16, 2014
 *      Author: Dr.-Ing. David Israel González Aguirre
 *      Mail:   david.gonzalez@kit.edu
 */
 
#include "IIMUEventDispatcher.h"
#include "IMUDevice.h"
 
namespace IMU
{
    IIMUEventDispatcher::IIMUEventDispatcher(CIMUDevice* pIMUDevice) :
        m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(pIMUDevice), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
    {
        pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
        pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
        pthread_mutex_init(&m_EventFlagsMutex, nullptr);
        pthread_mutex_init(&m_IMUDeviceMutex, nullptr);
        pthread_mutex_init(&m_EventsQueueMutex, nullptr);
        pthread_mutex_init(&m_LastStartTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastStopTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, nullptr);
    }
 
    IIMUEventDispatcher::IIMUEventDispatcher() :
        m_DispatchingMode(eCoupled), m_MaximalPendingEvents(2), m_EventFlags(0XFFFF), m_pIMUDevice(nullptr), m_EventsQueue(), m_LastStartTimeStamp(CTimeStamp::s_Zero), m_LastStopTimeStamp(CTimeStamp::s_Zero), m_LastCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastFusedCycleReferenceTimeStamp(CTimeStamp::s_Zero), m_LastIntegratedStateReferenceTimeStamp(CTimeStamp::s_Zero), m_LastCustomEventReferenceTimeStamp(CTimeStamp::s_Zero)
    {
        pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
        pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
        pthread_mutex_init(&m_EventFlagsMutex, nullptr);
        pthread_mutex_init(&m_IMUDeviceMutex, nullptr);
        pthread_mutex_init(&m_EventsQueueMutex, nullptr);
        pthread_mutex_init(&m_LastStartTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastStopTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, nullptr);
        pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, nullptr);
    }
 
    IIMUEventDispatcher::~IIMUEventDispatcher()
    {
        if (m_pIMUDevice)
        {
            m_pIMUDevice->UnregisterEventDispatcher(this);
        }
    }
 
    void IIMUEventDispatcher::SetIMU(CIMUDevice* pIMUDevice)
    {
        _MINIMAL___LOCK(m_IMUDeviceMutex)
        m_pIMUDevice = pIMUDevice;
        _MINIMAL_UNLOCK(m_IMUDeviceMutex)
    }
 
    uint32_t IIMUEventDispatcher::GetEventFlags()
    {
        _MINIMAL___LOCK(m_EventFlagsMutex)
        const uint32_t EventFlagsCurrentState = m_EventFlags;
        _MINIMAL_UNLOCK(m_EventFlagsMutex)
        return EventFlagsCurrentState;
    }
 
    void IIMUEventDispatcher::SetDispatchingMode(const DispatchingMode Mode)
    {
        _MINIMAL___LOCK(m_DispatchingModeMutex)
        m_DispatchingMode = Mode;
        _MINIMAL_UNLOCK(m_DispatchingModeMutex)
    }
 
    IIMUEventDispatcher::DispatchingMode IIMUEventDispatcher::GetDispatchingMode()
    {
        _MINIMAL___LOCK(m_DispatchingModeMutex)
        const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
        _MINIMAL_UNLOCK(m_DispatchingModeMutex)
        return DispatchingModeCurrentState;
    }
 
    void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
    {
        if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
        {
            _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
            m_MaximalPendingEvents = MaximalPendingEvents;
            _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
 
            if (m_DispatchingMode == eDecoupled)
            {
                PurgeEvents();
            }
        }
    }
 
    uint32_t IIMUEventDispatcher::GetMaximalPendingEvents()
    {
        _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
        const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
        _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
        return MaximalPendingEventsCurrentState;
    }
 
    void IIMUEventDispatcher::SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
    {
        _MINIMAL___LOCK(m_EventFlagsMutex)
        m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
        _MINIMAL_UNLOCK(m_EventFlagsMutex)
    }
 
    uint32_t IIMUEventDispatcher::GetEventHandlingFlags()
    {
        _MINIMAL___LOCK(m_EventFlagsMutex);
        const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
        _MINIMAL_UNLOCK(m_EventFlagsMutex);
        return EventHandlingFlagsCurrentState;
    }
 
    void IIMUEventDispatcher::ReceiveEvent(const CIMUEvent& Event)
    {
        _MINIMAL___LOCK(m_EventFlagsMutex)
        const bool HandelEvent = Event.GetEventType() & m_EventFlags;
        _MINIMAL_UNLOCK(m_EventFlagsMutex)
 
        if (HandelEvent)
        {
            if (m_DispatchingMode == eDecoupled)
            {
                _MINIMAL___LOCK(m_EventsQueueMutex)
 
                if (m_EventsQueue.size() == m_MaximalPendingEvents)
                {
                    m_EventsQueue.pop_front();
                }
 
                m_EventsQueue.push_back(Event);
                _MINIMAL_UNLOCK(m_EventsQueueMutex)
 
                switch (Event.GetEventType())
                {
                    case CIMUEvent::eOnIMUCycle:
                        _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
                        gettimeofday(&m_LastCycleReferenceTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
                        return;
 
                    case CIMUEvent::eOnIMUFusedCycle:
                        _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
                        gettimeofday(&m_LastFusedCycleReferenceTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
                        return;
 
                    case CIMUEvent::eOnIMUIntegratedState:
                        _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
                        gettimeofday(&m_LastIntegratedStateReferenceTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
                        return;
 
                    case CIMUEvent::eOnIMUCustomEvent:
                        _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
                        gettimeofday(&m_LastCustomEventReferenceTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
                        return;
 
                    case CIMUEvent::eOnIMUStart:
                        _MINIMAL___LOCK(m_LastStartTimeStampMutex)
                        gettimeofday(&m_LastStartTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
                        return;
 
                    case CIMUEvent::eOnIMUStop:
                        _MINIMAL___LOCK(m_LastStopTimeStampMutex)
                        gettimeofday(&m_LastStopTimeStamp, nullptr);
                        _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
                        return;
                }
            }
            else
            {
                OnIMUEvent(Event);
            }
        }
    }
 
    bool IIMUEventDispatcher::ProcessPendingEvent()
    {
        _MINIMAL___LOCK(m_EventsQueueMutex)
 
        if (m_EventsQueue.size())
        {
            OnIMUEvent(m_EventsQueue.front());
            m_EventsQueue.pop_front();
            _MINIMAL_UNLOCK(m_EventsQueueMutex)
            return true;
        }
        else
        {
            _MINIMAL_UNLOCK(m_EventsQueueMutex)
            return false;
        }
    }
 
    void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
    {
        _MINIMAL___LOCK(m_LastStartTimeStampMutex)
        m_LastStartTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
 
        _MINIMAL___LOCK(m_LastStopTimeStampMutex)
        m_LastStopTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
 
        _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
        m_LastCycleReferenceTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
 
        _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
        m_LastFusedCycleReferenceTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
 
        _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
        m_LastIntegratedStateReferenceTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
 
        _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
        m_LastCustomEventReferenceTimeStamp = Reference;
        _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
    }
 
    timeval IIMUEventDispatcher::GetLastStartTimeStamp()
    {
        _MINIMAL___LOCK(m_LastStartTimeStampMutex)
        timeval TimeStampCurrentState = m_LastStartTimeStamp;
        _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    timeval IIMUEventDispatcher::GetLastStopTimeStamp()
    {
        _MINIMAL___LOCK(m_LastStopTimeStampMutex)
        timeval TimeStampCurrentState = m_LastStopTimeStamp;
        _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    timeval IIMUEventDispatcher::GetLastCycleReferenceTimeStamp()
    {
        _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
        timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
        _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    timeval IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp()
    {
        _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
        timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
        _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    timeval IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp()
    {
        _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
        timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
        _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    timeval IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp()
    {
        _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
        timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
        _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
        return TimeStampCurrentState;
    }
 
    void IIMUEventDispatcher::PurgeEvents()
    {
        _MINIMAL___LOCK(m_EventsQueueMutex)
 
        if (m_EventsQueue.size() >= m_MaximalPendingEvents)
        {
            const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
 
            for (uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
            {
                m_EventsQueue.pop_front();
            }
        }
 
        _MINIMAL_UNLOCK(m_EventsQueueMutex)
    }
 
}