vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_OSVRHackerDevKit.C
Go to the documentation of this file.
1
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)
66 , _reportVersion(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)
79 , _reportVersion(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);
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);
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_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.
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...
virtual void reset()
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_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)
Definition: vrpn_Tracker.C:552
vrpn_float64 vel_quat[4]
Definition: vrpn_Tracker.h:96
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_int32 velocity_m_id
Definition: vrpn_Tracker.h:81
vrpn_float64 vel_quat_dt
Definition: vrpn_Tracker.h:97
virtual int encode_vel_to(char *buf)
Definition: vrpn_Tracker.C:577
vrpn_int32 num_sensors
Definition: vrpn_Tracker.h:114
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
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