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(),
15  m_IsVerbose(IsVerbose),
16  m_G(IMU::CGeolocationInformation::GetGravitationalAcceleration())
17  {
18  SetEventHandling(CIMUEvent::eOnIMUStart, true);
19  SetEventHandling(CIMUEvent::eOnIMUStop, true);
20  SetEventHandling(CIMUEvent::eOnIMUCycle, false);
21  SetEventHandling(CIMUEvent::eOnIMUFusedCycle, true);
22  SetEventHandling(CIMUEvent::eOnIMUIntegratedState, false);
23  SetEventHandling(CIMUEvent::eOnIMUCustomEvent, true);
24  }
25 
26  CIMUDeducedReckoning::~CIMUDeducedReckoning() = default;
27 
28  void
29  CIMUDeducedReckoning::OnIMUEvent(const CIMUEvent& Event)
30  {
31  switch (Event.GetEventType())
32  {
33  case CIMUEvent::eOnIMUCycle:
34  OnIMUCycle(Event.GetTimeStamp(), Event.GetIMU());
35  break;
36 
37  case CIMUEvent::eOnIMUFusedCycle:
38  OnIMUFusedCycle(Event.GetTimeStamp(), Event.GetIMU());
39  break;
40 
41  case CIMUEvent::eOnIMUIntegratedState:
42  OnIMUIntegratedState(Event.GetTimeStamp(), Event.GetIMU());
43  break;
44 
45  case CIMUEvent::eOnIMUCustomEvent:
46  OnIMUCustomEvent(Event);
47  break;
48 
49  case CIMUEvent::eOnIMUStart:
50  OnIMUStart(Event.GetTimeStamp(), Event.GetIMU());
51  break;
52 
53  case CIMUEvent::eOnIMUStop:
54  OnIMUStop(Event.GetTimeStamp(), Event.GetIMU());
55  break;
56  }
57  }
58 
59  void
60  CIMUDeducedReckoning::OnIMUStart(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
61  {
62  if (m_IsVerbose)
63  {
64  std::cout << "OnIMUStart(IMU Device ID = " << pIMUDevice->GetDeviceId()
65  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")"
66  << std::endl;
67  std::cout << "\t[ Global time = "
68  << CTimeStamp::GetElapsedSeconds(TimeStamp,
69  pIMUDevice->GetReferenceTimeStamp())
70  << " s]" << std::endl;
71  std::cout << "\t[ Latency = "
72  << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStartTimeStamp())
73  << "]" << std::endl;
74 
75  if (GetDispatchingMode() == eDecoupled)
76  {
77  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
78  << std::endl;
79  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
80  << std::endl;
81  }
82  }
83  }
84 
85  void
86  CIMUDeducedReckoning::OnIMUStop(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
87  {
88  if (m_IsVerbose)
89  {
90  std::cout << "OnIMUStop(IMU Device ID = " << pIMUDevice->GetDeviceId()
91  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled") << ")"
92  << std::endl;
93  std::cout << "\t[ Global time = "
94  << CTimeStamp::GetElapsedSeconds(TimeStamp,
95  pIMUDevice->GetReferenceTimeStamp())
96  << " s]" << std::endl;
97  std::cout << "\t[ Latency = "
98  << CTimeStamp::GetElapsedMicroseconds(TimeStamp, GetLastStopTimeStamp())
99  << "]" << std::endl;
100 
101  if (GetDispatchingMode() == eDecoupled)
102  {
103  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
104  << std::endl;
105  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
106  << std::endl;
107  }
108  }
109  }
110 
111  void
112  CIMUDeducedReckoning::OnIMUCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
113  {
114  const IMUState CurrentState = pIMUDevice->GetIMUState();
115 
116  if (m_IsVerbose)
117  {
118  std::cout << "OnIMUCycle(IMU Device ID = 0x" << std::hex << pIMUDevice->GetDeviceId()
119  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
120  << std::dec << ")" << std::endl;
121  std::cout << "\t[ Global time = "
122  << CTimeStamp::GetElapsedSeconds(TimeStamp,
123  pIMUDevice->GetReferenceTimeStamp())
124  << " s]" << std::endl;
125  std::cout << "\t[ Latency = "
126  << CTimeStamp::GetElapsedMicroseconds(TimeStamp,
127  GetLastCycleReferenceTimeStamp())
128  << " µs]" << std::endl;
129 
130  if (GetDispatchingMode() == eDecoupled)
131  {
132  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
133  << std::endl;
134  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
135  << std::endl;
136  }
137 
138  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
139  << std::endl;
140  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
141  << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
142  << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
143  std::cout << "\t[ Acceleration Magnitude = ["
144  << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
145  1000.0f
146  << " mm/s^2]" << std::endl;
147  std::cout << "\t[ Gyroscope Rotation = ["
148  << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
149  << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
150  << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
151  std::cout << "\t[ Magnetic Rotation = ["
152  << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
153  << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
154  << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
155  std::cout << "\t[ Quaternion Rotation = ["
156  << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
157  << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
158  << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
159  << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
160  }
161 
162  memcpy(m_OrientationQuaternion,
163  CurrentState.m_PhysicalData.m_QuaternionRotation,
164  sizeof(float) * 4);
165  memcpy(
166  m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
167  memcpy(m_GyroscopeRotation,
168  CurrentState.m_PhysicalData.m_GyroscopeRotation,
169  sizeof(float) * 3);
170  memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
171  }
172 
173  void
174  CIMUDeducedReckoning::OnIMUFusedCycle(const timeval& TimeStamp, const CIMUDevice* pIMUDevice)
175  {
176  const IMUState CurrentState = pIMUDevice->GetIMUState();
177 
178  if (m_IsVerbose)
179  {
180  std::cout << "OnIMUFusedCycle(IMU Device ID = 0x" << std::hex
181  << pIMUDevice->GetDeviceId()
182  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
183  << std::dec << ")" << std::endl;
184  std::cout << "\t[ Global time = "
185  << CTimeStamp::GetElapsedSeconds(TimeStamp,
186  pIMUDevice->GetReferenceTimeStamp())
187  << " s]" << std::endl;
188  std::cout << "\t[ Latency = "
189  << CTimeStamp::GetElapsedMicroseconds(TimeStamp,
190  GetLastFusedCycleReferenceTimeStamp())
191  << " µs]" << std::endl;
192 
193  if (GetDispatchingMode() == eDecoupled)
194  {
195  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
196  << std::endl;
197  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
198  << std::endl;
199  }
200 
201  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
202  << std::endl;
203  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
204  << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
205  << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
206  std::cout << "\t[ Acceleration Magnitude = ["
207  << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
208  1000.0f
209  << " mm/s^2]" << std::endl;
210  std::cout << "\t[ Gyroscope Rotation = ["
211  << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
212  << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
213  << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
214  std::cout << "\t[ Magnetic Rotation = ["
215  << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
216  << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
217  << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
218  std::cout << "\t[ Quaternion Rotation = ["
219  << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
220  << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
221  << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
222  << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
223  }
224 
225  memcpy(m_OrientationQuaternion,
226  CurrentState.m_PhysicalData.m_QuaternionRotation,
227  sizeof(float) * 4);
228  memcpy(
229  m_MagneticRotation, CurrentState.m_PhysicalData.m_MagneticRotation, sizeof(float) * 3);
230  memcpy(m_GyroscopeRotation,
231  CurrentState.m_PhysicalData.m_GyroscopeRotation,
232  sizeof(float) * 3);
233  memcpy(m_Accelaration, CurrentState.m_PhysicalData.m_Acceleration, sizeof(float) * 3);
234  }
235 
236  void
237  CIMUDeducedReckoning::OnIMUIntegratedState(const timeval& TimeStamp,
238  const CIMUDevice* pIMUDevice)
239  {
240  const IMUState CurrentState = pIMUDevice->GetIMUState();
241 
242  if (m_IsVerbose)
243  {
244  std::cout << "OnIMUStateUpdate(IMU Device ID = 0x" << std::hex
245  << pIMUDevice->GetDeviceId()
246  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
247  << std::dec << ")" << std::endl;
248  std::cout << "\t[ Global time = "
249  << CTimeStamp::GetElapsedSeconds(TimeStamp,
250  pIMUDevice->GetReferenceTimeStamp())
251  << " s]" << std::endl;
252  std::cout << "\t[ Latency = "
253  << CTimeStamp::GetElapsedMicroseconds(
254  TimeStamp, GetLastIntegratedStateReferenceTimeStamp())
255  << " µs]" << std::endl;
256 
257  if (GetDispatchingMode() == eDecoupled)
258  {
259  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
260  << std::endl;
261  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
262  << std::endl;
263  }
264 
265  std::cout << "\t[ Cycle = " << CurrentState.m_ControlData.m_MessageCounter << "]"
266  << std::endl;
267  std::cout << "\t[ Acceleration = [" << CurrentState.m_PhysicalData.m_Acceleration[0]
268  << "," << CurrentState.m_PhysicalData.m_Acceleration[1] << ","
269  << CurrentState.m_PhysicalData.m_Acceleration[2] << "]" << std::endl;
270  std::cout << "\t[ Acceleration Magnitude = ["
271  << std::abs(CurrentState.m_PhysicalData.m_AccelerationMagnitud - m_G) *
272  1000.0f
273  << " mm/s^2]" << std::endl;
274  std::cout << "\t[ Gyroscope Rotation = ["
275  << CurrentState.m_PhysicalData.m_GyroscopeRotation[0] << ","
276  << CurrentState.m_PhysicalData.m_GyroscopeRotation[1] << ","
277  << CurrentState.m_PhysicalData.m_GyroscopeRotation[2] << "]" << std::endl;
278  std::cout << "\t[ Magnetic Rotation = ["
279  << CurrentState.m_PhysicalData.m_MagneticRotation[0] << ","
280  << CurrentState.m_PhysicalData.m_MagneticRotation[1] << ","
281  << CurrentState.m_PhysicalData.m_MagneticRotation[2] << "]" << std::endl;
282  std::cout << "\t[ Quaternion Rotation = ["
283  << CurrentState.m_PhysicalData.m_QuaternionRotation[0] << ","
284  << CurrentState.m_PhysicalData.m_QuaternionRotation[1] << ","
285  << CurrentState.m_PhysicalData.m_QuaternionRotation[2] << ","
286  << CurrentState.m_PhysicalData.m_QuaternionRotation[3] << "]" << std::endl;
287  }
288  }
289 
290  void
291  CIMUDeducedReckoning::OnIMUCustomEvent(const CIMUEvent& CustomEvent)
292  {
293  if (m_IsVerbose)
294  {
295  std::cout << "OnIMUCustomEvent(IMU Device ID = 0x" << std::hex
296  << CustomEvent.GetIMU()->GetDeviceId()
297  << ((GetDispatchingMode() == eCoupled) ? " Coupled" : " Decoupled")
298  << std::dec << ")" << std::endl;
299  std::cout << "\t[ Latency = "
300  << CTimeStamp::GetElapsedMicroseconds(
301  CustomEvent.GetTimeStamp(),
302  CustomEvent.GetIMU()->GetReferenceTimeStamp())
303  << " µs]" << std::endl;
304  std::cout << "\t[ Latency = "
305  << CTimeStamp::GetElapsedMicroseconds(CustomEvent.GetTimeStamp(),
306  GetLastCustomEventReferenceTimeStamp())
307  << " µs]" << std::endl;
308 
309  if (GetDispatchingMode() == eDecoupled)
310  {
311  std::cout << "\t[ Pending Events = " << GetTotalPendingEvents() << "]"
312  << std::endl;
313  std::cout << "\t[ Maximal Pending Events = " << GetMaximalPendingEvents() << "]"
314  << std::endl;
315  }
316  }
317  }
318 } // namespace IMU
IMU::CTimeStamp::GetElapsedSeconds
static float GetElapsedSeconds(const timeval &Post, const timeval &Pre)
Definition: IMUHelpers.h:30
IMU::IMUState::PhysicalData::m_MagneticRotation
float m_MagneticRotation[3]
Definition: IMUState.h:52
IMU::CIMUDevice::GetIMUState
IMUState GetIMUState() const
Definition: IMUDevice.h:126
IMU::CIMUDeducedReckoning::OnIMUStart
void OnIMUStart(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:60
IMU::CIMUDeducedReckoning::OnIMUIntegratedState
void OnIMUIntegratedState(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:237
IMU::IIMUEventDispatcher::eDecoupled
@ eDecoupled
Definition: IIMUEventDispatcher.h:25
IMU::IIMUEventDispatcher::GetMaximalPendingEvents
uint32_t GetMaximalPendingEvents()
Definition: IIMUEventDispatcher.cpp:125
IMU::CIMUDeducedReckoning::OnIMUCustomEvent
void OnIMUCustomEvent(const CIMUEvent &CustomEvent)
Definition: IMUDeducedReckoning.cpp:291
IMU::CIMUDeducedReckoning::m_Accelaration
float m_Accelaration[3]
Definition: IMUDeducedReckoning.h:63
IMU::CIMUDeducedReckoning::m_MagneticRotation
float m_MagneticRotation[3]
Definition: IMUDeducedReckoning.h:61
IMU::CIMUEvent::eOnIMUStart
@ eOnIMUStart
Definition: IMUEvent.h:23
IMU::CIMUDeducedReckoning::~CIMUDeducedReckoning
~CIMUDeducedReckoning() override
IMU::IMUState::m_PhysicalData
PhysicalData m_PhysicalData
Definition: IMUState.h:65
IMU::CIMUDeducedReckoning::OnIMUFusedCycle
void OnIMUFusedCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:174
IMU::IIMUEventDispatcher::SetEventHandling
void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
Definition: IIMUEventDispatcher.cpp:134
IMU::CIMUDeducedReckoning::m_G
const float m_G
Definition: IMUDeducedReckoning.h:58
IMU::IIMUEventDispatcher::GetLastStartTimeStamp
timeval GetLastStartTimeStamp()
Definition: IIMUEventDispatcher.cpp:265
IMU::CIMUEvent
Definition: IMUEvent.h:18
IMU::CIMUEvent::GetIMU
const CIMUDevice * GetIMU() const
Definition: IMUEvent.h:57
IMU::CIMUEvent::eOnIMUCustomEvent
@ eOnIMUCustomEvent
Definition: IMUEvent.h:28
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition: IMUEvent.h:27
IMU::CIMUDeducedReckoning::m_IsVerbose
const bool m_IsVerbose
Definition: IMUDeducedReckoning.h:57
IMU::CIMUEvent::eOnIMUStop
@ eOnIMUStop
Definition: IMUEvent.h:24
IMU::CIMUEvent::GetEventType
EventType GetEventType() const
Definition: IMUEvent.h:51
armarx::abs
std::vector< T > abs(const std::vector< T > &v)
Definition: VectorHelpers.h:281
IMU::IMUState::ControlData::m_MessageCounter
long m_MessageCounter
Definition: IMUState.h:34
IMU::IIMUEventDispatcher
Definition: IIMUEventDispatcher.h:19
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:283
IMU::IMUState
Definition: IMUState.h:15
IMU::IIMUEventDispatcher::eCoupled
@ eCoupled
Definition: IIMUEventDispatcher.h:24
IMU::IIMUEventDispatcher::GetDispatchingMode
DispatchingMode GetDispatchingMode()
Definition: IIMUEventDispatcher.cpp:100
IMUDeducedReckoning.h
IMU::CIMUDeducedReckoning::OnIMUEvent
void OnIMUEvent(const CIMUEvent &Event) override
Definition: IMUDeducedReckoning.cpp:29
IMU::IIMUEventDispatcher::GetLastCustomEventReferenceTimeStamp
timeval GetLastCustomEventReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:310
IMU::CIMUDevice::GetDeviceId
uint64_t GetDeviceId() const
Definition: IMUDevice.cpp:49
IMU
Definition: IIMUEventDispatcher.cpp:13
IMU::CIMUDeducedReckoning::m_GyroscopeRotation
float m_GyroscopeRotation[3]
Definition: IMUDeducedReckoning.h:62
IMU::IMUState::PhysicalData::m_QuaternionRotation
float m_QuaternionRotation[4]
Definition: IMUState.h:53
IMU::IMUState::PhysicalData::m_AccelerationMagnitud
float m_AccelerationMagnitud
Definition: IMUState.h:50
IMU::IIMUEventDispatcher::GetLastStopTimeStamp
timeval GetLastStopTimeStamp()
Definition: IIMUEventDispatcher.cpp:274
IMU::IMUState::PhysicalData::m_GyroscopeRotation
float m_GyroscopeRotation[3]
Definition: IMUState.h:51
IMU::CIMUDeducedReckoning::CIMUDeducedReckoning
CIMUDeducedReckoning(const bool IsVerbose)
Definition: IMUDeducedReckoning.cpp:13
IMU::IIMUEventDispatcher::GetTotalPendingEvents
uint32_t GetTotalPendingEvents()
Definition: IIMUEventDispatcher.h:49
IMU::IMUState::PhysicalData::m_Acceleration
float m_Acceleration[3]
Definition: IMUState.h:49
IMU::CIMUDeducedReckoning::OnIMUStop
void OnIMUStop(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:86
IMU::CIMUEvent::eOnIMUCycle
@ eOnIMUCycle
Definition: IMUEvent.h:25
IMU::CTimeStamp::GetElapsedMicroseconds
static long GetElapsedMicroseconds(const timeval &Post, const timeval &Pre)
Definition: IMUHelpers.h:42
IMU::CIMUEvent::GetTimeStamp
const timeval & GetTimeStamp() const
Definition: IMUEvent.h:63
IMU::IMUState::m_ControlData
ControlData m_ControlData
Definition: IMUState.h:64
IMU::CGeolocationInformation
Definition: IMUHelpers.h:58
IMU::CIMUDeducedReckoning::OnIMUCycle
void OnIMUCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition: IMUDeducedReckoning.cpp:112
IMU::CIMUDevice::GetReferenceTimeStamp
const timeval & GetReferenceTimeStamp() const
Definition: IMUDevice.h:154
IMU::CIMUDeducedReckoning::m_OrientationQuaternion
float m_OrientationQuaternion[4]
Definition: IMUDeducedReckoning.h:60
IMU::IIMUEventDispatcher::GetLastFusedCycleReferenceTimeStamp
timeval GetLastFusedCycleReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:292
IMU::IIMUEventDispatcher::GetLastIntegratedStateReferenceTimeStamp
timeval GetLastIntegratedStateReferenceTimeStamp()
Definition: IIMUEventDispatcher.cpp:301
IMU::CIMUEvent::eOnIMUFusedCycle
@ eOnIMUFusedCycle
Definition: IMUEvent.h:26