IMUDeducedReckoning.cpp
Go to the documentation of this file.
1 /*
2  * IMUDeducedReckoning.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 "IMUDeducedReckoning.h"
10 
11 namespace IMU
12 {
13  CIMUDeducedReckoning::CIMUDeducedReckoning(const bool IsVerbose) :
14  IIMUEventDispatcher(), m_IsVerbose(IsVerbose), m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
15  {
16  SetEventHandling(CIMUEvent::eOnIMUStart, true);
17  SetEventHandling(CIMUEvent::eOnIMUStop, true);
18  SetEventHandling(CIMUEvent::eOnIMUCycle, false);
19  SetEventHandling(CIMUEvent::eOnIMUFusedCycle, true);
20  SetEventHandling(CIMUEvent::eOnIMUIntegratedState, false);
21  SetEventHandling(CIMUEvent::eOnIMUCustomEvent, true);
22  }
23 
24  CIMUDeducedReckoning::~CIMUDeducedReckoning()
25  = default;
26 
27  void CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
28  {
29  switch (Event.GetEventType())
30  {
31  case CIMUEvent::eOnIMUCycle:
32  OnIMUCycle(Event.GetTimeStamp(), Event.GetIMU());
33  break;
34 
35  case CIMUEvent::eOnIMUFusedCycle:
36  OnIMUFusedCycle(Event.GetTimeStamp(), Event.GetIMU());
37  break;
38 
39  case CIMUEvent::eOnIMUIntegratedState:
40  OnIMUIntegratedState(Event.GetTimeStamp(), Event.GetIMU());
41  break;
42 
43  case CIMUEvent::eOnIMUCustomEvent:
44  OnIMUCustomEvent(Event);
45  break;
46 
47  case CIMUEvent::eOnIMUStart:
48  OnIMUStart(Event.GetTimeStamp(), Event.GetIMU());
49  break;
50 
51  case CIMUEvent::eOnIMUStop:
52  OnIMUStop(Event.GetTimeStamp(), Event.GetIMU());
53  break;
54  }
55  }
56 
57  void CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
58  {
59  if (m_IsVerbose)
60  {
61  std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
62  std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
63  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStartTimeStamp()) << "]" << std::endl;
64 
65  if (GetDispatchingMode() == eDecoupled)
66  {
67  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
68  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
69  }
70  }
71  }
72 
73  void CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
74  {
75  if (m_IsVerbose)
76  {
77  std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")" << std::endl;
78  std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
79  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStopTimeStamp()) << "]" << std::endl;
80 
81  if (GetDispatchingMode() == eDecoupled)
82  {
83  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
84  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
85  }
86  }
87  }
88 
89  void CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
90  {
91  const IMUState CurrentState = pIMUDevice->GetIMUState();
92 
93  if (m_IsVerbose)
94  {
95  std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
96  std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
97  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastCycleReferenceTimeStamp()) << " µs]" << std::endl;
98 
99  if (GetDispatchingMode() == eDecoupled)
100  {
101  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
102  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
103  }
104 
105  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
106  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
107  std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
108  std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
109  std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
110  std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
111  }
112 
113  memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
114  memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
115  memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
116  memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
117  }
118 
119  void CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
120  {
121  const IMUState CurrentState = pIMUDevice->GetIMUState();
122 
123  if (m_IsVerbose)
124  {
125  std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
126  std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
127  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastFusedCycleReferenceTimeStamp()) << " µs]" << std::endl;
128 
129  if (GetDispatchingMode() == eDecoupled)
130  {
131  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
132  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
133  }
134 
135  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
136  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
137  std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
138  std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
139  std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
140  std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
141  }
142 
143  memcpy(m_OrientationQuaternion, CurrentState.m_PhysicalData.m_QuaternionRotation, sizeof(float) * 4);
144  memcpy(m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
145  memcpy(m_GyroscopeRotation, CurrentState.m_PhysicalData.m_GyroscopeRotation, sizeof(float) * 3);
146  memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
147  }
148 
149  void CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
150  {
151  const IMUState CurrentState = pIMUDevice->GetIMUState();
152 
153  if (m_IsVerbose)
154  {
155  std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
156  std::cout << "\t[ Global time = " << CTimeStamp::GetElapsedSeconds(TimeStamp, pIMUDevice->GetReferenceTimeStamp()) << " s]" << std::endl;
157  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastIntegratedStateReferenceTimeStamp()) << " µs]" << std::endl;
158 
159  if (GetDispatchingMode() == eDecoupled)
160  {
161  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
162  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
163  }
164 
165  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]" << std::endl;
166  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0] << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << "," << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
167  std::cout << "\t[ Acceleration Magnitude = [" << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) * 1000.0f << " mm/s^2]" << std::endl;
168  std::cout << "\t[ Gyroscope Rotation = [" << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << "," << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
169  std::cout << "\t[ Magnetic Rotation = [" << CurrentState.m_PhysicalData.m_MagneticRotation[0] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[1] << "," << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
170  std::cout << "\t[ Quaternion Rotation = [" << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << "," << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
171  }
172  }
173 
174  void CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
175  {
176  if (m_IsVerbose)
177  {
178  std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex << CustomEvent.GetIMU()->GetDeviceId() << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << std::dec << ")" << std::endl;
179  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), CustomEvent.GetIMU()->GetReferenceTimeStamp()) << " µs]" << std::endl;
180  std::cout << "\t[ Latency = " << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(), GetLastCustomEventReferenceTimeStamp()) << " µs]" << std::endl;
181 
182  if (GetDispatchingMode() == eDecoupled)
183  {
184  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]" << std::endl;
185  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]" << std::endl;
186  }
187  }
188  }
189 }
IMU::CTimeStamp::GetElapsedSeconds
static float GetElapsedSeconds(const timeval &Post, const timeval &Pre)
Definition: IMUHelpers.h:29
IMU::IMUState::PhysicalData::m_MagneticRotation
float m_MagneticRotation[3]
Definition: IMUState.h:51
IMU::CIMUDevice::GetIMUState
IMUState GetIMUState() const
Definition: IMUDevice.h:121
IMU::CIMUDeducedReckoning::OnIMUStart
void OnIMUStart(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:57
IMU::CIMUDeducedReckoning::OnIMUIntegratedState
void OnIMUIntegratedState(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:149
IMU::IIMUEventDispatcher::eDecoupled
@ eDecoupled
Definition: IIMUEventDispatcher.h:24
IMU::IIMUEventDispatcher::GetMaximalPendingEvents
uint32_t GetMaximalPendingEvents()
Definition: IIMUEventDispatcher.cpp:99
IMU::CIMUDeducedReckoning::OnIMUCustomEvent
void OnIMUCustomEvent(const CIMUEvent &CustomEvent)
Definition: IMUDeducedReckoning.cpp:174
IMU::CIMUDeducedReckoning::m_Accelaration
float m_Accelaration[3]
Definition: IMUDeducedReckoning.h:61
IMU::CIMUDeducedReckoning::m_MagneticRotation
float m_MagneticRotation[3]
Definition: IMUDeducedReckoning.h:59
IMU::CIMUEvent::eOnIMUStart
@ eOnIMUStart
Definition: IMUEvent.h:23
IMU::CIMUDeducedReckoning::~CIMUDeducedReckoning
~CIMUDeducedReckoning() override
IMU::IMUState::m_PhysicalData
PhysicalData m_PhysicalData
Definition: IMUState.h:61
IMU::CIMUDeducedReckoning::OnIMUFusedCycle
void OnIMUFusedCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:119
IMU::IIMUEventDispatcher::SetEventHandling
void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
Definition: IIMUEventDispatcher.cpp:107
IMU::CIMUDeducedReckoning::m_G
const float m_G
Definition: IMUDeducedReckoning.h:56
IMU::IIMUEventDispatcher::GetLastStartTimeStamp
timeval GetLastStartTimeStamp()
Definition: IIMUEventDispatcher.cpp:233
IMU::CIMUEvent
Definition: IMUEvent.h:17
IMU::CIMUEvent::GetIMU
const CIMUDevice * GetIMU() const
Definition: IMUEvent.h:44
IMU::CIMUEvent::eOnIMUCustomEvent
@ eOnIMUCustomEvent
Definition: IMUEvent.h:23
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition: IMUEvent.h:23
IMU::CIMUDeducedReckoning::m_IsVerbose
const bool m_IsVerbose
Definition: IMUDeducedReckoning.h:55
IMU::CIMUEvent::eOnIMUStop
@ eOnIMUStop
Definition: IMUEvent.h:23
IMU::CIMUEvent::GetEventType
EventType GetEventType() const
Definition: IMUEvent.h:39
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:253
IMU::IMUState::ControlData::m_MessageCounter
long m_MessageCounter
Definition: IMUState.h:32
IMU::IIMUEventDispatcher
Definition: IIMUEventDispatcher.h:18
IMU::CIMUDevice
This class contains the the devices module and the thread for read the measurements.
Definition: IMUDevice.h:41
IMU::IIMUEventDispatcher::GetLastCycleReferenceTimeStamp
timeval GetLastCycleReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:249
IMU::IMUState
Definition: IMUState.h:15
IMU::IIMUEventDispatcher::eCoupled
@ eCoupled
Definition: IIMUEventDispatcher.h:24
IMU::IIMUEventDispatcher::GetDispatchingMode
DispatchingMode GetDispatchingMode()
Definition: IIMUEventDispatcher.cpp:76
IMUDeducedReckoning.h
IMU::CIMUDeducedReckoning::OnIMUEvent
void OnIMUEvent(const CIMUEvent &Event) override
Definition: IMUDeducedReckoning.cpp:27
IMU::IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp
timeval GetLastCustomEventReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:273
IMU::CIMUDevice::GetDeviceId
uint64_t GetDeviceId() const
Definition: IMUDevice.cpp:34
IMU
Definition: IIMUEventDispatcher.cpp:12
IMU::CIMUDeducedReckoning::m_GyroscopeRotation
float m_GyroscopeRotation[3]
Definition: IMUDeducedReckoning.h:60
IMU::IMUState::PhysicalData::m_QuaternionRotation
float m_QuaternionRotation[4]
Definition: IMUState.h:52
IMU::IMUState::PhysicalData::m_AccelerationMagnitud
float m_AccelerationMagnitud
Definition: IMUState.h:49
IMU::IIMUEventDispatcher::GetLastStopTimeStamp
timeval GetLastStopTimeStamp()
Definition: IIMUEventDispatcher.cpp:241
IMU::IMUState::PhysicalData::m_GyroscopeRotation
float m_GyroscopeRotation[3]
Definition: IMUState.h:50
IMU::CIMUDeducedReckoning::CIMUDeducedReckoning
CIMUDeducedReckoning(const bool IsVerbose)
Definition: IMUDeducedReckoning.cpp:13
IMU::IIMUEventDispatcher::GetTotalPendingEvents
uint32_t GetTotalPendingEvents()
Definition: IIMUEventDispatcher.h:47
IMU::IMUState::PhysicalData::m_Acceleration
float m_Acceleration[3]
Definition: IMUState.h:48
IMU::CIMUDeducedReckoning::OnIMUStop
void OnIMUStop(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:73
IMU::CIMUEvent::eOnIMUCycle
@ eOnIMUCycle
Definition: IMUEvent.h:23
IMU::CTimeStamp::GetElapsedMicroseconds
static long GetElapsedMicroseconds(const timeval &Post, const timeval &Pre)
Definition: IMUHelpers.h:39
IMU::CIMUEvent::GetTimeStamp
const timeval & GetTimeStamp() const
Definition: IMUEvent.h:49
IMU::IMUState::m_ControlData
ControlData m_ControlData
Definition: IMUState.h:60
IMU::CGeolocationInformation
Definition: IMUHelpers.h:54
IMU::CIMUDeducedReckoning::OnIMUCycle
void OnIMUCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:89
IMU::CIMUDevice::GetReferenceTimeStamp
const timeval & GetReferenceTimeStamp() const
Definition: IMUDevice.h:148
IMU::CIMUDeducedReckoning::m_OrientationQuaternion
float m_OrientationQuaternion[4]
Definition: IMUDeducedReckoning.h:58
IMU::IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp
timeval GetLastFusedCycleReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:257
IMU::IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp
timeval GetLastIntegratedStateReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:265
IMU::CIMUEvent::eOnIMUFusedCycle
@ eOnIMUFusedCycle
Definition: IMUEvent.h:23