void vrpn_Tracker_DeadReckoning_Rotation::handle_tracker_report(void *userdata,
const vrpn_TRACKERCB info)
{
// Find the pointer to the class that registered the callback and get a
// reference to the RotationState we're supposed to be using.
vrpn_Tracker_DeadReckoning_Rotation *me =
static_cast<vrpn_Tracker_DeadReckoning_Rotation *>(userdata);
if (info.sensor >= me->d_numSensors) {
me->send_text_message(vrpn_TEXT_WARNING)
<< "Received tracker message from sensor " << info.sensor
<< " but I only have " << me->d_numSensors
<< "sensors. Discarding.";
return;
}
vrpn_Tracker_DeadReckoning_Rotation::RotationState &state =
me->d_rotationStates[info.sensor];
if (!state.d_receivedAngularVelocityReport && me->d_estimateVelocity) {
// If we have not received any velocity reports, then we estimate
// the angular velocity using the last report (if any). The new
// combined rotation T3 = T2 * T1, where T2 is the difference in
// rotation between the last time (T1) and now (T3). We want to
// solve for T2 (so we can keep applying it going forward). We
// find it by right-multiuplying both sides of the equation by
// T1i (inverse of T1): T3 * T1i = T2.
if (state.d_lastReportTime.tv_sec != 0) {
q_type inverted;
q_invert(inverted, state.d_lastOrientation);
q_mult(state.d_rotationAmount, info.quat, inverted);
state.d_rotationInterval = vrpn_TimevalDurationSeconds(
info.msg_time, state.d_lastReportTime);
// If we get a zero or negative rotation interval, we're
// not going to be able to handle it, so we set things back
// to no rotation over a unit-time interval.
if (state.d_rotationInterval < 0) {
state.d_rotationInterval = 1;
q_make(state.d_rotationAmount, 0, 0, 0, 1);
}
}
}
// Keep track of the position, orientation and time for the next report
q_vec_copy(state.d_lastPosition, info.pos);
q_copy(state.d_lastOrientation, info.quat);
state.d_lastReportTime = info.msg_time;
// We have new data, so we send a new prediction.
me->sendNewPrediction(info.sensor);
}
void
q_xyz_quat_invert(q_xyz_quat_type *destPtr, q_xyz_quat_type *srcPtr)
{
/* invert rotation first */
q_invert(destPtr->quat, srcPtr->quat);
/* vec = -vec */
q_vec_invert(destPtr->xyz, srcPtr->xyz);
/* rotate translation offsets into inverted system */
q_xform(destPtr->xyz, destPtr->quat, destPtr->xyz);
} /* q_xyz_quat_invert */
void vrpn_IMU_SimpleCombiner::update_matrix_based_on_values(double time_interval)
{
//==================================================================
// Adjust the orientation based on the rotational velocity, which is
// measured in the rotated coordinate system. We need to rotate the
// difference vector back to the canonical orientation, apply the
// orientation change there, and then rotate back.
// Be sure to scale by the time value.
q_type forward, inverse;
q_copy(forward, d_quat);
q_invert(inverse, forward);
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_type delta;
q_from_euler(delta,
time_interval * d_rotational_vel.values[Q_Z],
time_interval * d_rotational_vel.values[Q_Y],
time_interval * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_type canonical;
q_mult(canonical, delta, inverse);
q_mult(canonical, forward, canonical);
q_mult(d_quat, canonical, d_quat);
//==================================================================
// To the extent that the acceleration vector's magnitude is equal
// to the expected gravity, rotate the orientation so that the vector
// points downward. This is measured in the rotated coordinate system,
// so we need to rotate back to canonical orientation and apply
// the difference there and then rotate back. The rate of rotation
// should be as specified in the gravity-rotation-rate parameter so
// we don't swing the head around too quickly but only slowly re-adjust.
double accel = sqrt(
d_acceleration.values[0] * d_acceleration.values[0] +
d_acceleration.values[1] * d_acceleration.values[1] +
d_acceleration.values[2] * d_acceleration.values[2] );
double diff = fabs(accel - 9.80665);
// Only adjust if we're close enough to the expected gravity
// @todo In a more complicated IMU tracker, base this on state and
// error estimates from a Kalman or other filter.
double scale = 1.0 - diff;
if (scale > 0) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double gravity_scale = scale * d_gravity_restore_rate * time_interval;
// Rotate the gravity vector by the estimated transform.
// We expect this direction to match the global down (-Y) vector.
q_vec_type gravity_global;
q_vec_set(gravity_global, d_acceleration.values[0],
d_acceleration.values[1], d_acceleration.values[2]);
q_vec_normalize(gravity_global, gravity_global);
q_xform(gravity_global, forward, gravity_global);
//printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]);
// Determine the rotation needed to take gravity and rotate
// it into the direction of -Y.
q_vec_type neg_y;
q_vec_set(neg_y, 0, -1, 0);
q_type rot;
q_from_two_vecs(rot, gravity_global, neg_y);
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_type scaled_rot;
q_slerp(scaled_rot, identity, rot, gravity_scale);
//printf("XXX Scaling gravity vector by %lg\n", gravity_scale);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
//==================================================================
// If we are getting compass data, and to the extent that the
// acceleration vector's magnitude is equal to the expected gravity,
// compute the cross product of the cross product to find the
// direction of north perpendicular to down. This is measured in
// the rotated coordinate system, so we need to rotate back to the
// canonical orientation and do the comparison there.
// The fraction of rotation should be as specified in the
// magnetometer-rotation-rate parameter so we don't swing the head
// around too quickly but only slowly re-adjust.
if (d_magnetometer.ana != NULL) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Find the North vector that is perpendicular to gravity by
// clearing its Y axis to zero and renormalizing.
q_vec_type magnetometer;
q_vec_set(magnetometer, d_magnetometer.values[0],
d_magnetometer.values[1], d_magnetometer.values[2]);
q_vec_type magnetometer_global;
q_xform(magnetometer_global, forward, magnetometer);
magnetometer_global[Q_Y] = 0;
q_vec_type north_global;
q_vec_normalize(north_global, magnetometer_global);
//printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]);
// Determine the rotation needed to take north and rotate it
// into the direction of negative Z.
q_vec_type neg_z;
q_vec_set(neg_z, 0, 0, -1);
q_from_two_vecs(rot, north_global, neg_z);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double north_rate = scale * d_north_restore_rate * time_interval;
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_slerp(scaled_rot, identity, rot, north_rate);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
}
}
//==================================================================
// Compute and store the velocity information
// This will be in the rotated coordinate system, so we need to
// rotate back to the identity orientation before storing.
// Convert from radians/second into a quaternion rotation as
// rotated in a hundredth of a second and set the rotational
// velocity dt to a hundredth of a second so that we don't
// risk wrapping.
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_from_euler(delta,
1e-2 * d_rotational_vel.values[Q_Z],
1e-2 * d_rotational_vel.values[Q_Y],
1e-2 * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_mult(canonical, delta, inverse);
q_mult(vel_quat, forward, canonical);
vel_quat_dt = 1e-2;
}
void vrpn_Tracker_OSVRHackerDevKit::on_data_received(std::size_t bytes,
vrpn_uint8 *buffer)
{
if (bytes != 32 && bytes != 16) {
send_text_message(vrpn_TEXT_WARNING)
<< "Received a report " << bytes
<< " in length, but expected it to be 32 or 16 bytes. Discarding. "
"(May indicate issues with HID!)";
return;
}
vrpn_uint8 firstByte = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);
vrpn_uint8 version = vrpn_uint8(0x0f) & firstByte;
_reportVersion = version;
switch (version) {
case 1:
if (bytes != 32 && bytes != 16) {
send_text_message(vrpn_TEXT_WARNING)
<< "Received a v1 report " << bytes
<< " in length, but expected it to be 32 or 16 bytes. "
"Discarding. "
"(May indicate issues with HID!)";
return;
}
break;
case 2:
if (bytes != 16) {
send_text_message(vrpn_TEXT_WARNING)
<< "Received a v2 report " << bytes
<< " in length, but expected it to be 16 bytes. Discarding. "
"(May indicate issues with HID!)";
return;
}
break;
case 3:
/// @todo once this report format is finalized, tighten up the
/// requirements.
if (bytes < 16) {
send_text_message(vrpn_TEXT_WARNING)
<< "Received a v3 report " << bytes
<< " in length, but expected it to be at least 16 bytes. "
"Discarding. "
"(May indicate issues with HID!)";
return;
}
break;
default:
/// Highlight that we don't know this report version well...
_knownVersion = false;
/// Do a minimal check of it.
if (bytes < 16) {
send_text_message(vrpn_TEXT_WARNING)
<< "Received a report claiming to be version " << int(version)
<< " that was " << bytes << " in length, but expected it to be "
"at least 16 bytes. Discarding. "
"(May indicate issues with HID!)";
return;
}
break;
}
// Report version as analog channel 0.
channel[0] = version;
vrpn_uint8 msg_seq = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);
// Signed, 16-bit, fixed-point numbers in Q1.14 format.
typedef vrpn::FixedPoint<1, 14> FixedPointValue;
d_quat[Q_X] =
FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
d_quat[Q_Y] =
FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
d_quat[Q_Z] =
FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
d_quat[Q_W] =
FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
vrpn_Tracker::timestamp = _timestamp;
{
char msgbuf[512];
int len = vrpn_Tracker::encode_to(msgbuf);
if (d_connection->pack_message(len, _timestamp, position_m_id,
d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
"message: tossing\n");
}
}
if (version >= 2) {
// We've got angular velocity in this message too
// Given XYZ radians per second velocity.
// Signed Q6.9
typedef vrpn::FixedPoint<6, 9> VelFixedPoint;
q_vec_type angVel;
angVel[0] =
VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
angVel[1] =
VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
angVel[2] =
VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
.get<vrpn_float64>();
//==================================================================
// Determine the rotational velocity, which is
// measured in the rotated coordinate system. We need to rotate the
// difference Euler angles back to the canonical orientation, apply
// the change, and then rotate back to change our coordinates.
// Be sure to scale by the time value vrpn_HDK_DT.
q_type forward, inverse;
q_copy(forward, d_quat);
q_invert(inverse, forward);
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_type delta;
{
delta[Q_W] = 0;
delta[Q_X] = angVel[Q_X] * vrpn_HDK_DT * 0.5;
delta[Q_Y] = angVel[Q_Y] * vrpn_HDK_DT * 0.5;
delta[Q_Z] = angVel[Q_Z] * vrpn_HDK_DT * 0.5;
q_exp(delta, delta);
q_normalize(delta, delta);
}
// Bring the delta back into canonical space
q_type canonical;
q_mult(canonical, delta, inverse);
q_mult(vel_quat, forward, canonical);
// Send the rotational velocity information.
// The dt value was set once, in the constructor.
char msgbuf[512];
int len = vrpn_Tracker::encode_vel_to(msgbuf);
if (d_connection->pack_message(len, _timestamp, velocity_m_id,
d_sender_id, msgbuf,
vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
"message: tossing\n");
}
}
if (version < 3) {
// No status info hidden in the first byte.
channel[1] = STATUS_UNKNOWN;
}
else {
// v3+: We've got status info in the upper nibble of the first byte.
bool gotVideo = (firstByte & (0x01 << 4)) != 0; // got video?
bool gotPortrait = (firstByte & (0x01 << 5)) != 0; // portrait mode?
if (!gotVideo) {
channel[1] = STATUS_NO_VIDEO_INPUT;
}
else {
if (gotPortrait) {
channel[1] = STATUS_PORTRAIT_VIDEO_INPUT;
}
else {
channel[1] = STATUS_LANDSCAPE_VIDEO_INPUT;
}
}
}
if (_messageCount == 0) {
// When _messageCount overflows, send a report whether or not there was
// a change.
vrpn_Analog::report();
}
else {
// otherwise just report if we have a change.
vrpn_Analog::report_changes();
};
_messageCount = (_messageCount + 1) % vrpn_HDK_STATUS_STRIDE;
}
