OSVR头部追踪数据格式及VRPN数据处理流程

头部追踪器是以一个普通HID设备呈现的，并且不断的以高速率(一般为400次每秒，除了早期的硬件达不到这个速率之外)上报输入数据，但是在设备过渡时（比如在HDMI状态过渡时）会暂时性地挂起追踪器数据的上报。

以下为它的协议（以字节为单位）：

第0个字节：

  位0~3：上报版本号，当前为3

  位4：只存在于版本3中，如果检测到视频数据则为“1”，否则为0

  位5：只存在于版本3中，如果检测到为竖屏模式（1080X1920 视频）则为“1”，如果检测到为横屏模式（1920X1080）,则为“0”

第1个字节：

  消息序列号（8位）

第2个字节：

  单位四元素i的低8位

第3个字节：

  单位四元素i的高8位

第4个字节：

  单位四元素j的低8位

第5个字节：

  单位四元数j的高8位

第6个字节：

  单位四元素k的低8位

第7个字节：

  单位四元素k的高8位

第8个字节：

  单位四元素w的低8位

第9个字节

  单位四元素w的高8位

第10~31个字节：

  版本1中：暂未使用，为未来扩展功能作保留

  版本2和3中：

    10~11字节：陀螺仪X轴速度，以弧度每秒为单位

    12~13字节：陀螺仪Y轴速度，以弧度每秒为单位

    14~15字节：陀螺仪Z轴速度，以弧度每秒为单位

    16~31：为未来扩展作保留

每个速度表示为有符号，16位定点，2的补码，Q9格式的表示形式。这些速度应该以指数坐标对待，它们是本身相对于本地坐标的，而不是全局的世界相对于房间坐标。

版本2：

 添加角速率数据

版本3：

  重新定义第0个字节中的高序位为检测视频状态位。

每个四元素都是有符号，16位定点，2的补码（反码+1），Q14格式的数据表示形式（因此，在sink端程序，需要解析它们并转成float格式的）。

定点数与浮点数相对应，定点数为小数点的位置是固定的，而浮点数则小数点位置为浮动的（指数浮动）。定点数表示法如下所示：

纯小数表示法：把小数点固定在数值部分最高位之前

符号

小数点

数值部分

纯整数表示法：把小数点固定在数值部分的最后面

符号

数值部分

小数点

Q格式就是将一个小数放大若干倍后，用整数来表示小数。Q越大，数值范围越小，但精度越高，相反，Q越小，数值范围越大，但精度越低。例如在一个16位定点中，Q0范围为-32768到+32767，其精度为1，而Q15的数值范围为-1到0.9999695，精度为1/32768 = 0.00003051。因此，对定点数而言，其数值范围与精度是一对矛盾，一个变量要想能够表示比较大的数值范围，必须牺牲精度为代码；而想提高精度，则数的表示范围就相应地减小。

浮点数与定点数的转换关系可表示为：

 






因此，

在单片机端：

将从dmp或其他算法获取到的float型四元数扩大2的14次方的倍数，即16384，再通过USB HID往Sink端传，类似以下片段：

buff[0] = 0x03;
buff[1] = cnt++;
buff[2] = ((int16_t)(q1*16384))>>0;
buff[3] = ((int16_t)(q1*16384))>>8;
buff[4] = ((int16_t)(q2*16384))>>0;
buff[5] = ((int16_t)(q2*16384))>>8;

buff[6] = ((int16_t)(q3*16384))>>0;
buff[7] = ((int16_t)(q3*16384))>>8;
buff[8] = ((int16_t)(q0*16384))>>0;
buff[9] = ((int16_t)(q0*16384))>>8;
buff[10] = ((int16_t)(radianX*512))>>0;
buff[11] = ((int16_t)(radianX*512))>>8;

buff[12] = ((int16_t)(radianY*512))>>0;
buff[13] = ((int16_t)(radianY*512))>>8;
buff[14] = ((int16_t)(radianZ*512))>>0;
buff[15] = ((int16_t)(radianZ*512))>>8;

buff[16] = 0;
buff[17] = 0;

在PC端：

  vrpn_Tracker_OSVRHackerDevKit.C内：

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;
}

on_data_received方法继承自父类vrpn_HidInterface，同时在父类vrpn_HidInterface中update函数不停的获取hid数据往on_data_received中发送，父类vrpn_HidInterface中关于hid的硬件信息又来自com_osvr_Multiserver.cpp时的枚举操作。而vrpn_Tracker_OSVRHackerDevKit又在mainloop中操作update函数，vrpn_Tracker_OSVRHackerDevKit的update函数是从它的祖父类vrpn_BaseClass继承而来的，类中vrpn_BaseClass对mainloop解释为：

通过每个main循环去操作update函数，远程对象的mainloop()应该先调用client_mainloop()，随后调用d_connection->mainloop()函数，而服务对象mainloop()应该先为设备服务，随后再调用server_mainloop()函数，该函数会不停的给客户程序回应心跳包，让客户端程序知道它仍然与服务端相连，而不应该去调用d_connection->mainloop()函数。

最终的mainloop函数调用都在client端被调用，都被vrpn_python域封装好了。比如button:

static PyMethodDef Button_methods[] = {
{"mainloop", (PyCFunction)Button::_definition::mainloop, METH_NOARGS, "Run the mainloop" },
{"register_change_handler", (PyCFunction)Button::_definition::register_change_handler, METH_VARARGS, "Register a callback handler to handle a position change" },
{"unregister_change_handler", (PyCFunction)Button::_definition::unregister_change_handler, METH_VARARGS, "Unregister a callback handler to handle a position change" },
{NULL}  /* Sentinel */
};