IIMUEventDispatcher.cpp
Go to the documentation of this file.
1 /*
2  * IIMUEventDispatcher.cpp
3  *
4  * Created on: Mar 16, 2014
5  * Author: Dr.-Ing. David Israel Gonz├ílez Aguirre
6  * Mail: david.gonzalez@kit.edu
7  */
8 
9 #include "IIMUEventDispatcher.h"
10 #include "IMUDevice.h"
11 
12 namespace IMU
13 {
15  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)
16  {
17  pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
18  pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
19  pthread_mutex_init(&m_EventFlagsMutex, nullptr);
20  pthread_mutex_init(&m_IMUDeviceMutex, nullptr);
21  pthread_mutex_init(&m_EventsQueueMutex, nullptr);
22  pthread_mutex_init(&m_LastStartTimeStampMutex, nullptr);
23  pthread_mutex_init(&m_LastStopTimeStampMutex, nullptr);
24  pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, nullptr);
25  pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, nullptr);
26  pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, nullptr);
27  pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, nullptr);
28  }
29 
31  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)
32  {
33  pthread_mutex_init(&m_DispatchingModeMutex, nullptr);
34  pthread_mutex_init(&m_MaximalPendingEventsMutex, nullptr);
35  pthread_mutex_init(&m_EventFlagsMutex, nullptr);
36  pthread_mutex_init(&m_IMUDeviceMutex, nullptr);
37  pthread_mutex_init(&m_EventsQueueMutex, nullptr);
38  pthread_mutex_init(&m_LastStartTimeStampMutex, nullptr);
39  pthread_mutex_init(&m_LastStopTimeStampMutex, nullptr);
40  pthread_mutex_init(&m_LastCycleReferenceTimeStampMutex, nullptr);
41  pthread_mutex_init(&m_LastFusedCycleReferenceTimeStampMutex, nullptr);
42  pthread_mutex_init(&m_LastIntegratedStateReferenceTimeStampMutex, nullptr);
43  pthread_mutex_init(&m_LastCustomEventReferenceTimeStampMutex, nullptr);
44  }
45 
47  {
48  if (m_pIMUDevice)
49  {
50  m_pIMUDevice->UnregisterEventDispatcher(this);
51  }
52  }
53 
55  {
56  _MINIMAL___LOCK(m_IMUDeviceMutex)
57  m_pIMUDevice = pIMUDevice;
58  _MINIMAL_UNLOCK(m_IMUDeviceMutex)
59  }
60 
62  {
63  _MINIMAL___LOCK(m_EventFlagsMutex)
64  const uint32_t EventFlagsCurrentState = m_EventFlags;
65  _MINIMAL_UNLOCK(m_EventFlagsMutex)
66  return EventFlagsCurrentState;
67  }
68 
70  {
71  _MINIMAL___LOCK(m_DispatchingModeMutex)
72  m_DispatchingMode = Mode;
73  _MINIMAL_UNLOCK(m_DispatchingModeMutex)
74  }
75 
77  {
78  _MINIMAL___LOCK(m_DispatchingModeMutex)
79  const DispatchingMode DispatchingModeCurrentState = m_DispatchingMode;
80  _MINIMAL_UNLOCK(m_DispatchingModeMutex)
81  return DispatchingModeCurrentState;
82  }
83 
84  void IIMUEventDispatcher::SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
85  {
86  if ((MaximalPendingEvents > 1) && (MaximalPendingEvents != m_MaximalPendingEvents))
87  {
88  _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
89  m_MaximalPendingEvents = MaximalPendingEvents;
90  _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
91 
92  if (m_DispatchingMode == eDecoupled)
93  {
94  PurgeEvents();
95  }
96  }
97  }
98 
100  {
101  _MINIMAL___LOCK(m_MaximalPendingEventsMutex)
102  const uint32_t MaximalPendingEventsCurrentState = m_MaximalPendingEvents;
103  _MINIMAL_UNLOCK(m_MaximalPendingEventsMutex)
104  return MaximalPendingEventsCurrentState;
105  }
106 
108  {
109  _MINIMAL___LOCK(m_EventFlagsMutex)
110  m_EventFlags = Enabled ? (m_EventFlags | Type) : (m_EventFlags & (~Type));
111  _MINIMAL_UNLOCK(m_EventFlagsMutex)
112  }
113 
115  {
116  _MINIMAL___LOCK(m_EventFlagsMutex);
117  const uint32_t EventHandlingFlagsCurrentState = m_EventFlags;
118  _MINIMAL_UNLOCK(m_EventFlagsMutex);
119  return EventHandlingFlagsCurrentState;
120  }
121 
123  {
124  _MINIMAL___LOCK(m_EventFlagsMutex)
125  const bool HandelEvent = Event.GetEventType() & m_EventFlags;
126  _MINIMAL_UNLOCK(m_EventFlagsMutex)
127 
128  if (HandelEvent)
129  {
130  if (m_DispatchingMode == eDecoupled)
131  {
132  _MINIMAL___LOCK(m_EventsQueueMutex)
133 
134  if (m_EventsQueue.size() == m_MaximalPendingEvents)
135  {
136  m_EventsQueue.pop_front();
137  }
138 
139  m_EventsQueue.push_back(Event);
140  _MINIMAL_UNLOCK(m_EventsQueueMutex)
141 
142  switch (Event.GetEventType())
143  {
145  _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
146  gettimeofday(&m_LastCycleReferenceTimeStamp, nullptr);
147  _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
148  return;
149 
151  _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
152  gettimeofday(&m_LastFusedCycleReferenceTimeStamp, nullptr);
153  _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
154  return;
155 
157  _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
158  gettimeofday(&m_LastIntegratedStateReferenceTimeStamp, nullptr);
159  _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
160  return;
161 
163  _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
164  gettimeofday(&m_LastCustomEventReferenceTimeStamp, nullptr);
165  _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
166  return;
167 
169  _MINIMAL___LOCK(m_LastStartTimeStampMutex)
170  gettimeofday(&m_LastStartTimeStamp, nullptr);
171  _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
172  return;
173 
175  _MINIMAL___LOCK(m_LastStopTimeStampMutex)
176  gettimeofday(&m_LastStopTimeStamp, nullptr);
177  _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
178  return;
179  }
180  }
181  else
182  {
183  OnIMUEvent(Event);
184  }
185  }
186  }
187 
189  {
190  _MINIMAL___LOCK(m_EventsQueueMutex)
191 
192  if (m_EventsQueue.size())
193  {
194  OnIMUEvent(m_EventsQueue.front());
195  m_EventsQueue.pop_front();
196  _MINIMAL_UNLOCK(m_EventsQueueMutex)
197  return true;
198  }
199  else
200  {
201  _MINIMAL_UNLOCK(m_EventsQueueMutex)
202  return false;
203  }
204  }
205 
206  void IIMUEventDispatcher::SetReferenceTimeStamps(const timeval& Reference)
207  {
208  _MINIMAL___LOCK(m_LastStartTimeStampMutex)
209  m_LastStartTimeStamp = Reference;
210  _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
211 
212  _MINIMAL___LOCK(m_LastStopTimeStampMutex)
213  m_LastStopTimeStamp = Reference;
214  _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
215 
216  _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
217  m_LastCycleReferenceTimeStamp = Reference;
218  _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
219 
220  _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
221  m_LastFusedCycleReferenceTimeStamp = Reference;
222  _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
223 
224  _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
225  m_LastIntegratedStateReferenceTimeStamp = Reference;
226  _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
227 
228  _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
229  m_LastCustomEventReferenceTimeStamp = Reference;
230  _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
231  }
232 
234  {
235  _MINIMAL___LOCK(m_LastStartTimeStampMutex)
236  timeval TimeStampCurrentState = m_LastStartTimeStamp;
237  _MINIMAL_UNLOCK(m_LastStartTimeStampMutex)
238  return TimeStampCurrentState;
239  }
240 
242  {
243  _MINIMAL___LOCK(m_LastStopTimeStampMutex)
244  timeval TimeStampCurrentState = m_LastStopTimeStamp;
245  _MINIMAL_UNLOCK(m_LastStopTimeStampMutex)
246  return TimeStampCurrentState;
247  }
248 
250  {
251  _MINIMAL___LOCK(m_LastCycleReferenceTimeStampMutex)
252  timeval TimeStampCurrentState = m_LastCycleReferenceTimeStamp;
253  _MINIMAL_UNLOCK(m_LastCycleReferenceTimeStampMutex)
254  return TimeStampCurrentState;
255  }
256 
258  {
259  _MINIMAL___LOCK(m_LastFusedCycleReferenceTimeStampMutex)
260  timeval TimeStampCurrentState = m_LastFusedCycleReferenceTimeStamp;
261  _MINIMAL_UNLOCK(m_LastFusedCycleReferenceTimeStampMutex)
262  return TimeStampCurrentState;
263  }
264 
266  {
267  _MINIMAL___LOCK(m_LastIntegratedStateReferenceTimeStampMutex)
268  timeval TimeStampCurrentState = m_LastIntegratedStateReferenceTimeStamp;
269  _MINIMAL_UNLOCK(m_LastIntegratedStateReferenceTimeStampMutex)
270  return TimeStampCurrentState;
271  }
272 
274  {
275  _MINIMAL___LOCK(m_LastCustomEventReferenceTimeStampMutex)
276  timeval TimeStampCurrentState = m_LastCustomEventReferenceTimeStamp;
277  _MINIMAL_UNLOCK(m_LastCustomEventReferenceTimeStampMutex)
278  return TimeStampCurrentState;
279  }
280 
281  void IIMUEventDispatcher::PurgeEvents()
282  {
283  _MINIMAL___LOCK(m_EventsQueueMutex)
284 
285  if (m_EventsQueue.size() >= m_MaximalPendingEvents)
286  {
287  const uint32_t TotalEventsToRemove = (uint32_t(m_EventsQueue.size()) - m_MaximalPendingEvents) + 1;
288 
289  for (uint32_t i = 0 ; i < TotalEventsToRemove ; ++i)
290  {
291  m_EventsQueue.pop_front();
292  }
293  }
294 
295  _MINIMAL_UNLOCK(m_EventsQueueMutex)
296  }
297 
298 }
299 
IMU::IIMUEventDispatcher::SetDispatchingMode
void SetDispatchingMode(const DispatchingMode Mode)
Definition: IIMUEventDispatcher.cpp:69
IMU::CIMUEvent::EventType
EventType
Definition: IMUEvent.h:21
IMU::IIMUEventDispatcher::eDecoupled
@ eDecoupled
Definition: IIMUEventDispatcher.h:24
IMU::IIMUEventDispatcher::GetMaximalPendingEvents
uint32_t GetMaximalPendingEvents()
Definition: IIMUEventDispatcher.cpp:99
IMU::IIMUEventDispatcher::~IIMUEventDispatcher
virtual ~IIMUEventDispatcher()
Definition: IIMUEventDispatcher.cpp:46
IMU::IIMUEventDispatcher::DispatchingMode
DispatchingMode
Definition: IIMUEventDispatcher.h:22
IMU::IIMUEventDispatcher::SetMaximalPendingEvents
void SetMaximalPendingEvents(const uint32_t MaximalPendingEvents)
Definition: IIMUEventDispatcher.cpp:84
IMU::CIMUEvent::eOnIMUStart
@ eOnIMUStart
Definition: IMUEvent.h:23
IMU::IIMUEventDispatcher::SetEventHandling
void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
Definition: IIMUEventDispatcher.cpp:107
IMU::IIMUEventDispatcher::SetReferenceTimeStamps
void SetReferenceTimeStamps(const timeval &Reference)
Definition: IIMUEventDispatcher.cpp:206
IMU::IIMUEventDispatcher::GetLastStartTimeStamp
timeval GetLastStartTimeStamp()
Definition: IIMUEventDispatcher.cpp:233
IMU::IIMUEventDispatcher::SetIMU
void SetIMU(CIMUDevice *pIMUDevice)
Definition: IIMUEventDispatcher.cpp:54
IMUDevice.h
IMU::CIMUEvent
Definition: IMUEvent.h:17
IIMUEventDispatcher.h
IMU::CIMUEvent::eOnIMUCustomEvent
@ eOnIMUCustomEvent
Definition: IMUEvent.h:23
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition: IMUEvent.h:23
IMU::CIMUEvent::eOnIMUStop
@ eOnIMUStop
Definition: IMUEvent.h:23
IMU::CIMUEvent::GetEventType
EventType GetEventType() const
Definition: IMUEvent.h:39
IMU::CIMUDevice
This class contains the the devices module and the thread for read the measurements.
Definition: IMUDevice.h:41
IMU::IIMUEventDispatcher::GetLastCycleReferenceTimeStamp
timeval GetLastCycleReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:249
IMU::IIMUEventDispatcher::ProcessPendingEvent
bool ProcessPendingEvent()
Definition: IIMUEventDispatcher.cpp:188
IMU::IIMUEventDispatcher::GetDispatchingMode
DispatchingMode GetDispatchingMode()
Definition: IIMUEventDispatcher.cpp:76
IMU::IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp
timeval GetLastCustomEventReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:273
armarx::aron::similarity::FloatSimilarity::Type
Type
The Type enum.
Definition: FloatSimilarity.h:8
IMU::IIMUEventDispatcher::ReceiveEvent
void ReceiveEvent(const CIMUEvent &Event)
Definition: IIMUEventDispatcher.cpp:122
IMU::IIMUEventDispatcher::IIMUEventDispatcher
IIMUEventDispatcher()
Definition: IIMUEventDispatcher.cpp:30
IMU::CTimeStamp
Definition: IMUHelpers.h:18
IMU::IIMUEventDispatcher::OnIMUEvent
virtual void OnIMUEvent(const CIMUEvent &Event)=0
IMU
Definition: IIMUEventDispatcher.cpp:12
IMU::CIMUDevice::UnregisterEventDispatcher
bool UnregisterEventDispatcher(IIMUEventDispatcher *pIMUEventDispatcher)
Definition: IMUDevice.cpp:139
IMU::IIMUEventDispatcher::GetLastStopTimeStamp
timeval GetLastStopTimeStamp()
Definition: IIMUEventDispatcher.cpp:241
IMU::IIMUEventDispatcher::GetEventFlags
uint32_t GetEventFlags()
Definition: IIMUEventDispatcher.cpp:61
IMU::IIMUEventDispatcher::GetEventHandlingFlags
uint32_t GetEventHandlingFlags()
Definition: IIMUEventDispatcher.cpp:114
_MINIMAL_UNLOCK
#define _MINIMAL_UNLOCK(MUTEX)
Definition: IMUHelpers.h:14
IMU::CIMUEvent::eOnIMUCycle
@ eOnIMUCycle
Definition: IMUEvent.h:23
_MINIMAL___LOCK
#define _MINIMAL___LOCK(MUTEX)
Definition: IMUHelpers.h:13
IMU::IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp
timeval GetLastFusedCycleReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:257
IMU::IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp
timeval GetLastIntegratedStateReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:265
IMU::CIMUEvent::eOnIMUFusedCycle
@ eOnIMUFusedCycle
Definition: IMUEvent.h:23