IMUDevice.cpp
Go to the documentation of this file.
1/*
2 * IMU.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 "IMUDevice.h"
10
11#include "IIMUEventDispatcher.h"
12
13namespace IMU
14{
15 CIMUDevice::CIMUDevice() :
16 m_DeviceId(0),
17 m_SamplingFrequency(SamplingFrequency(0)),
18 m_PeriodMicroSeconds(0),
19 m_FusionStrategy(eNoFusion),
20 m_SamplesPerFusion(0),
21 m_CollectedFusionSamples(0),
22 m_IsActive(false),
23 m_IsDispatching(false),
24 m_IsInitialized(false),
25 m_pInternalThreadHandel(0),
26 m_IMUEventDispatchers(),
27 m_ReferenceTimeStamp(CTimeStamp::s_Zero),
28 m_LastFrameTimeStamp(CTimeStamp::s_Zero)
29
30#ifdef _IMU_USE_XSENS_DEVICE_
31
32 ,
33 m_pXsensMTiModule(nullptr)
34#endif
35
36 {
37 pthread_mutex_init(&m_IsActiveMutex, nullptr);
38 pthread_mutex_init(&m_IsDispatchingMutex, nullptr);
39 pthread_mutex_init(&m_EventDispatchersMutex, nullptr);
40 pthread_mutex_init(&m_DeviceMutex, nullptr);
41 }
42
43 CIMUDevice::~CIMUDevice()
44 {
45 FinalizeModuleDevice();
46 }
47
48 uint64_t
49 CIMUDevice::GetDeviceId() const
50 {
51 return m_DeviceId;
52 }
53
54 bool
55 CIMUDevice::Connect(const std::string& PortName, const SamplingFrequency Frequency)
56 {
57 if (m_IsInitialized)
58 {
59 return true;
60 }
61
62 if (!PortName.length())
63 {
64 std::cerr << "[IMU Error: Cannot connect to empty port name!]\n\t[Operation result: "
65 "(PortName.length()==0)]\n[Source location: "
66 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
67 return false;
68 }
69
70 m_IsInitialized = InitializeDevice(PortName, Frequency);
71
72 return m_IsInitialized;
73 }
74
75 bool
76 CIMUDevice::Start(const bool Blocking)
77 {
78 if (m_IsInitialized && (!m_IsActive))
79 {
80 const int Result = pthread_create(
81 &m_pInternalThreadHandel, nullptr, CIMUDevice::ThreadLoop, (void*)this);
82
83 if (Result == 0)
84 {
85 while (Blocking && !m_IsActive)
86 {
87 pthread_yield();
88 }
89
90 return true;
91 }
92 }
93
94 return false;
95 }
96
97 void
98 CIMUDevice::Stop(const bool Blocking)
99 {
100 if (m_IsActive)
101 {
102 _MINIMAL___LOCK(m_IsActiveMutex)
103 m_IsActive = false;
104 _MINIMAL_UNLOCK(m_IsActiveMutex)
105 pthread_join(m_pInternalThreadHandel, nullptr);
106
107 while (Blocking && m_IsDispatching)
108 {
109 pthread_yield();
110 }
111 }
112 }
113
114 bool
115 CIMUDevice::SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
116 {
117 if (SamplesPerFusion > 1)
118 {
119 if ((m_FusionStrategy != Strategy) || (m_SamplesPerFusion != SamplesPerFusion))
120 {
121 m_FusionStrategy = Strategy;
122 m_SamplesPerFusion = SamplesPerFusion;
123 m_CollectedFusionSamples = 0;
124 }
125
126 return true;
127 }
128 else
129 {
130 std::cerr << "[IMU Device error: Cannot set fusion with less than 2 samples per "
131 "fusion!]\n\t[Source location: "
132 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
133 return false;
134 }
135 }
136
137 bool
138 CIMUDevice::IsActive() const
139 {
140 return m_IsActive;
141 }
142
143 bool
144 CIMUDevice::RegisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
145 {
146 if (pIMUEventDispatcher)
147 {
148 _MINIMAL___LOCK(m_EventDispatchersMutex)
149
150 if (m_IMUEventDispatchers.find(pIMUEventDispatcher) == m_IMUEventDispatchers.end())
151 {
152 pIMUEventDispatcher->SetIMU(this);
153 pIMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
154 std::pair<std::set<IIMUEventDispatcher*>::iterator, bool> Result =
155 m_IMUEventDispatchers.insert(pIMUEventDispatcher);
156 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
157 return Result.second;
158 }
159
160 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
161 }
162
163 return false;
164 }
165
166 bool
167 CIMUDevice::UnregisterEventDispatcher(IIMUEventDispatcher* pIMUEventDispatcher)
168 {
169 if (pIMUEventDispatcher)
170 {
171 _MINIMAL___LOCK(m_EventDispatchersMutex)
172 std::set<IIMUEventDispatcher*>::iterator ppElement =
173 m_IMUEventDispatchers.find(pIMUEventDispatcher);
174
175 if (ppElement != m_IMUEventDispatchers.end())
176 {
177 pIMUEventDispatcher->SetIMU(nullptr);
178 m_IMUEventDispatchers.erase(ppElement);
179 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
180 return true;
181 }
182
183 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
184 }
185
186 return false;
187 }
188
189 void
190 CIMUDevice::UnregisterEventDispatchers()
191 {
192 if (m_IMUEventDispatchers.size())
193 {
194 _MINIMAL___LOCK(m_EventDispatchersMutex)
195
196 for (auto m_IMUEventDispatcher : m_IMUEventDispatchers)
197 {
198 m_IMUEventDispatcher->SetIMU(nullptr);
199 }
200
201 m_IMUEventDispatchers.clear();
202 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
203 }
204 }
205
206#ifdef _IMU_USE_XSENS_DEVICE_
207
208 bool
209 CIMUDevice::InitializeXsensDevice(const std::string& PortName,
210 const SamplingFrequency Frequency)
211 {
212 if (m_IsInitialized)
213 {
214 return true;
215 }
216
217 m_pXsensMTiModule = new Xsens::CXsensMTiModule();
218
219 if (m_pXsensMTiModule->openPort(PortName.c_str()) != MTRV_OK)
220 {
221 std::cerr << "[IMU Device error: Cannot open port!]\n\t[Operation result: "
222 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
223 << ":" << __LINE__ << "]" << std::endl;
224 DestroyXsensModuleDevice();
225 return false;
226 }
227
228 if (m_pXsensMTiModule->writeMessage(MID_GOTOCONFIG) != MTRV_OK)
229 {
230 std::cerr
231 << "[IMU Device error: Cannot set configuration state!]\n\t[Operation result: "
232 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
233 << ":" << __LINE__ << "]" << std::endl;
234 DestroyXsensModuleDevice();
235 return false;
236 }
237
238 if (m_pXsensMTiModule->setDeviceMode(OUTPUTMODE_CALIB | OUTPUTMODE_ORIENT,
239 OUTPUTSETTINGS_ORIENTMODE_QUATERNION |
240 OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT) != MTRV_OK)
241 {
242 std::cerr << "[IMU Device error: Cannot set output mode!]\n\t[Operation result: "
243 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
244 << ":" << __LINE__ << "]" << std::endl;
245 DestroyXsensModuleDevice();
246 return false;
247 }
248
249 if (m_pXsensMTiModule->setSetting(MID_SETPERIOD, Frequency, LEN_PERIOD) != MTRV_OK)
250 {
251 std::cerr << "[IMU Device error: Cannot set sampling period!]\n\t[Operation result: "
252 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
253 << ":" << __LINE__ << "]" << std::endl;
254 DestroyXsensModuleDevice();
255 return false;
256 }
257
258 unsigned long DeviceId;
259
260 if (m_pXsensMTiModule->reqSetting(MID_REQDID, DeviceId) != MTRV_OK)
261 {
262 std::cerr << "[IMU Device error: Cannot get device ID!]\n\t[Operation result: "
263 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
264 << ":" << __LINE__ << "]" << std::endl;
265 DestroyXsensModuleDevice();
266 return false;
267 }
268
269 m_DeviceId = DeviceId;
270
271 if (m_pXsensMTiModule->writeMessage(MID_GOTOMEASUREMENT) != MTRV_OK)
272 {
273 std::cerr
274 << "[IMU Device error: Cannot enter measurement state!]\n\t[Operation result: "
275 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
276 << ":" << __LINE__ << "]" << std::endl;
277 DestroyXsensModuleDevice();
278 return false;
279 }
280
281 return true;
282 }
283
284 void
285 CIMUDevice::FinalizeXsensModuleDevice()
286 {
287 if (m_IsInitialized)
288 {
289 while (m_IsActive || m_IsDispatching)
290 {
291 pthread_yield();
292 }
293
294 _MINIMAL___LOCK(m_DeviceMutex)
295 DestroyXsensModuleDevice();
296 _MINIMAL_UNLOCK(m_DeviceMutex)
297 }
298 }
299
300 void
301 CIMUDevice::DestroyXsensModuleDevice()
302 {
303 if (m_pXsensMTiModule)
304 {
305 if (m_pXsensMTiModule->isPortOpen())
306 {
307 m_pXsensMTiModule->close();
308 }
309
310 delete m_pXsensMTiModule;
311 m_pXsensMTiModule = nullptr;
312 m_DeviceId = 0;
313 }
314 }
315
316#endif
317
318 bool
319 CIMUDevice::LoadCurrentState()
320 {
321
322 _MINIMAL___LOCK(m_DeviceMutex)
323
324#ifdef _IMU_USE_XSENS_DEVICE_
325
326 if (m_pXsensMTiModule->readDataMessage(m_XsensMTiFrame.m_Data,
327 m_XsensMTiFrame.m_DataLength) == MTRV_OK)
328 if (m_pXsensMTiModule->getValue(
329 VALUE_CALIB_ACC,
330 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration,
331 m_XsensMTiFrame.m_Data) == MTRV_OK)
332 if (m_pXsensMTiModule->getValue(
333 VALUE_CALIB_GYR,
334 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_GyroscopeRotation,
335 m_XsensMTiFrame.m_Data) == MTRV_OK)
336 if (m_pXsensMTiModule->getValue(
337 VALUE_CALIB_MAG,
338 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_MagneticRotation,
339 m_XsensMTiFrame.m_Data) == MTRV_OK)
340 if (m_pXsensMTiModule->getValue(
341 VALUE_ORIENT_QUAT,
342 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_QuaternionRotation,
343 m_XsensMTiFrame.m_Data) == MTRV_OK)
344 if (m_pXsensMTiModule->getValue(
345 VALUE_SAMPLECNT,
346 m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount,
347 m_XsensMTiFrame.m_Data) == MTRV_OK)
348 {
349 if (m_XsensMTiFrame.m_IMUState.m_ControlData
350 .m_PreviousSampleCount != -1)
351 {
352 m_XsensMTiFrame.m_IMUState.m_ControlData.m_IsConsecutive =
353 ((m_XsensMTiFrame.m_IMUState.m_ControlData
354 .m_PreviousSampleCount +
355 1) %
356 65536) == m_XsensMTiFrame.m_IMUState.m_ControlData
357 .m_CurrentSampleCount;
358 }
359
360 m_XsensMTiFrame.m_IMUState.m_ControlData.m_PreviousSampleCount =
361 m_XsensMTiFrame.m_IMUState.m_ControlData.m_CurrentSampleCount;
362 m_XsensMTiFrame.m_IMUState.m_ControlData.m_MessageCounter++;
363 gettimeofday(&m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp,
364 nullptr);
365 m_LastFrameTimeStamp =
366 m_XsensMTiFrame.m_IMUState.m_ControlData.m_TimeStamp;
367
368 m_XsensMTiFrame.m_IMUState.m_PhysicalData
369 .UpdateAccelerationMagnitud();
370
371 _MINIMAL_UNLOCK(m_DeviceMutex)
372 return true;
373 }
374 else
375 {
376 std::cerr << "[IMU Device error: Fail to get sample "
377 "count!]\n\t[Operation result: "
378 << m_pXsensMTiModule->getLastRetVal()
379 << "]\n\t[Source location: " << __FILE__ << ":"
380 << __LINE__ << "]" << std::endl;
381 }
382 else
383 {
384 std::cerr << "[IMU Device error: Fail to get quaternion "
385 "rotation!]\n\t[Operation result: "
386 << m_pXsensMTiModule->getLastRetVal()
387 << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__
388 << "]" << std::endl;
389 }
390 else
391 {
392 std::cerr << "[IMU Device error: Fail to get magnetic "
393 "rotation!]\n\t[Operation result: "
394 << m_pXsensMTiModule->getLastRetVal()
395 << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]"
396 << std::endl;
397 }
398 else
399 {
400 std::cerr << "[IMU Device error: Fail to get gyroscope "
401 "rotation!]\n\t[Operation result: "
402 << m_pXsensMTiModule->getLastRetVal()
403 << "]\n\t[Source location: " << __FILE__ << ":" << __LINE__ << "]"
404 << std::endl;
405 }
406 else
407 {
408 std::cerr
409 << "[IMU Device error: Fail to get acceleration vector!]\n\t[Operation result: "
410 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
411 << ":" << __LINE__ << "]" << std::endl;
412 }
413 else
414 {
415 std::cerr << "[IMU Device error: Fail to read message!]\n\t[Operation result: "
416 << m_pXsensMTiModule->getLastRetVal() << "]\n\t[Source location: " << __FILE__
417 << ":" << __LINE__ << "]" << std::endl;
418 }
419
420#endif
421
422 _MINIMAL_UNLOCK(m_DeviceMutex)
423
424 return false;
425 }
426
427 bool
428 CIMUDevice::MeanFuseCurrentState()
429 {
430 IncorporateCurrentStateMeanFusion();
431
432 if (m_CollectedFusionSamples == m_SamplesPerFusion)
433 {
434 MeanFusion();
435 return true;
436 }
437
438 return false;
439 }
440
441 void
442 CIMUDevice::IncorporateCurrentStateMeanFusion()
443 {
444 m_FusedPhysicalData.m_Acceleration[0] +=
445 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
446 m_FusedPhysicalData.m_Acceleration[1] +=
447 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
448 m_FusedPhysicalData.m_Acceleration[2] +=
449 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
450 ++m_CollectedFusionSamples;
451 }
452
453 void
454 CIMUDevice::MeanFusion()
455 {
456 //Execution the fusion
457 const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
458 m_FusedIMUState.m_PhysicalData.m_Acceleration[0] =
459 m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
460 m_FusedIMUState.m_PhysicalData.m_Acceleration[1] =
461 m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
462 m_FusedIMUState.m_PhysicalData.m_Acceleration[2] =
463 m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
464
465 //Derivated from fusion
466 m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
467
468 //Reset counters and accumulators
469 memset(m_FusedPhysicalData.m_Acceleration, 0, sizeof(float) * 3);
470 m_CollectedFusionSamples = 0;
471 }
472
473 bool
474 CIMUDevice::GaussianFuseCurrentState()
475 {
476 IncorporateCurrentStateGaussianFusion();
477
478 if (m_CollectedFusionSamples == m_SamplesPerFusion)
479 {
480 GaussianFusion();
481 return true;
482 }
483
484 return false;
485 }
486
487 void
488 CIMUDevice::IncorporateCurrentStateGaussianFusion()
489 {
490 m_FusedPhysicalData.m_Acceleration[0] +=
491 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[0];
492 m_FusedPhysicalData.m_Acceleration[1] +=
493 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[1];
494 m_FusedPhysicalData.m_Acceleration[2] +=
495 m_XsensMTiFrame.m_IMUState.m_PhysicalData.m_Acceleration[2];
496
497 ++m_CollectedFusionSamples;
498 }
499
500 void
501 CIMUDevice::GaussianFusion()
502 {
503 //Execution the fusion
504 const float NormalizationFactor = 1.0f / float(m_CollectedFusionSamples);
505 m_FusedIMUState.m_PhysicalData.m_Acceleration[0] =
506 m_FusedPhysicalData.m_Acceleration[0] * NormalizationFactor;
507 m_FusedIMUState.m_PhysicalData.m_Acceleration[1] =
508 m_FusedPhysicalData.m_Acceleration[1] * NormalizationFactor;
509 m_FusedIMUState.m_PhysicalData.m_Acceleration[2] =
510 m_FusedPhysicalData.m_Acceleration[2] * NormalizationFactor;
511
512 //Derivated from fusion
513 m_FusedIMUState.m_PhysicalData.UpdateAccelerationMagnitud();
514
515 //Reset counters and accumulators
516 memset(m_FusedPhysicalData.m_Acceleration, 0, sizeof(float) * 3);
517 m_CollectedFusionSamples = 0;
518 }
519
520 bool
521 CIMUDevice::IntegrateCurrentState()
522 {
523 if (m_FusionStrategy == eNoFusion)
524 {
525 return IntegrateWithOutFusion();
526 }
527 else
528 {
529 return IntegrateWithFusion();
530 }
531 }
532
533 bool
534 CIMUDevice::IntegrateWithOutFusion()
535 {
536 return true;
537 }
538
539 bool
540 CIMUDevice::IntegrateWithFusion()
541 {
542 return true;
543 }
544
545 bool
546 CIMUDevice::InitializeDevice(const std::string& PortName, const SamplingFrequency Frequency)
547 {
548
549 _MINIMAL___LOCK(m_DeviceMutex)
550
551#ifdef _IMU_USE_XSENS_DEVICE_
552
553 if (InitializeXsensDevice(PortName, Frequency))
554 {
555 m_SamplingFrequency = Frequency;
556 const int Cylces = 0X1C200 / int(m_SamplingFrequency);
557 m_PeriodMicroSeconds =
558 int(round((1000000.0f * _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_) / float(Cylces)));
559 _MINIMAL_UNLOCK(m_DeviceMutex)
560 return true;
561 }
562 else
563 {
564 _MINIMAL_UNLOCK(m_DeviceMutex)
565 return false;
566 }
567
568#endif
569 }
570
571 void
572 CIMUDevice::FinalizeModuleDevice()
573 {
574 Stop(true);
575
576#ifdef _IMU_USE_XSENS_DEVICE_
577
578 FinalizeXsensModuleDevice();
579
580#endif
581
582 UnregisterEventDispatchers();
583 }
584
585 void
586 CIMUDevice::ShouldYield()
587 {
588 const long int RemainingTime =
589 m_PeriodMicroSeconds - CTimeStamp::GetElapsedMicroseconds(m_LastFrameTimeStamp);
590
591 if (RemainingTime > 0)
592 {
593 usleep(__useconds_t(RemainingTime));
594 }
595 }
596
597 bool
598 CIMUDevice::DispatchCylcle()
599 {
600 if (LoadCurrentState())
601 {
602 SendEvent(CIMUEvent(m_LastFrameTimeStamp, CIMUEvent::eOnIMUCycle, this));
603
604 switch (m_FusionStrategy)
605 {
606 case eMeanFusion:
607 if (MeanFuseCurrentState())
608 {
609 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
610 CIMUEvent::eOnIMUFusedCycle,
611 this,
612 m_FusedIMUState));
613
614 if (IntegrateCurrentState())
615 {
616 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
617 CIMUEvent::eOnIMUIntegratedState,
618 this,
619 m_IntegratedIMUState));
620 }
621 }
622
623 break;
624
625 case eGaussianFusion:
626 if (GaussianFuseCurrentState())
627 {
628 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
629 CIMUEvent::eOnIMUFusedCycle,
630 this,
631 m_FusedIMUState));
632
633 if (IntegrateCurrentState())
634 {
635 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
636 CIMUEvent::eOnIMUIntegratedState,
637 this,
638 m_IntegratedIMUState));
639 }
640 }
641
642 break;
643
644 case eNoFusion:
645
646
647 if (IntegrateCurrentState())
648 {
649 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
650 CIMUEvent::eOnIMUFusedCycle,
651 this,
652 m_FusedIMUState));
653 SendEvent(CIMUEvent(m_LastFrameTimeStamp,
654 CIMUEvent::eOnIMUIntegratedState,
655 this,
656 m_IntegratedIMUState));
657 }
658
659 break;
660 }
661
662 return true;
663 }
664
665 return false;
666 }
667
668 void
669 CIMUDevice::SendEvent(const CIMUEvent& Event)
670 {
671 _MINIMAL___LOCK(m_EventDispatchersMutex)
672 const unsigned long int TotalDispatchers = m_IMUEventDispatchers.size();
673
674 if (TotalDispatchers)
675 {
676 if (TotalDispatchers == 1)
677 {
678 (*m_IMUEventDispatchers.begin())->ReceiveEvent(Event);
679 }
680 else
681 for (auto m_IMUEventDispatcher : m_IMUEventDispatchers)
682 {
683 m_IMUEventDispatcher->ReceiveEvent(Event);
684 }
685 }
686
687 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
688 }
689
690 void
691 CIMUDevice::SetReferenceTimeStamps()
692 {
693 _MINIMAL___LOCK(m_EventDispatchersMutex)
694 gettimeofday(&m_ReferenceTimeStamp, nullptr);
695 gettimeofday(&m_LastFrameTimeStamp, nullptr);
696
697 if (m_IMUEventDispatchers.size())
698 for (auto m_IMUEventDispatcher : m_IMUEventDispatchers)
699 {
700 m_IMUEventDispatcher->SetReferenceTimeStamps(m_ReferenceTimeStamp);
701 }
702
703 _MINIMAL_UNLOCK(m_EventDispatchersMutex)
704 }
705
706 bool
707 CIMUDevice::SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy,
708 const float NormalizedPriority)
709 {
710 if (m_IsActive)
711 {
712 int Policy = -1;
713 struct sched_param SchedulingParameters;
714
715 if (pthread_getschedparam(m_pInternalThreadHandel, &Policy, &SchedulingParameters) == 0)
716 {
717 const int MaximalPriority = sched_get_priority_max(ThreadPolicy);
718 const int MinimalPriority = sched_get_priority_min(ThreadPolicy);
719 const int PriorityRange = MaximalPriority - MinimalPriority;
720 const int Priority =
721 int(round(float(PriorityRange) * NormalizedPriority + float(MinimalPriority)));
722 SchedulingParameters.sched_priority = Priority;
723 const int Result = pthread_setschedparam(
724 m_pInternalThreadHandel, ThreadPolicy, &SchedulingParameters);
725
726 switch (Result)
727 {
728 case 0:
729 return true;
730 break;
731
732 case EINVAL:
733 std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
734 "EINVAL!]\n[Source location: "
735 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
736 break;
737
738 case ENOTSUP:
739 std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
740 "ENOTSUP!]\n[Source location: "
741 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
742 break;
743
744 case EPERM:
745 std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
746 "EPERM!]\n[Source location: "
747 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
748 break;
749
750 case ESRCH:
751 std::cerr << "[IMU Device error: SetThreadRunnigMode() returns "
752 "ESRCH!]\n[Source location: "
753 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
754 break;
755 }
756 }
757
758 return false;
759 }
760 else
761 {
762 std::cerr << "[IMU Device error: SetThreadRunnigMode() cannot set running mode while "
763 "thread is not active!]\n[Source location: "
764 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
765 return false;
766 }
767 }
768
769 void*
770 CIMUDevice::ThreadLoop(void* pData)
771 {
772 if (pData)
773 {
774
775 CIMUDevice* pIMUDevice = (CIMUDevice*)pData;
776
777 _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
778 pIMUDevice->m_IsActive = true;
779 _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
780
781 pIMUDevice->SetReferenceTimeStamps();
782
783 pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStart, pIMUDevice));
784
785 while (pIMUDevice->m_IsActive)
786 {
787 pIMUDevice->ShouldYield();
788
789 _MINIMAL___LOCK(pIMUDevice->m_IsDispatchingMutex)
790 pIMUDevice->m_IsDispatching = true;
791
792 const bool DispatchingResult = pIMUDevice->DispatchCylcle();
793
794 pIMUDevice->m_IsDispatching = false;
795 _MINIMAL_UNLOCK(pIMUDevice->m_IsDispatchingMutex)
796
797 if (!DispatchingResult)
798 {
799 std::cerr
800 << "[IMU Device error: DispatchCylcle() returns false!]\n[Source location: "
801 << __FILE__ << ":" << __LINE__ << "]" << std::endl;
802 break;
803 }
804 }
805
806 if (pIMUDevice->m_IsActive)
807 {
808 _MINIMAL___LOCK(pIMUDevice->m_IsActiveMutex)
809 pIMUDevice->m_IsActive = false;
810 _MINIMAL_UNLOCK(pIMUDevice->m_IsActiveMutex)
811 }
812
813 pIMUDevice->SendEvent(CIMUEvent(CIMUEvent::eOnIMUStop, pIMUDevice));
814 }
815
816 return nullptr;
817 }
818} // namespace IMU
float
#define float
Definition 16_Level.h:22
if
if(!yyvaluep)
Definition Grammar.cpp:645
_IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_
#define _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_
Definition IMUDevice.h:25
_MINIMAL_UNLOCK
#define _MINIMAL_UNLOCK(MUTEX)
Definition IMUHelpers.h:14
_MINIMAL___LOCK
#define _MINIMAL___LOCK(MUTEX)
Definition IMUHelpers.h:13
_IMU_USE_XSENS_DEVICE_
#define _IMU_USE_XSENS_DEVICE_
Definition Includes.h:54
while
while(1)
Definition Scanner.cpp:721
VALUE_ORIENT_QUAT
#define VALUE_ORIENT_QUAT
Definition XsensMTiModule.h:501
MID_REQDID
#define MID_REQDID
Definition XsensMTiModule.h:184
LEN_PERIOD
#define LEN_PERIOD
Definition XsensMTiModule.h:192
MTRV_OK
#define MTRV_OK
Definition XsensMTiModule.h:906
OUTPUTSETTINGS_ORIENTMODE_QUATERNION
#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION
Definition XsensMTiModule.h:807
OUTPUTMODE_CALIB
#define OUTPUTMODE_CALIB
Definition XsensMTiModule.h:800
OUTPUTMODE_ORIENT
#define OUTPUTMODE_ORIENT
Definition XsensMTiModule.h:801
VALUE_CALIB_GYR
#define VALUE_CALIB_GYR
Definition XsensMTiModule.h:499
VALUE_CALIB_ACC
#define VALUE_CALIB_ACC
Definition XsensMTiModule.h:498
OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition XsensMTiModule.h:806
MID_GOTOMEASUREMENT
#define MID_GOTOMEASUREMENT
Definition XsensMTiModule.h:212
MID_SETPERIOD
#define MID_SETPERIOD
Definition XsensMTiModule.h:193
VALUE_CALIB_MAG
#define VALUE_CALIB_MAG
Definition XsensMTiModule.h:500
VALUE_SAMPLECNT
#define VALUE_SAMPLECNT
Definition XsensMTiModule.h:504
MID_GOTOCONFIG
#define MID_GOTOCONFIG
Definition XsensMTiModule.h:335
IMU::CIMUDevice
This class contains the the devices module and the thread for read the measurements.
Definition IMUDevice.h:42
IMU::CIMUDevice::LoadCurrentState
bool LoadCurrentState()
Definition IMUDevice.cpp:319
IMU::CIMUDevice::Start
bool Start(const bool Blocking=true)
Definition IMUDevice.cpp:76
IMU::CIMUDevice::IntegrateWithFusion
bool IntegrateWithFusion()
Definition IMUDevice.cpp:540
IMU::CIMUDevice::SetFusion
bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
Definition IMUDevice.cpp:115
IMU::CIMUDevice::SetThreadRunnigMode
bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
Definition IMUDevice.cpp:707
IMU::CIMUDevice::MeanFuseCurrentState
bool MeanFuseCurrentState()
Definition IMUDevice.cpp:428
IMU::CIMUDevice::~CIMUDevice
virtual ~CIMUDevice()
The destructor.
Definition IMUDevice.cpp:43
IMU::CIMUDevice::IncorporateCurrentStateGaussianFusion
void IncorporateCurrentStateGaussianFusion()
Definition IMUDevice.cpp:488
IMU::CIMUDevice::CIMUDevice
CIMUDevice()
The default constructor. The default constructor sets all member variables to zero,...
Definition IMUDevice.cpp:15
IMU::CIMUDevice::SamplingFrequency
SamplingFrequency
Enum specifying the supported sampling frequencies.
Definition IMUDevice.h:59
IMU::CIMUDevice::Connect
bool Connect(const std::string &PortName, const SamplingFrequency Frequency)
Definition IMUDevice.cpp:55
IMU::CIMUDevice::Stop
void Stop(const bool Blocking=true)
Definition IMUDevice.cpp:98
IMU::CIMUDevice::IntegrateWithOutFusion
bool IntegrateWithOutFusion()
Definition IMUDevice.cpp:534
IMU::CIMUDevice::GaussianFusion
void GaussianFusion()
Definition IMUDevice.cpp:501
IMU::CIMUDevice::IsActive
bool IsActive() const
Definition IMUDevice.cpp:138
IMU::CIMUDevice::GetDeviceId
uint64_t GetDeviceId() const
Definition IMUDevice.cpp:49
IMU::CIMUDevice::UnregisterEventDispatcher
bool UnregisterEventDispatcher(IIMUEventDispatcher *pIMUEventDispatcher)
Definition IMUDevice.cpp:167
IMU::CIMUDevice::IntegrateCurrentState
bool IntegrateCurrentState()
Definition IMUDevice.cpp:521
IMU::CIMUDevice::GaussianFuseCurrentState
bool GaussianFuseCurrentState()
Definition IMUDevice.cpp:474
IMU::CIMUDevice::ThreadPolicyType
ThreadPolicyType
Enum specifying the running thread policy.
Definition IMUDevice.h:48
IMU::CIMUDevice::IncorporateCurrentStateMeanFusion
void IncorporateCurrentStateMeanFusion()
Definition IMUDevice.cpp:442
IMU::CIMUDevice::RegisterEventDispatcher
bool RegisterEventDispatcher(IIMUEventDispatcher *pIMUEventDispatcher)
Definition IMUDevice.cpp:144
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition IMUEvent.h:27
IMU::CTimeStamp::GetElapsedMicroseconds
static long GetElapsedMicroseconds(const timeval &Post, const timeval &Pre)
Definition IMUHelpers.h:42
IMU::IIMUEventDispatcher::SetReferenceTimeStamps
void SetReferenceTimeStamps(const timeval &Reference)
Definition IIMUEventDispatcher.cpp:237
IMU::IIMUEventDispatcher::SetIMU
void SetIMU(CIMUDevice *pIMUDevice)
Definition IIMUEventDispatcher.cpp:75