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