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