void VRPN_CALLBACK vrpn_Tracker_FilterOneEuro::handle_tracker_update(void *userdata, const vrpn_TRACKERCB info)
{
// Get pointer to the object we're dealing with.
vrpn_Tracker_FilterOneEuro *me = static_cast<vrpn_Tracker_FilterOneEuro *>(userdata);
// See if this sensor is within our range. If not, we ignore it.
if (info.sensor >= me->d_channels) {
return;
}
// Filter the position and orientation and then report the filtered value
// for this channel. Keep track of the delta-time, and update our current
// time so we get the right one next time.
double dt = vrpn_TimevalDurationSeconds(info.msg_time, me->d_last_report_times[info.sensor]);
if (dt <= 0) { dt = 1; } // Avoid divide-by-zero in case of fluke.
vrpn_float64 pos[3];
vrpn_float64 quat[4];
memcpy(pos, info.pos, sizeof(pos));
memcpy(quat, info.quat, sizeof(quat));
const vrpn_float64 *filtered = me->d_filters[info.sensor].filter(dt, pos);
q_vec_copy(me->pos, filtered);
const double *q_filtered = me->d_qfilters[info.sensor].filter(dt, quat);
q_normalize(me->d_quat, q_filtered);
me->timestamp = info.msg_time;
me->d_sensor = info.sensor;
me->d_last_report_times[info.sensor] = info.msg_time;
// Send the filtered report.
char msgbuf[512];
int len = me->encode_to(msgbuf);
if (me->d_connection->pack_message(len, me->timestamp, me->position_m_id,
me->d_sender_id, msgbuf,vrpn_CONNECTION_LOW_LATENCY)) {
fprintf(stderr, "vrpn_Tracker_FilterOneEuro: cannot write message: tossing\n");
}
}
void
q_xyz_quat_compose(q_xyz_quat_type *C_from_A_ptr,
q_xyz_quat_type *C_from_B_ptr,
q_xyz_quat_type *B_from_A_ptr)
{
q_vec_type rotated_BA_vec;
/* rotate local xlate into global */
q_xform(rotated_BA_vec, C_from_B_ptr->quat, B_from_A_ptr->xyz);
/* now add the xformed local vec to the unchanged global vec */
q_vec_add(C_from_A_ptr->xyz, C_from_B_ptr->xyz, rotated_BA_vec);
/* compose the rotations */
/* CA_rotate = CB_rotate . BA_rotate */
q_mult(C_from_A_ptr->quat, C_from_B_ptr->quat, B_from_A_ptr->quat);
q_normalize(C_from_A_ptr->quat, C_from_A_ptr->quat);
} /* qp_xyz_quat_compose */
void vrpn_Freespace::handleUserFrame(const struct freespace_UserFrame& user) {
vrpn_gettimeofday(&(vrpn_Tracker::timestamp), NULL);
// Translate the Freespace linear position into VRPN linear position.
vrpn_Tracker::pos[0] = user.linearPosY;
vrpn_Tracker::pos[1] = user.linearPosZ;
vrpn_Tracker::pos[2] = user.linearPosX;
// Transfer the Freespace coordinate system angular position quanternion
// into the VRPN coordinate system.
vrpn_Tracker::d_quat[Q_W] = user.angularPosA;
vrpn_Tracker::d_quat[Q_X] = user.angularPosC;
vrpn_Tracker::d_quat[Q_Y] = -user.angularPosD;
vrpn_Tracker::d_quat[Q_Z] = user.angularPosB;
q_normalize(vrpn_Tracker::d_quat, vrpn_Tracker::d_quat);
vrpn_Tracker_Server::report_pose(0, vrpn_Tracker::timestamp, vrpn_Tracker::pos, vrpn_Tracker::d_quat);
vrpn_Button::buttons[0] = user.button1;
vrpn_Button::buttons[1] = user.button2;
vrpn_Button::buttons[2] = user.button3;
vrpn_Button::buttons[3] = user.button4;
vrpn_Button::buttons[4] = user.button5;
vrpn_Button::timestamp = vrpn_Tracker::timestamp;
vrpn_Button::report_changes();
// this is totally arbitrary
// I'm not sure how exactly the 'deltaWheel' should be interpreted, but it seems
// like it would be the number of 'clicks' while turning the scroll wheel...
// vrpn was this as a 'fraction of a revolution' so I would assume that a return of 1 means
// the dial was spun 360 degrees. I'm going to pretend it takes 16 clicks to make a full
// revolution....'
// this values should probably set in a configuration file.
vrpn_Dial::dials[0] = user.deltaWheel / (vrpn_float64) 16;
vrpn_Dial::timestamp = vrpn_Tracker::timestamp;
vrpn_Dial::report_changes();
}
void vrpn_Tracker_OculusRift::_generate_report(int sensorNum)
{
if (!d_connection)
{
return;
}
d_sensor = sensorNum;
// Keep position set to 0, position may be added later by Oculus.
pos[0] = 0;
pos[1] = 0;
pos[2] = 0;
// Set d_quat
//fprintf(stderr, "Got here\n");
Quatf tmpOrientation = pRiftSFusion.GetOrientation();
//fprintf(stderr, "Got here too\n");
d_quat[Q_W] = tmpOrientation.w;
d_quat[Q_X] = tmpOrientation.x;
d_quat[Q_Y] = tmpOrientation.y;
d_quat[Q_Z] = tmpOrientation.z;
q_normalize(d_quat, d_quat);
// Replace old positon with current position
//q_vec_copy(_old_position[sensorNum], pos);
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_OculusRift: cannot write message: tossing\n");
}
}
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;
}
