45 static const vrpn_uint8 HYDRA_FEATURE_REPORT[] =
48 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x03, 0x00, 0x00, 0x00,
49 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
50 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
51 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
52 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
53 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
54 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
55 0x00, 0x00, 0x00, 0x00, 0x06, 0x00
57 static const int HYDRA_FEATURE_REPORT_LEN = 91;
60 static const vrpn_uint8 HYDRA_GAMEPAD_COMMAND[] =
63 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00,
64 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
65 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
66 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
67 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
68 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
69 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
70 0x00, 0x00, 0x00, 0x00, 0x05, 0x00
72 static const int HYDRA_GAMEPAD_COMMAND_LEN = 91;
76 static const unsigned long MAXIMUM_INITIAL_WAIT_USEC = 1000000L;
80 static const unsigned long MAXIMUM_WAIT_USEC = 5000000L;
99 d_my_interface = which_interface;
109 <<
"Got report on controller channel. This means that we need to swap channels. " 110 <<
"Swapping channels.";
112 MyInterface *t = d_hydra->_ctrl;
113 d_hydra->_ctrl = d_hydra->_data;
118 fprintf(stderr,
"Unexpected receipt of %d bytes on Hydra control interface!\n", static_cast<int>(bytes));
119 for (
size_t i = 0; i < bytes; ++i)
121 fprintf(stderr,
"%x ", buffer[i]);
123 fprintf(stderr,
"\n");
131 <<
"Got input report of " << bytes <<
" bytes, expected 52! Discarding, and re-connecting to Hydra." 133 <<
" Please make sure that you have completely quit the Hydra Configurator software and the Hydra system tray icon," 134 <<
" since this usually indicates that the Razer software has changed the Hydra's mode behind our back." 137 d_hydra->reconnect();
141 if (d_hydra->status < HYDRA_REPORTING)
145 <<
"Got first motion controller report! This means everything is working properly now. " 146 <<
"(Took " << d_hydra->_attempt <<
" attempt" << (d_hydra->_attempt > 1 ?
"s" :
"") <<
" to change modes.)";
147 d_hydra->status = HYDRA_REPORTING;
152 d_hydra->vrpn_Button::timestamp = d_hydra->_timestamp;
153 d_hydra->vrpn_Tracker::timestamp = d_hydra->_timestamp;
155 d_hydra->_report_for_sensor(0, buffer + 8, dt);
156 d_hydra->_report_for_sensor(1, buffer + 30, dt);
159 d_hydra->vrpn_Button::report_changes();
164 std::string getSerialNumber()
169 memset(buf, 0,
sizeof(buf));
173 return std::string(buf + 216, 17);
177 return "[FAILED TO GET FEATURE REPORT]";
182 return "[HYDRA CONTROL INTERFACE NOT CONNECTED]";
186 void setMotionControllerMode()
191 vrpn_uint8 buf[91] = {0};
196 void setGamepadMode()
212 void set_interface(
unsigned which_interface)
214 d_my_interface = which_interface;
218 unsigned d_my_interface;
226 , status(HYDRA_WAITING_FOR_CONNECT)
227 , _wasInGamepadMode(false)
229 , _docking_distance(0.1f)
250 _calibration_done[i] =
false;
254 memset(_old_position[i], 0,
sizeof(q_vec_type));
255 memset(_calibration_pose_conj[i], 0,
sizeof(q_type));
256 _calibration_pose_conj[i][Q_W] = 1.0;
262 if (status == HYDRA_REPORTING && _wasInGamepadMode)
265 <<
"Hydra was in gamepad mode when we started: switching back to gamepad mode.";
266 _ctrl->setGamepadMode();
281 if (_data->connected())
290 case HYDRA_WAITING_FOR_CONNECT:
291 _waiting_for_connect();
294 case HYDRA_LISTENING_AFTER_CONNECT:
295 _listening_after_connect();
298 case HYDRA_LISTENING_AFTER_SET_FEATURE:
299 _listening_after_set_feature();
302 case HYDRA_REPORTING:
311 status = HYDRA_WAITING_FOR_CONNECT;
316 _calibration_done[i] =
false;
321 return _ctrl->reconnect();
324 void vrpn_Tracker_RazerHydra::_waiting_for_connect()
326 if (status != HYDRA_WAITING_FOR_CONNECT)
328 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_waiting_for_connect(): bad status\n");
331 if (_data->connected() && _ctrl->connected())
335 status = HYDRA_LISTENING_AFTER_CONNECT;
337 send_text_message() <<
"Listening to see if device is in motion controller mode.";
342 _wasInGamepadMode =
false;
346 void vrpn_Tracker_RazerHydra::_listening_after_connect()
348 if (status != HYDRA_LISTENING_AFTER_CONNECT)
350 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_connect(): bad status\n");
353 if (!_data->connected() || !_ctrl->connected())
355 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_connect(): Data or control channel not connected\n");
362 _enter_motion_controller_mode();
366 void vrpn_Tracker_RazerHydra::_listening_after_set_feature()
368 if (status != HYDRA_LISTENING_AFTER_SET_FEATURE)
370 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_set_feature(): bad status\n");
373 if (!_data->connected() || !_ctrl->connected())
375 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_set_feature(): Data or control channel not connected\n");
383 <<
"Really sleepy device - won't start motion controller reports despite our earlier " 384 << _attempt <<
" attempt" << (_attempt > 1 ?
". " :
"s. ")
385 <<
" Will give it another try. " 386 <<
"If this doesn't work, unplug and replug device and restart the VRPN server.";
388 if ((_attempt % 2) == 0)
391 <<
"Switching control and data interface (mac can't tell the difference).";
392 MyInterface *t = _ctrl;
399 _enter_motion_controller_mode();
403 void vrpn_Tracker_RazerHydra::_enter_motion_controller_mode()
405 if ( (status != HYDRA_LISTENING_AFTER_CONNECT) &&
406 (status != HYDRA_LISTENING_AFTER_SET_FEATURE) )
408 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_enter_motion_controller_mode(): bad status\n");
411 if (!_data->connected())
413 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_enter_motion_controller_mode(): Control channel not connected\n");
418 _wasInGamepadMode =
true;
429 <<
"Hydra not in motion-controller mode - attempting to change modes. " 430 <<
"Please be sure that the left and right sensors are to the left and " 431 <<
"right sides of the base for automatic calibration to take place.";
434 _ctrl->setMotionControllerMode();
436 status = HYDRA_LISTENING_AFTER_SET_FEATURE;
440 void vrpn_Tracker_RazerHydra::_report_for_sensor(
int sensorNum, vrpn_uint8 * data,
double dt)
446 static const double MM_PER_METER = 0.001;
447 static const double SCALE_INT16_TO_FLOAT_PLUSMINUS_1 = 1.0 / 32768.0;
448 static const double SCALE_UINT8_TO_FLOAT_0_TO_1 = 1.0 / 255.0;
449 const int channelOffset = sensorNum * 3;
453 const int buttonOffset = sensorNum * 8;
460 pos[0] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
461 pos[1] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
462 pos[2] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
467 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
468 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
469 d_quat[Q_Y] = -vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
470 d_quat[Q_Z] = -vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
475 _docked[sensorNum] = q_vec_magnitude(
pos) < _docking_distance;
476 if(_docked[sensorNum])
478 _calibration_done[sensorNum] =
true;
482 q_invert(_calibration_pose_conj[sensorNum],
d_quat);
487 if(
pos[1] > 0 ||
pos[2] > 0)
488 _mirror[sensorNum] = -1;
490 _mirror[sensorNum] = 1;
493 q_vec_copy(tmp,
pos);
494 q_vec_scale(tmp, _mirror[sensorNum], tmp);
499 _sign_x[0] = (tmp[0] < 0) ? 1 : -1;
501 _sign_x[1] = (tmp[0] > 0) ? 1 : -1;
503 tmp[0] *= _sign_x[sensorNum];
505 q_vec_copy(_old_position[sensorNum], tmp);
508 if (_calibration_done[sensorNum])
513 q_mult(
d_quat,
d_quat, _calibration_pose_conj[sensorNum]);
516 pos[0] *= _sign_x[sensorNum];
519 q_vec_scale(
pos, _mirror[sensorNum],
pos);
521 if(!_docked[sensorNum])
524 q_vec_type v_direct, v_mirror, pos_inv;
525 q_vec_subtract(v_direct,
pos, _old_position[sensorNum]);
527 q_vec_invert(pos_inv,
pos);
528 q_vec_subtract(v_mirror, pos_inv, _old_position[sensorNum]);
530 double dist_direct = q_vec_magnitude(v_direct);
531 double dist_mirror = q_vec_magnitude(v_mirror);
535 if (dist_direct > dist_mirror)
544 q_vec_copy(
pos, pos_inv);
545 _mirror[sensorNum] *= -1;
551 q_vec_copy(_old_position[sensorNum],
pos);
557 vrpn_uint8 buttonBits = vrpn_unbuffer_from_little_endian<vrpn_uint8>(data);
560 buttons[0 + buttonOffset] = (buttonBits & 0x20) != 0;
563 buttons[1 + buttonOffset] = (buttonBits & 0x04) != 0;
564 buttons[2 + buttonOffset] = (buttonBits & 0x08) != 0;
565 buttons[3 + buttonOffset] = (buttonBits & 0x02) != 0;
566 buttons[4 + buttonOffset] = (buttonBits & 0x10) != 0;
569 buttons[5 + buttonOffset] = (buttonBits & 0x01) != 0;
572 buttons[6 + buttonOffset] = (buttonBits & 0x40) != 0;
580 channel[0 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
581 channel[1 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
584 channel[2 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_uint8>(data) * SCALE_UINT8_TO_FLOAT_0_TO_1;
595 fprintf(stderr,
"vrpn_Tracker_RazerHydra: cannot write message: tossing\n");
599 #endif // VRPN_USE_HID const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
const unsigned int HYDRA_PRODUCT
Device supporting the Razer Hydra game controller as a tracker, analog device, and button device,...
void vrpn_SleepMsecs(double dMsecs)
~vrpn_Tracker_RazerHydra()
int get_feature_report(size_t bytes, vrpn_uint8 *buffer)
Call this to get a feature report from the device - first byte must be Report ID (or 0x0 for devices ...
const unsigned int HYDRA_VENDOR
virtual void on_data_received(size_t bytes, vrpn_uint8 *buffer)=0
Derived class reimplements this callback.
vrpn_Tracker_RazerHydra(const char *name, vrpn_Connection *trackercon)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Accepts any device with the given vendor and product IDs.
Generic connection class not specific to the transport mechanism.
Accepts any device with a particular interface number. Best in conjunction with vrpn_HidBooleanAndAcc...
void send_feature_report(size_t bytes, const vrpn_uint8 *buffer)
Call this to send a feature report to the device - first byte must be Report ID (or 0x0 for devices w...
const unsigned int HYDRA_CONTROL_INTERFACE
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Accepts the Nth device matching a given acceptor.
vrpn_Connection * d_connection
Connection that this object talks to.
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...
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.
virtual int encode_to(char *buf)
Header allowing use of a output stream-style method of sending text messages from devices.
#define vrpn_gettimeofday
virtual void update()
Polls the device buffers and causes on_data_received callbacks if appropriate You NEED to call this f...
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Accepts only devices meeting two criteria. NOT SHORT-CIRCUIT. Another demonstration of acceptor compo...
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
const unsigned int HYDRA_INTERFACE
virtual bool connected() const
Returns true iff the last device I/O succeeded.
vrpn_HidInterface(vrpn_HidAcceptor *acceptor)
vrpn_float64 last[vrpn_CHANNEL_MAX]