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 
13 namespace 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
IMU::CIMUDevice::CIMUDevice
CIMUDevice()
The default constructor. The default constructor sets all member variables to zero,...
Definition: IMUDevice.cpp:15
LEN_PERIOD
#define LEN_PERIOD
Definition: XsensMTiModule.h:192
IMU::IMUState::PhysicalData::m_MagneticRotation
float m_MagneticRotation[3]
Definition: IMUState.h:52
IMU::CIMUDevice::IntegrateCurrentState
bool IntegrateCurrentState()
Definition: IMUDevice.cpp:521
MID_GOTOCONFIG
#define MID_GOTOCONFIG
Definition: XsensMTiModule.h:335
IMU::CIMUDevice::SetThreadRunnigMode
bool SetThreadRunnigMode(const ThreadPolicyType ThreadPolicy, const float NormalizedPriority)
Definition: IMUDevice.cpp:707
IMU::IMUState::ControlData::m_TimeStamp
timeval m_TimeStamp
Definition: IMUState.h:36
OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
#define OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT
Definition: XsensMTiModule.h:806
IMU::Xsens::CXsensMTiModule::openPort
short openPort(const int portNumber, const unsigned long baudrate=PBR_115K2, const unsigned long inqueueSize=4096, const unsigned long outqueueSize=1024)
Definition: XsensMTiModule.cpp:199
IMU::Xsens::CXsensMTiModule::reqSetting
short reqSetting(const unsigned char mid, unsigned long &value, const unsigned char bid=BID_MASTER)
Definition: XsensMTiModule.cpp:1524
IMU::IMUState::m_PhysicalData
PhysicalData m_PhysicalData
Definition: IMUState.h:65
MID_GOTOMEASUREMENT
#define MID_GOTOMEASUREMENT
Definition: XsensMTiModule.h:212
IMU::CIMUDevice::ThreadPolicyType
ThreadPolicyType
Enum specifying the running thread policy.
Definition: IMUDevice.h:47
IMU::CIMUDevice::MeanFuseCurrentState
bool MeanFuseCurrentState()
Definition: IMUDevice.cpp:428
OUTPUTSETTINGS_ORIENTMODE_QUATERNION
#define OUTPUTSETTINGS_ORIENTMODE_QUATERNION
Definition: XsensMTiModule.h:807
IMU::CIMUDevice::FusionStrategy
FusionStrategy
Definition: IMUDevice.h:105
IMU::CIMUDevice::RegisterEventDispatcher
bool RegisterEventDispatcher(IIMUEventDispatcher *pIMUEventDispatcher)
Definition: IMUDevice.cpp:144
IMU::IMUState::ControlData::m_PreviousSampleCount
long m_PreviousSampleCount
Definition: IMUState.h:32
VALUE_ORIENT_QUAT
#define VALUE_ORIENT_QUAT
Definition: XsensMTiModule.h:501
IMU::CIMUDevice::GaussianFuseCurrentState
bool GaussianFuseCurrentState()
Definition: IMUDevice.cpp:474
return
return() armarx_set_target("PointCloudViewerGuiPlugin") find_package(MPI REQUIRED) find_package(VTK REQUIRED) find_package(Coin QUIET) armarx_build_if(Coin_FOUND "Coin 3D not found") set(COMPONENT_LIBS VisionXInterfaces VisionXCore VisionXPointCloud VTK
Definition: CMakeLists.txt:1
IMU::IIMUEventDispatcher::SetReferenceTimeStamps
void SetReferenceTimeStamps(const timeval &Reference)
Definition: IIMUEventDispatcher.cpp:237
IMU::IIMUEventDispatcher::SetIMU
void SetIMU(CIMUDevice *pIMUDevice)
Definition: IIMUEventDispatcher.cpp:75
IMU::CIMUDevice::IncorporateCurrentStateMeanFusion
void IncorporateCurrentStateMeanFusion()
Definition: IMUDevice.cpp:442
IMUDevice.h
IMU::CIMUEvent
Definition: IMUEvent.h:18
IMU::Xsens::CXsensMTiModule::isPortOpen
bool isPortOpen()
Definition: XsensMTiModule.cpp:554
IIMUEventDispatcher.h
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition: IMUEvent.h:27
IMU::CIMUDevice::IntegrateWithOutFusion
bool IntegrateWithOutFusion()
Definition: IMUDevice.cpp:534
IMU::IMUState::ControlData::m_MessageCounter
long m_MessageCounter
Definition: IMUState.h:34
IMU::IIMUEventDispatcher
Definition: IIMUEventDispatcher.h:19
if
if(!yyvaluep)
Definition: Grammar.cpp:645
IMU::CIMUDevice
This class contains the the devices module and the thread for read the measurements.
Definition: IMUDevice.h:41
MTRV_OK
#define MTRV_OK
Definition: XsensMTiModule.h:906
IMU::Xsens::CXsensMTiModule::readDataMessage
short readDataMessage(unsigned char data[], short &dataLen)
Definition: XsensMTiModule.cpp:946
IMU::Xsens::CXsensMTiModule::setDeviceMode
short setDeviceMode(unsigned long OutputMode, unsigned long OutputSettings, const unsigned char bid=BID_MASTER)
Definition: XsensMTiModule.cpp:2630
VALUE_SAMPLECNT
#define VALUE_SAMPLECNT
Definition: XsensMTiModule.h:504
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
OUTPUTMODE_CALIB
#define OUTPUTMODE_CALIB
Definition: XsensMTiModule.h:800
IMU::Xsens::CXsensMTiModule::getValue
short getValue(const unsigned long valueSpec, unsigned short &value, const unsigned char data[], const unsigned char bid=BID_MT)
Definition: XsensMTiModule.cpp:2859
IMU::Xsens::XsensMTiFrame::m_IMUState
IMUState m_IMUState
Definition: Xsens.h:28
_IMU_USE_XSENS_DEVICE_
#define _IMU_USE_XSENS_DEVICE_
Definition: Includes.h:54
IMU::CIMUDevice::IncorporateCurrentStateGaussianFusion
void IncorporateCurrentStateGaussianFusion()
Definition: IMUDevice.cpp:488
IMU::CTimeStamp
Definition: IMUHelpers.h:18
IMU::CIMUDevice::eMeanFusion
@ eMeanFusion
Definition: IMUDevice.h:108
IMU::CIMUDevice::GetDeviceId
uint64_t GetDeviceId() const
Definition: IMUDevice.cpp:49
IMU
Definition: IIMUEventDispatcher.cpp:13
IMU::CIMUDevice::eNoFusion
@ eNoFusion
Definition: IMUDevice.h:107
IMU::CIMUDevice::MeanFusion
void MeanFusion()
Definition: IMUDevice.cpp:454
IMU::CIMUDevice::~CIMUDevice
virtual ~CIMUDevice()
The destructor.
Definition: IMUDevice.cpp:43
IMU::CIMUDevice::UnregisterEventDispatcher
bool UnregisterEventDispatcher(IIMUEventDispatcher *pIMUEventDispatcher)
Definition: IMUDevice.cpp:167
IMU::Xsens::CXsensMTiModule::getLastRetVal
short getLastRetVal()
Definition: XsensMTiModule.cpp:3232
IMU::CIMUDevice::SamplingFrequency
SamplingFrequency
Enum specifying the supported sampling frequencies.
Definition: IMUDevice.h:58
IMU::IMUState::PhysicalData::m_QuaternionRotation
float m_QuaternionRotation[4]
Definition: IMUState.h:53
IMU::Xsens::CXsensMTiModule::setSetting
short setSetting(const unsigned char mid, const unsigned long value, const unsigned short valuelen, const unsigned char bid=BID_MASTER)
Definition: XsensMTiModule.cpp:2120
IMU::CIMUDevice::LoadCurrentState
bool LoadCurrentState()
Definition: IMUDevice.cpp:319
IMU::CIMUDevice::IntegrateWithFusion
bool IntegrateWithFusion()
Definition: IMUDevice.cpp:540
VALUE_CALIB_MAG
#define VALUE_CALIB_MAG
Definition: XsensMTiModule.h:500
float
#define float
Definition: 16_Level.h:22
IMU::IMUState::PhysicalData::m_GyroscopeRotation
float m_GyroscopeRotation[3]
Definition: IMUState.h:51
VALUE_CALIB_GYR
#define VALUE_CALIB_GYR
Definition: XsensMTiModule.h:499
IMU::Xsens::CXsensMTiModule::close
short close()
Definition: XsensMTiModule.cpp:840
IMU::IMUState::PhysicalData::m_Acceleration
float m_Acceleration[3]
Definition: IMUState.h:49
IMU::IMUState::ControlData::m_IsConsecutive
bool m_IsConsecutive
Definition: IMUState.h:35
_MINIMAL_UNLOCK
#define _MINIMAL_UNLOCK(MUTEX)
Definition: IMUHelpers.h:14
OUTPUTMODE_ORIENT
#define OUTPUTMODE_ORIENT
Definition: XsensMTiModule.h:801
IMU::CIMUDevice::eGaussianFusion
@ eGaussianFusion
Definition: IMUDevice.h:109
IMU::IMUState::PhysicalData::UpdateAccelerationMagnitud
void UpdateAccelerationMagnitud()
Definition: IMUState.h:56
IMU::CIMUEvent::eOnIMUCycle
@ eOnIMUCycle
Definition: IMUEvent.h:25
IMU::CTimeStamp::GetElapsedMicroseconds
static long GetElapsedMicroseconds(const timeval &Post, const timeval &Pre)
Definition: IMUHelpers.h:42
MID_SETPERIOD
#define MID_SETPERIOD
Definition: XsensMTiModule.h:193
IMU::IMUState::m_ControlData
ControlData m_ControlData
Definition: IMUState.h:64
IMU::IMUState::ControlData::m_CurrentSampleCount
unsigned short m_CurrentSampleCount
Definition: IMUState.h:33
IMU::Xsens::XsensMTiFrame::m_DataLength
short m_DataLength
Definition: Xsens.h:26
IMU::CIMUDevice::Start
bool Start(const bool Blocking=true)
Definition: IMUDevice.cpp:76
while
while(1)
Definition: Scanner.cpp:721
IMU::CIMUDevice::SetFusion
bool SetFusion(const FusionStrategy Strategy, const ushort SamplesPerFusion)
Definition: IMUDevice.cpp:115
_MINIMAL___LOCK
#define _MINIMAL___LOCK(MUTEX)
Definition: IMUHelpers.h:13
endif
endif() find_package(Simox QUIET) armarx_build_if(Simox_FOUND "Simox not available") find_package(PythonLibs 3.6 QUIET) armarx_build_if(PYTHONLIBS_FOUND "Python libs not available") if(PYTHONLIBS_FOUND) include_directories($
Definition: CMakeLists.txt:10
IMU::Xsens::CXsensMTiModule::writeMessage
short writeMessage(const unsigned char mid, const unsigned long dataValue=0, const unsigned char dataValueLen=0, const unsigned char bid=BID_MASTER)
Definition: XsensMTiModule.cpp:1229
IMU::CIMUDevice::GaussianFusion
void GaussianFusion()
Definition: IMUDevice.cpp:501
IMU::Xsens::XsensMTiFrame::m_Data
unsigned char m_Data[MAXMSGLEN]
Definition: Xsens.h:27
VALUE_CALIB_ACC
#define VALUE_CALIB_ACC
Definition: XsensMTiModule.h:498
_IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_
#define _IMU_DEVICE_DEFAUL_CHECK_PERIOD_FACTOR_
Definition: IMUDevice.h:25
MID_REQDID
#define MID_REQDID
Definition: XsensMTiModule.h:184
IMU::CIMUDevice::IsActive
bool IsActive() const
Definition: IMUDevice.cpp:138
IMU::CIMUEvent::eOnIMUFusedCycle
@ eOnIMUFusedCycle
Definition: IMUEvent.h:26