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
11namespace IMU
12{
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::CIMUDeducedReckoning::OnIMUCustomEvent
void OnIMUCustomEvent(const CIMUEvent &CustomEvent)
Definition IMUDeducedReckoning.cpp:291
IMU::CIMUDeducedReckoning::CIMUDeducedReckoning
CIMUDeducedReckoning(const bool IsVerbose)
Definition IMUDeducedReckoning.cpp:13
IMU::CIMUDeducedReckoning::~CIMUDeducedReckoning
~CIMUDeducedReckoning() override
IMU::CIMUDeducedReckoning::OnIMUEvent
void OnIMUEvent(const CIMUEvent &Event) override
Definition IMUDeducedReckoning.cpp:29
IMU::CIMUDeducedReckoning::OnIMUCycle
void OnIMUCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition IMUDeducedReckoning.cpp:112
IMU::CIMUDeducedReckoning::OnIMUIntegratedState
void OnIMUIntegratedState(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition IMUDeducedReckoning.cpp:237
IMU::CIMUDeducedReckoning::OnIMUStart
void OnIMUStart(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition IMUDeducedReckoning.cpp:60
IMU::CIMUDeducedReckoning::OnIMUStop
void OnIMUStop(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition IMUDeducedReckoning.cpp:86
IMU::CIMUDeducedReckoning::OnIMUFusedCycle
void OnIMUFusedCycle(const timeval &TimeStamp, const CIMUDevice *pIMUDevice)
Definition IMUDeducedReckoning.cpp:174
IMU::CIMUDevice
This class contains the the devices module and the thread for read the measurements.
Definition IMUDevice.h:42
IMU::CIMUDevice::GetReferenceTimeStamp
const timeval & GetReferenceTimeStamp() const
Definition IMUDevice.h:154
IMU::CIMUDevice::GetIMUState
IMUState GetIMUState() const
Definition IMUDevice.h:126
IMU::CIMUDevice::GetDeviceId
uint64_t GetDeviceId() const
Definition IMUDevice.cpp:49
IMU::CIMUEvent::eOnIMUIntegratedState
@ eOnIMUIntegratedState
Definition IMUEvent.h:27
IMU::CIMUEvent::GetTimeStamp
const timeval & GetTimeStamp() const
Definition IMUEvent.h:63
IMU::CIMUEvent::GetEventType
EventType GetEventType() const
Definition IMUEvent.h:51
IMU::CIMUEvent::GetIMU
const CIMUDevice * GetIMU() const
Definition IMUEvent.h:57
IMU::CTimeStamp::GetElapsedSeconds
static float GetElapsedSeconds(const timeval &Post, const timeval &Pre)
Definition IMUHelpers.h:30
IMU::CTimeStamp::GetElapsedMicroseconds
static long GetElapsedMicroseconds(const timeval &Post, const timeval &Pre)
Definition IMUHelpers.h:42
IMU::IIMUEventDispatcher::SetEventHandling
void SetEventHandling(const CIMUEvent::EventType Type, const bool Enabled)
Definition IIMUEventDispatcher.cpp:134
IMU::IIMUEventDispatcher::IIMUEventDispatcher
IIMUEventDispatcher(CIMUDevice *pIMUDevice)
Definition IIMUEventDispatcher.cpp:15
IMU::IMUState::m_PhysicalData
PhysicalData m_PhysicalData
Definition IMUState.h:65
IMU::IMUState::m_ControlData
ControlData m_ControlData
Definition IMUState.h:64