14 float update_rate,
bool report_changes) :
16 d_update_interval (update_rate ? (1/update_rate) : 1.0),
17 d_report_changes (report_changes)
46 for (
size_t i = 0; i < 3; i++) {
67 for (
size_t i = 0; i < 3; i++) {
89 if (vector->
params.
name.size() == 0) {
return 0; }
99 printf(
"vrpn_IMU_Magnetometer: Adding local analog %s\n",
105 printf(
"vrpn_IMU_Magnetometer: Adding remote analog %s\n",
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());
128 if (vector->
ana == NULL) {
return 0; }
138 fprintf(stderr,
"vrpn_IMU_Magnetometer::teardown_vector(): delete failed\n");
159 for (
size_t i = 0; i < 3; i++) {
183 for (
size_t i = 0; i < 3; i++) {
212 fprintf(stderr,
"vrpn_IMU_Magnetometer: "
213 "No valid connection\n");
231 float update_rate,
bool report_changes)
233 d_update_interval(update_rate ? (1 / update_rate) : 1.0),
234 d_report_changes(report_changes)
251 for (
int i = 0; i < 3; i++) {
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);
296 for (
size_t i = 0; i < 3; i++) {
314 if (vector->
params.
name.size() == 0) {
return 0; }
324 printf(
"vrpn_Tracker_AnalogFly: Adding local analog %s\n",
330 printf(
"vrpn_Tracker_AnalogFly: Adding remote analog %s\n",
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());
354 if (vector->
ana == NULL) {
return 0; }
363 fprintf(stderr,
"vrpn_IMU_SimpleCombiner::teardown_vector(): delete failed\n");
401 fprintf(stderr,
"vrpn_IMU_SimpleCombiner: "
402 "cannot write pose message: tossing\n");
408 fprintf(stderr,
"vrpn_IMU_SimpleCombiner: "
409 "cannot write velocity message: tossing\n");
412 fprintf(stderr,
"vrpn_IMU_SimpleCombiner: "
413 "No valid connection\n");
433 q_type forward, inverse;
435 q_invert(inverse, forward);
446 q_mult(canonical, delta, inverse);
447 q_mult(canonical, forward, canonical);
463 double diff = fabs(accel - 9.80665);
468 double scale = 1.0 - diff;
481 q_vec_type gravity_global;
484 q_vec_normalize(gravity_global, gravity_global);
485 q_xform(gravity_global, forward, gravity_global);
491 q_vec_set(neg_y, 0, -1, 0);
493 q_from_two_vecs(rot, gravity_global, neg_y);
499 static q_type identity = { 0, 0, 0, 1 };
501 q_slerp(scaled_rot, identity, rot, gravity_scale);
525 q_vec_type magnetometer;
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);
538 q_vec_set(neg_z, 0, 0, -1);
539 q_from_two_vecs(rot, north_global, neg_z);
550 static q_type identity = { 0, 0, 0, 1 };
551 q_slerp(scaled_rot, identity, rot, north_rate);
574 q_mult(canonical, delta, inverse);
575 q_mult(
vel_quat, forward, canonical);
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]
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 ...
double d_gravity_restore_rate
double d_north_restore_rate
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
struct timeval d_prevtime
vrpn_IMU_Axis_Params params
vrpn_IMU_Axis_Params d_rotational_vel
std::string d_magnetometer_name
vrpn_IMU_Axis_Params d_acceleration
virtual int encode_to(char *buf)
virtual int encode_vel_to(char *buf)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
void(VRPN_CALLBACK * vrpn_ANALOGCHANGEHANDLER)(void *userdata, const vrpn_ANALOGCB info)
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