vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_IMU.C
Go to the documentation of this file.
1
5
6#include "vrpn_Tracker_IMU.h"
7#include <cmath>
8
9#undef VERBOSE
10
12 (std::string const &name, vrpn_Connection *output_con,
14 float update_rate, bool report_changes) :
15 vrpn_Analog_Server(name.c_str(), output_con),
16 d_update_interval (update_rate ? (1/update_rate) : 1.0),
17 d_report_changes (report_changes)
18{
19 // We have 3 output channels.
20 num_channel = 3;
21
22 //--------------------------------------------------------------------
23 // Set the current timestamp to "now" and current direction to along X.
24 // This is done in case we never hear from the analog devices.
27 channel[0] = 1;
28 channel[1] = 0;
29 channel[2] = 0;
30
31 // Set empty axis values.
32 d_vector.params = params;
34
35 //--------------------------------------------------------------------
36 // Open analog remote to use to listen to.
37 // If the name starts with the "*" character, use our output
38 // connection rather than getting a new connection for it.
39 // Set up callbacks to handle updates to the analog reports.
41
42 //--------------------------------------------------------------------
43 // Set the minimum and maximum values past each other and past what
44 // we expect from any real device, so they will be inialized properly
45 // the first time we get a report.
46 for (size_t i = 0; i < 3; i++) {
47 d_mins[i] = 1e100;
48 d_maxes[i] = -1e100;
49 }
50}
51
53{
54 // Tear down the analog update callbacks and remotes
56}
57
58// This routine handles updates of the analog values. The value coming in is
59// adjusted per the parameters in the axis description, and then used to
60// update the value there. The value is used by the device code in
61// mainloop() to update its internal state; that work is not done here.
62
64 (void *userdata, const vrpn_ANALOGCB info)
65{
66 vrpn_IMU_Vector *vector = static_cast<vrpn_IMU_Vector *>(userdata);
67 for (size_t i = 0; i < 3; i++) {
68 vector->values[i] =
69 (info.channel[vector->params.channels[i]] - vector->params.offsets[i])
70 * vector->params.scales[i];
71 }
72
73 // Store the time we got the value.
74 vector->time = info.msg_time;
75}
76
77// This sets up the Analog Remote for our vector, setting up the callback
78// needed to adjust the value based on changes in the analog input.
79// Returns 0 on success and -1 on failure.
80
82{
83 // Set a default time of now.
84 struct timeval now;
85 vrpn_gettimeofday(&now, NULL);
86 vector->time = now;
87
88 // If the name is empty, we're done.
89 if (vector->params.name.size() == 0) { return 0; }
90
91 // Open the analog device and point the remote at it.
92 // If the name starts with the '*' character, use the server
93 // connection rather than making a new one.
94 try {
95 if (vector->params.name[0] == '*') {
96 vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str()+1,
98#ifdef VERBOSE
99 printf("vrpn_IMU_Magnetometer: Adding local analog %s\n",
100 vector->params.name.c_str()+1);
101#endif
102 } else {
103 vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str());
104#ifdef VERBOSE
105 printf("vrpn_IMU_Magnetometer: Adding remote analog %s\n",
106 vector->params.name.c_str());
107#endif
108 }
109 } catch (...) { vector->ana = NULL; }
110 if (vector->ana == NULL) {
111 fprintf(stderr,"vrpn_IMU_Magnetometer: "
112 "Can't open Analog %s\n", vector->params.name.c_str());
113 return -1;
114 }
115
116 // Set up the callback handler for the channel
117 return vector->ana->register_change_handler(vector, handle_analog_update);
118}
119
120// This tears down the Analog Remote for our vector, undoing everything that
121// the setup did. Returns 0 on success and -1 on failure.
122
124{
125 int ret;
126
127 // If the analog pointer is empty, we're done.
128 if (vector->ana == NULL) { return 0; }
129
130 // Turn off the callback handler for the channel
131 ret = vector->ana->unregister_change_handler((void*)vector,
133
134 // Delete the analog device.
135 try {
136 delete vector->ana;
137 } catch (...) {
138 fprintf(stderr, "vrpn_IMU_Magnetometer::teardown_vector(): delete failed\n");
139 return -1;
140 }
141
142 return ret;
143}
144
146{
147 struct timeval now;
148 double interval; // How long since the last report, in secs
149
150 // Call generic server mainloop, since we are a server
152
153 // Mainloop() all of the analogs that are defined
154 // so that we will get all of the values fresh.
155 if (d_vector.ana != NULL) { d_vector.ana->mainloop(); };
156
157 // Keep track of the minimum and maximum values of
158 // our reports.
159 for (size_t i = 0; i < 3; i++) {
160 if (d_vector.values[i] < d_mins[i]) {
161 d_mins[i] = d_vector.values[i];
162 }
163 if (d_vector.values[i] > d_maxes[i]) {
164 d_maxes[i] = d_vector.values[i];
165 }
166 }
167
168 // See if it has been long enough since our last report.
169 // If so, generate a new one.
170 vrpn_gettimeofday(&now, NULL);
172
173 if (interval >= d_update_interval) {
174
175 // Set the time on the report
177
178 // Compute the unit vector by finding the normalized
179 // distance between the min and max for each axis and
180 // converting it to the range (-1,1) and then normalizing
181 // the resulting vector. Watch out for min and max
182 // being the same.
183 for (size_t i = 0; i < 3; i++) {
184 double diff = d_vector.values[i] - d_mins[i];
185 if (diff == 0) {
186 channel[i] = 0;
187 }
188 else {
189 channel[i] = -1 + 2 * diff / (d_maxes[i] - d_mins[i]);
190 }
191 }
192 double len = sqrt( channel[0] * channel[0] +
193 channel[1] * channel[1] + channel[2] * channel[2]);
194 if (len == 0) {
195 channel[0] = 1;
196 channel[1] = 0;
197 channel[2] = 0;
198 } else {
199 channel[0] /= len;
200 channel[1] /= len;
201 channel[2] /= len;
202 }
203
204 // pack and deliver analog unit vector report;
205 if (d_connection) {
206 if (d_report_changes) {
208 } else {
210 }
211 } else {
212 fprintf(stderr,"vrpn_IMU_Magnetometer: "
213 "No valid connection\n");
214 }
215
216 // We just sent a report, so reset the time
217 d_prevtime = now;
218 }
219
220 // We're not always sending reports, but we still want to
221 // update the interval clock so that we don't integrate over
222 // too long a timespan when we do finally report a change.
223 if (interval >= d_update_interval) {
224 d_prevtime = now;
225 }
226}
227
229 (const char * name, vrpn_Connection * trackercon,
231 float update_rate, bool report_changes)
232 : vrpn_Tracker(name, trackercon),
233 d_update_interval(update_rate ? (1 / update_rate) : 1.0),
234 d_report_changes(report_changes)
235{
236 // Set the restoration rates for gravity and north.
237 // These are the fraction of the way to full restoration that should
238 // occur over one second.
241
242 // Hook up the parameters for acceleration and rotational velocity
245
246 // Magetometers by definition produce unit vectors and
247 // have their axes as (x,y,z) so we just have to specify
248 // the name here and the others are all set to pass the
249 // values through.
251 for (int i = 0; i < 3; i++) {
255 }
256
257 //--------------------------------------------------------------------
258 // Open analog remotes for any channels that have non-NULL names.
259 // If the name starts with the "*" character, use tracker
260 // connection rather than getting a new connection for it.
261 // Set up callbacks to handle updates to the analog values.
265
266 //--------------------------------------------------------------------
267 // Set the current timestamp to "now", the current orientation to identity
268 // and the angular rotation velocity to zero.
272 q_vec_set(pos, 0, 0, 0);
273 q_from_axis_angle(d_quat, 0, 0, 1, 0);
274 q_vec_set(vel, 0, 0, 0);
275 q_from_axis_angle(vel_quat, 0, 0, 1, 0);
276 vel_quat_dt = 1;
277}
278
280{
281 // Tear down the analog update callbacks and remotes
285}
286
287// This routine handles updates of the analog values. The value coming in is
288// adjusted per the parameters in the axis description, and then used to
289// update the value there. The value is used by the device code in
290// mainloop() to update its internal state; that work is not done here.
291
293(void *userdata, const vrpn_ANALOGCB info)
294{
295 vrpn_IMU_Vector *vector = static_cast<vrpn_IMU_Vector *>(userdata);
296 for (size_t i = 0; i < 3; i++) {
297 vector->values[i] =
298 (info.channel[vector->params.channels[i]] - vector->params.offsets[i])
299 * vector->params.scales[i];
300 }
301
302 // Store the time we got the value.
303 vector->time = info.msg_time;
304}
305
306// This sets up the Analog Remote for one channel, setting up the callback
307// needed to adjust the value based on changes in the analog input.
308// Returns 0 on success and -1 on failure.
309
312{
313 // If the name is empty, we're done.
314 if (vector->params.name.size() == 0) { return 0; }
315
316 // Open the analog device and point the remote at it.
317 // If the name starts with the '*' character, use the server
318 // connection rather than making a new one.
319 try {
320 if (vector->params.name[0] == '*') {
321 vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str() + 1,
323#ifdef VERBOSE
324 printf("vrpn_Tracker_AnalogFly: Adding local analog %s\n",
325 vector->params.name.c_str() + 1);
326#endif
327 } else {
328 vector->ana = new vrpn_Analog_Remote(vector->params.name.c_str());
329#ifdef VERBOSE
330 printf("vrpn_Tracker_AnalogFly: Adding remote analog %s\n",
331 vector->params.name.c_str());
332#endif
333 }
334 } catch (...) { vector->ana = NULL; }
335 if (vector->ana == NULL) {
336 fprintf(stderr, "vrpn_Tracker_AnalogFly: "
337 "Can't open Analog %s\n", vector->params.name.c_str());
338 return -1;
339 }
340
341 // Set up the callback handler for the channel
342 return vector->ana->register_change_handler(vector, f);
343}
344
345// This tears down the Analog Remote for one channel, undoing everything that
346// the setup did. Returns 0 on success and -1 on failure.
347
350{
351 int ret;
352
353 // If the analog pointer is NULL, we're done.
354 if (vector->ana == NULL) { return 0; }
355
356 // Turn off the callback handler for the channel
357 ret = vector->ana->unregister_change_handler((void*)vector, f);
358
359 // Delete the analog device.
360 try {
361 delete vector->ana;
362 } catch (...) {
363 fprintf(stderr, "vrpn_IMU_SimpleCombiner::teardown_vector(): delete failed\n");
364 return -1;
365 }
366
367 return ret;
368}
369
371{
372 // Call generic server mainloop, since we are a server
374
375 // Mainloop() all of the analogs that are defined and the button
376 // so that we will get all of the values fresh.
377 if (d_acceleration.ana != NULL) { d_acceleration.ana->mainloop(); }
379 if (d_magnetometer.ana != NULL) { d_magnetometer.ana->mainloop(); }
380
381 // Update the matrix based on the change in time since the last
382 // update and the current values.
383 struct timeval now;
384 vrpn_gettimeofday(&now, NULL);
385 double delta_t = vrpn_TimevalDurationSeconds(now, d_prev_update_time);
387 d_prev_update_time = now;
388
389 // See if it has been long enough since our last report.
390 // If so, generate a new one.
391 double interval = vrpn_TimevalDurationSeconds(now, d_prevtime);
392 if (interval >= d_update_interval) {
393
394 // pack and deliver tracker report with info from the current matrix;
395 if (d_connection) {
396 char msgbuf[1000];
397 int len = encode_to(msgbuf);
398 if (d_connection->pack_message(len, now,
399 position_m_id, d_sender_id, msgbuf,
401 fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
402 "cannot write pose message: tossing\n");
403 }
404 len = encode_vel_to(msgbuf);
405 if (d_connection->pack_message(len, now,
406 velocity_m_id, d_sender_id, msgbuf,
408 fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
409 "cannot write velocity message: tossing\n");
410 }
411 } else {
412 fprintf(stderr, "vrpn_IMU_SimpleCombiner: "
413 "No valid connection\n");
414 }
415
416 // We just sent a report, so reset the time
417 d_prevtime = now;
418 }
419}
420
421// This routine will update the current matrix based on the current values
422// in the offsets list for each axis, and the length of time over which the
423// action is taking place (time_interval).
424
426{
427 //==================================================================
428 // Adjust the orientation based on the rotational velocity, which is
429 // measured in the rotated coordinate system. We need to rotate the
430 // difference vector back to the canonical orientation, apply the
431 // orientation change there, and then rotate back.
432 // Be sure to scale by the time value.
433 q_type forward, inverse;
434 q_copy(forward, d_quat);
435 q_invert(inverse, forward);
436 // Remember that Euler angles in Quatlib have rotation around Z in
437 // the first term. Compute the time-scaled delta transform in
438 // canonical space.
439 q_type delta;
440 q_from_euler(delta,
441 time_interval * d_rotational_vel.values[Q_Z],
442 time_interval * d_rotational_vel.values[Q_Y],
443 time_interval * d_rotational_vel.values[Q_X]);
444 // Bring the delta back into canonical space
445 q_type canonical;
446 q_mult(canonical, delta, inverse);
447 q_mult(canonical, forward, canonical);
448 q_mult(d_quat, canonical, d_quat);
449
450 //==================================================================
451 // To the extent that the acceleration vector's magnitude is equal
452 // to the expected gravity, rotate the orientation so that the vector
453 // points downward. This is measured in the rotated coordinate system,
454 // so we need to rotate back to canonical orientation and apply
455 // the difference there and then rotate back. The rate of rotation
456 // should be as specified in the gravity-rotation-rate parameter so
457 // we don't swing the head around too quickly but only slowly re-adjust.
458
459 double accel = sqrt(
463 double diff = fabs(accel - 9.80665);
464
465 // Only adjust if we're close enough to the expected gravity
466 // @todo In a more complicated IMU tracker, base this on state and
467 // error estimates from a Kalman or other filter.
468 double scale = 1.0 - diff;
469 if (scale > 0) {
470 // Get a new forward and inverse matrix from the current, just-
471 // rotated matrix.
472 q_copy(forward, d_quat);
473
474 // Change how fast we adjust based on how close we are to the
475 // expected value of gravity. Then further scale this by the
476 // amount of time since the last estimate.
477 double gravity_scale = scale * d_gravity_restore_rate * time_interval;
478
479 // Rotate the gravity vector by the estimated transform.
480 // We expect this direction to match the global down (-Y) vector.
481 q_vec_type gravity_global;
482 q_vec_set(gravity_global, d_acceleration.values[0],
484 q_vec_normalize(gravity_global, gravity_global);
485 q_xform(gravity_global, forward, gravity_global);
486 //printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]);
487
488 // Determine the rotation needed to take gravity and rotate
489 // it into the direction of -Y.
490 q_vec_type neg_y;
491 q_vec_set(neg_y, 0, -1, 0);
492 q_type rot;
493 q_from_two_vecs(rot, gravity_global, neg_y);
494
495 // Scale the rotation by the fraction of the orientation we
496 // should remove based on the time that has passed, how well our
497 // gravity vector matches expected, and the specified rate of
498 // correction.
499 static q_type identity = { 0, 0, 0, 1 };
500 q_type scaled_rot;
501 q_slerp(scaled_rot, identity, rot, gravity_scale);
502 //printf("XXX Scaling gravity vector by %lg\n", gravity_scale);
503
504 // Rotate by this angle.
505 q_mult(d_quat, scaled_rot, d_quat);
506
507 //==================================================================
508 // If we are getting compass data, and to the extent that the
509 // acceleration vector's magnitude is equal to the expected gravity,
510 // compute the cross product of the cross product to find the
511 // direction of north perpendicular to down. This is measured in
512 // the rotated coordinate system, so we need to rotate back to the
513 // canonical orientation and do the comparison there.
514 // The fraction of rotation should be as specified in the
515 // magnetometer-rotation-rate parameter so we don't swing the head
516 // around too quickly but only slowly re-adjust.
517
518 if (d_magnetometer.ana != NULL) {
519 // Get a new forward and inverse matrix from the current, just-
520 // rotated matrix.
521 q_copy(forward, d_quat);
522
523 // Find the North vector that is perpendicular to gravity by
524 // clearing its Y axis to zero and renormalizing.
525 q_vec_type magnetometer;
526 q_vec_set(magnetometer, d_magnetometer.values[0],
528 q_vec_type magnetometer_global;
529 q_xform(magnetometer_global, forward, magnetometer);
530 magnetometer_global[Q_Y] = 0;
531 q_vec_type north_global;
532 q_vec_normalize(north_global, magnetometer_global);
533 //printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]);
534
535 // Determine the rotation needed to take north and rotate it
536 // into the direction of negative Z.
537 q_vec_type neg_z;
538 q_vec_set(neg_z, 0, 0, -1);
539 q_from_two_vecs(rot, north_global, neg_z);
540
541 // Change how fast we adjust based on how close we are to the
542 // expected value of gravity. Then further scale this by the
543 // amount of time since the last estimate.
544 double north_rate = scale * d_north_restore_rate * time_interval;
545
546 // Scale the rotation by the fraction of the orientation we
547 // should remove based on the time that has passed, how well our
548 // gravity vector matches expected, and the specified rate of
549 // correction.
550 static q_type identity = { 0, 0, 0, 1 };
551 q_slerp(scaled_rot, identity, rot, north_rate);
552
553 // Rotate by this angle.
554 q_mult(d_quat, scaled_rot, d_quat);
555 }
556 }
557
558 //==================================================================
559 // Compute and store the velocity information
560 // This will be in the rotated coordinate system, so we need to
561 // rotate back to the identity orientation before storing.
562 // Convert from radians/second into a quaternion rotation as
563 // rotated in a hundredth of a second and set the rotational
564 // velocity dt to a hundredth of a second so that we don't
565 // risk wrapping.
566 // Remember that Euler angles in Quatlib have rotation around Z in
567 // the first term. Compute the time-scaled delta transform in
568 // canonical space.
569 q_from_euler(delta,
570 1e-2 * d_rotational_vel.values[Q_Z],
571 1e-2 * d_rotational_vel.values[Q_Y],
572 1e-2 * d_rotational_vel.values[Q_X]);
573 // Bring the delta back into canonical space
574 q_mult(canonical, delta, inverse);
575 q_mult(vel_quat, forward, canonical);
576 vel_quat_dt = 1e-2;
577}
578
virtual int unregister_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
virtual int register_change_handler(void *userdata, vrpn_ANALOGCHANGEHANDLER handler)
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Makes public the protected base class function.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Makes public the protected base class function.
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:38
struct timeval timestamp
Definition vrpn_Analog.h:41
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Generic connection class not specific to the transport mechanism.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
This file contains classes useful in producing tracker reports from inertial-navitation units (IMUs)....
double d_mins[3]
Minimum, maximum, and current values for each axis.
virtual VRPN_API ~vrpn_IMU_Magnetometer()
int setup_vector(vrpn_IMU_Vector *vector)
VRPN_API vrpn_IMU_Magnetometer(std::string const &name, vrpn_Connection *output_con, vrpn_IMU_Axis_Params params, float update_rate, bool report_changes=VRPN_FALSE)
Constructor.
virtual VRPN_API void mainloop()
Override base class function.
int teardown_vector(vrpn_IMU_Vector *vector)
vrpn_IMU_Vector d_vector
Axes to handle gathering and scaling the required data.
static void VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
struct timeval d_prevtime
void update_matrix_based_on_values(double time_interval)
int setup_vector(vrpn_IMU_Vector *vector, vrpn_ANALOGCHANGEHANDLER f)
vrpn_IMU_Vector d_magnetometer
vrpn_IMU_Vector d_rotational_vel
virtual VRPN_API void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
int teardown_vector(vrpn_IMU_Vector *vector, vrpn_ANALOGCHANGEHANDLER f)
VRPN_API vrpn_IMU_SimpleCombiner(const char *name, vrpn_Connection *trackercon, vrpn_Tracker_IMU_Params *params, float update_rate, bool report_changes=VRPN_FALSE)
virtual VRPN_API ~vrpn_IMU_SimpleCombiner(void)
vrpn_IMU_Vector d_acceleration
static void VRPN_CALLBACK handle_analog_update(void *userdata, const vrpn_ANALOGCB info)
struct timeval d_prev_update_time
vrpn_IMU_Axis_Params params
struct timeval time
vrpn_Analog_Remote * ana
vrpn_IMU_Axis_Params d_rotational_vel
vrpn_IMU_Axis_Params d_acceleration
virtual int encode_to(char *buf)
vrpn_float64 vel_quat[4]
vrpn_float64 d_quat[4]
vrpn_int32 velocity_m_id
vrpn_float64 vel_quat_dt
vrpn_float64 vel[3]
virtual int encode_vel_to(char *buf)
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
struct timeval msg_time
vrpn_float64 channel[vrpn_CHANNEL_MAX]
void(VRPN_CALLBACK * vrpn_ANALOGCHANGEHANDLER)(void *userdata, const vrpn_ANALOGCB info)
#define VRPN_CALLBACK
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99