vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Tracker_OSVRHackerDevKit.C
Go to the documentation of this file.
1
10
12
14#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_NORMAL, etc
15#include "vrpn_FixedPoint.h" // for vrpn::FixedPoint
16#include <quat.h> // for Q_W, Q_X, etc.
17
18#include <cstring> // for memset
19#include <stdexcept> // for logic_error
20#include <cmath>
21
23
24#if defined(VRPN_USE_HID)
25
26// USB vendor and product IDs for the models we support
27static const vrpn_uint16 vrpn_OSVR_VENDOR = 0x1532;
28static const vrpn_uint16 vrpn_OSVR_HACKER_DEV_KIT_HMD = 0x0b00;
29
31static const vrpn_uint16 vrpn_OSVR_ALT_VENDOR = 0x03EB;
32static const vrpn_uint16 vrpn_OSVR_ALT_HACKER_DEV_KIT_HMD = 0x2421;
33
34// Every nth report, we'll force an analog report with the status data, even if
35// no changes have occurred. 400 is the most common tracker rate (reports per
36// second) so this is about once a second.
37static const vrpn_uint16 vrpn_HDK_STATUS_STRIDE = 400;
38static const vrpn_float64 vrpn_HDK_DT = 1.0 / 50;
39
40static vrpn_HidAcceptor *makeHDKHidAcceptor()
41{
43 try {
45 new vrpn_HidProductAcceptor(vrpn_OSVR_VENDOR,
46 vrpn_OSVR_HACKER_DEV_KIT_HMD),
47 new vrpn_HidProductAcceptor(vrpn_OSVR_ALT_VENDOR,
48 vrpn_OSVR_ALT_HACKER_DEV_KIT_HMD));
49 } catch (...) { return NULL; }
50 return ret;
51}
52
53// NOTE: Cannot use the vendor-and-product parameters in the
54// vrpn_HidInterface because there are one of two possible
55// vendor/product pairs. The Acceptor will still correctly
56// work, it will just do more work during the enumeration phase
57// because it will have to check all devices in the system.
59 hid_device *dev,
61 : vrpn_Tracker(name, c)
62 , vrpn_Analog(name, c)
63 , vrpn_HidInterface(makeHDKHidAcceptor(), dev)
64 , _wasConnected(false)
65 , _messageCount(0)
67 , _knownVersion(true)
68{
70}
71
74 : vrpn_Tracker(name, c)
75 , vrpn_Analog(name, c)
76 , vrpn_HidInterface(makeHDKHidAcceptor())
77 , _wasConnected(false)
78 , _messageCount(0)
80 , _knownVersion(true)
81{
83}
84
87 vrpn_Tracker::num_sensors = 1; // only orientation
88
89 // Initialize the state
90 std::memset(d_quat, 0, sizeof(d_quat));
91 d_quat[Q_W] = 1.0;
92 // Arbitrary dt that will be less than a full rotation.
93 vel_quat_dt = vrpn_HDK_DT;
94
96 vrpn_Analog::num_channel = 2; // version and video input status
97
98 // Set the timestamp
100}
101
103{
104 try {
105 delete m_acceptor;
106 } catch (...) {
107 fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit::~vrpn_Tracker_OSVRHackerDevKit(): delete failed\n");
108 return;
109 }
110}
111
113 vrpn_uint8 *buffer)
114{
115 if (bytes != 32 && bytes != 16) {
117 << "Received a report " << bytes
118 << " in length, but expected it to be 32 or 16 bytes. Discarding. "
119 "(May indicate issues with HID!)";
120 return;
121 }
122
123 vrpn_uint8 firstByte = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);
124
125 vrpn_uint8 version = vrpn_uint8(0x0f) & firstByte;
126 _reportVersion = version;
127
128 switch (version) {
129 case 1:
130 if (bytes != 32 && bytes != 16) {
132 << "Received a v1 report " << bytes
133 << " in length, but expected it to be 32 or 16 bytes. "
134 "Discarding. "
135 "(May indicate issues with HID!)";
136 return;
137 }
138 break;
139 case 2:
140 if (bytes != 16) {
142 << "Received a v2 report " << bytes
143 << " in length, but expected it to be 16 bytes. Discarding. "
144 "(May indicate issues with HID!)";
145 return;
146 }
147 break;
148
149 case 3:
152 if (bytes < 16) {
154 << "Received a v3 report " << bytes
155 << " in length, but expected it to be at least 16 bytes. "
156 "Discarding. "
157 "(May indicate issues with HID!)";
158 return;
159 }
160 break;
161 default:
163 _knownVersion = false;
165 if (bytes < 16) {
167 << "Received a report claiming to be version " << int(version)
168 << " that was " << bytes << " in length, but expected it to be "
169 "at least 16 bytes. Discarding. "
170 "(May indicate issues with HID!)";
171 return;
172 }
173 break;
174 }
175
176 // Report version as analog channel 0.
177 channel[0] = version;
178
179 vrpn_uint8 msg_seq = vrpn_unbuffer_from_little_endian<vrpn_uint8>(buffer);
180
181 // Signed, 16-bit, fixed-point numbers in Q1.14 format.
182 typedef vrpn::FixedPoint<1, 14> FixedPointValue;
183 d_quat[Q_X] =
184 FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
185 .get<vrpn_float64>();
186 d_quat[Q_Y] =
187 FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
188 .get<vrpn_float64>();
189 d_quat[Q_Z] =
190 FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
191 .get<vrpn_float64>();
192 d_quat[Q_W] =
193 FixedPointValue(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
194 .get<vrpn_float64>();
195
197 {
198 char msgbuf[512];
199 int len = vrpn_Tracker::encode_to(msgbuf);
200 if (d_connection->pack_message(len, _timestamp, position_m_id,
201 d_sender_id, msgbuf,
203 fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
204 "message: tossing\n");
205 }
206 }
207 if (version >= 2) {
208 // We've got angular velocity in this message too
209 // Given XYZ radians per second velocity.
210 // Signed Q6.9
211 typedef vrpn::FixedPoint<6, 9> VelFixedPoint;
212 q_vec_type angVel;
213 angVel[0] =
214 VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
215 .get<vrpn_float64>();
216 angVel[1] =
217 VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
218 .get<vrpn_float64>();
219 angVel[2] =
220 VelFixedPoint(vrpn_unbuffer_from_little_endian<vrpn_int16>(buffer))
221 .get<vrpn_float64>();
222
223 //==================================================================
224 // Determine the rotational velocity, which is
225 // measured in the rotated coordinate system. We need to rotate the
226 // difference Euler angles back to the canonical orientation, apply
227 // the change, and then rotate back to change our coordinates.
228 // Be sure to scale by the time value vrpn_HDK_DT.
229 q_type forward, inverse;
230 q_copy(forward, d_quat);
231 q_invert(inverse, forward);
232 // Remember that Euler angles in Quatlib have rotation around Z in
233 // the first term. Compute the time-scaled delta transform in
234 // canonical space.
235 q_type delta;
236 {
237 delta[Q_W] = 0;
238 delta[Q_X] = angVel[Q_X] * vrpn_HDK_DT * 0.5;
239 delta[Q_Y] = angVel[Q_Y] * vrpn_HDK_DT * 0.5;
240 delta[Q_Z] = angVel[Q_Z] * vrpn_HDK_DT * 0.5;
241 q_exp(delta, delta);
242 q_normalize(delta, delta);
243 }
244 // Bring the delta back into canonical space
245 q_type canonical;
246 q_mult(canonical, delta, inverse);
247 q_mult(vel_quat, forward, canonical);
248
249 // Send the rotational velocity information.
250 // The dt value was set once, in the constructor.
251 char msgbuf[512];
252 int len = vrpn_Tracker::encode_vel_to(msgbuf);
253 if (d_connection->pack_message(len, _timestamp, velocity_m_id,
254 d_sender_id, msgbuf,
256 fprintf(stderr, "vrpn_Tracker_OSVRHackerDevKit: cannot write "
257 "message: tossing\n");
258 }
259 }
260 if (version < 3) {
261 // No status info hidden in the first byte.
263 }
264 else {
265 // v3+: We've got status info in the upper nibble of the first byte.
266 bool gotVideo = (firstByte & (0x01 << 4)) != 0; // got video?
267 bool gotPortrait = (firstByte & (0x01 << 5)) != 0; // portrait mode?
268 if (!gotVideo) {
270 }
271 else {
272 if (gotPortrait) {
274 }
275 else {
277 }
278 }
279 }
280
281 if (_messageCount == 0) {
282 // When _messageCount overflows, send a report whether or not there was
283 // a change.
285 }
286 else {
287 // otherwise just report if we have a change.
289 };
290 _messageCount = (_messageCount + 1) % vrpn_HDK_STATUS_STRIDE;
291}
292
294{
296
297 update();
298
299 if (connected() && !_wasConnected) {
301 << "Successfully connected to OSVR Hacker Dev Kit HMD, receiving "
302 "version "
303 << int(_reportVersion) << " reports.";
304
305 if (!_knownVersion) {
306
308 << "Connected to OSVR Hacker Dev Kit HMD, receiving "
309 "version "
310 << int(_reportVersion)
311 << " reports, newer than what is specifically recognized. You "
312 "may want to update your server to best make use of this "
313 "new report format.";
314 }
315 }
316 if (!connected()) {
317 channel[0] = 0;
320 }
321
323
324 if (!_wasConnected) {
325 m_acceptor->reset();
326 reconnect();
327 }
328
330}
331
332#endif
A fixed-point value class.
T get() const
Returns a floating-point representation of this fixed-point value.
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:38
vrpn_Analog(const char *name, vrpn_Connection *c=NULL)
Definition vrpn_Analog.C:14
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
Definition vrpn_Analog.C:94
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition vrpn_Analog.C:71
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...
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
Generic connection class not specific to the transport mechanism.
Accepts devices meeting at least one of two criteria. NOT SHORT-CIRCUIT. Another demonstration of acc...
virtual bool reconnect()
Tries to reconnect to an acceptable device. Call this if you suspect a hotplug event has occurred.
vrpn_HidInterface(vrpn_HidAcceptor *acceptor, vrpn_uint16 vendor=0, vrpn_uint16 product=0, hid_device *device=NULL)
Constructor If we already have a HID device from some other source, it can be passed and we'll take o...
vrpn_HidAcceptor * m_acceptor
This is the HidAcceptor we use when reconnecting.
virtual void update()
Polls the device buffers and causes on_data_received callbacks if appropriate You NEED to call this f...
virtual bool connected() const
Returns true iff the last device I/O succeeded.
Accepts any device with the given vendor and product IDs.
bool _wasConnected
Flag indicating whether we were connected last time through the mainloop. Used to send a "normal"-sev...
void on_data_received(std::size_t bytes, vrpn_uint8 *buffer)
Extracts the sensor values from each report.
vrpn_Tracker_OSVRHackerDevKit(const char *name, hid_device *dev=NULL, vrpn_Connection *c=NULL)
Constructor.
vrpn_uint16 _messageCount
Used to forcibly send the analog update every so often.
struct timeval _timestamp
Timestamp updated during mainloop()
virtual void mainloop()
Standard VRPN mainloop method.
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_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
virtual int encode_vel_to(char *buf)
vrpn_int32 num_sensors
struct timeval timestamp
vrpn_int32 position_m_id
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_NORMAL
@ vrpn_TEXT_WARNING
#define VRPN_SUPPRESS_EMPTY_OBJECT_WARNING()
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
struct hid_device_ hid_device
Header allowing use of a output stream-style method of sending text messages from devices.
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
header for OSVR Hacker Dev Kit