46static const vrpn_uint8 HYDRA_FEATURE_REPORT[] =
49 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 0x03, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
56 0x00, 0x00, 0x00, 0x00, 0x06, 0x00
58static const int HYDRA_FEATURE_REPORT_LEN = 91;
61static const vrpn_uint8 HYDRA_GAMEPAD_COMMAND[] =
64 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x04, 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
71 0x00, 0x00, 0x00, 0x00, 0x05, 0x00
73static const int HYDRA_GAMEPAD_COMMAND_LEN = 91;
77static const unsigned long MAXIMUM_INITIAL_WAIT_USEC = 1000000L;
81static const unsigned long MAXIMUM_WAIT_USEC = 5000000L;
84#define VRPN_HAVE_RELIABLE_INTERFACE_NUMBER
87static vrpn_HidAcceptor * makeHydraInterfaceAcceptor(
unsigned whichInterface) {
92#ifdef VRPN_HAVE_RELIABLE_INTERFACE_NUMBER
98 interfaceAcceptor.release(), productAcceptor.release()));
108 productAcceptor.release()));
110 }
catch (...) {
return NULL; }
121 d_my_interface = which_interface;
129 d_my_interface = which_interface;
138 static MyInterface *make(
unsigned which_interface,
143 try { ret =
new MyInterface(which_interface, &hydra, dev); }
144 catch (...) {
return NULL; }
147 static MyInterface *make(
unsigned which_interface,
152 try { ret =
new MyInterface(which_interface, &hydra, path); }
153 catch (...) {
return NULL; }
157 void on_data_received(
size_t bytes, vrpn_uint8 *buffer)
161#ifdef VRPN_HAVE_RELIABLE_INTERFACE_NUMBER
162 fprintf(stderr,
"Unexpected receipt of %d bytes on Hydra control interface!\n",
static_cast<int>(bytes));
163 for (
size_t i = 0; i < bytes; ++i)
165 fprintf(stderr,
"%x ", buffer[i]);
167 fprintf(stderr,
"\n");
170 <<
"Got report on controller channel. This means that we need to swap channels. "
171 <<
"Swapping channels.";
172 d_hydra->_swap_channels();
180 <<
"Got input report of " << bytes <<
" bytes, expected 52! Discarding, and re-connecting to Hydra."
182 <<
" Please make sure that you have completely quit the Hydra Configurator software and the Hydra system tray icon,"
183 <<
" since this usually indicates that the Razer software has changed the Hydra's mode behind our back."
190 if (d_hydra->status < HYDRA_REPORTING)
194 <<
"Got first motion controller report! This means everything is working properly now. "
195 <<
"(Took " << d_hydra->_attempt <<
" attempt" << (d_hydra->_attempt > 1 ?
"s" :
"") <<
" to change modes.)";
196 d_hydra->status = HYDRA_REPORTING;
201 d_hydra->vrpn_Button::timestamp = d_hydra->_timestamp;
202 d_hydra->vrpn_Tracker::timestamp = d_hydra->_timestamp;
204 d_hydra->_report_for_sensor(0, buffer + 8, dt);
205 d_hydra->_report_for_sensor(1, buffer + 30, dt);
208 d_hydra->vrpn_Button::report_changes();
213 std::string getSerialNumber()
218 memset(buf, 0,
sizeof(buf));
222 return std::string(buf + 216, 17);
226 return "[FAILED TO GET FEATURE REPORT]";
231 return "[HYDRA CONTROL INTERFACE NOT CONNECTED]";
235 void setMotionControllerMode()
240 vrpn_uint8 buf[91] = {0};
245 void setGamepadMode()
261 void set_interface(
unsigned which_interface)
263 d_my_interface = which_interface;
266 void reset_acceptor()
273 unsigned d_my_interface;
282 , status(HYDRA_WAITING_FOR_CONNECT)
284 , _wasInGamepadMode(false)
286 , _docking_distance(0.1f)
301 , status(HYDRA_WAITING_FOR_CONNECT)
303 , _wasInGamepadMode(false)
305 , _docking_distance(0.1f)
314 const char *ctrl_dev_path,
315 const char *data_dev_path,
320 , status(HYDRA_WAITING_FOR_CONNECT)
322 , _wasInGamepadMode(false)
324 , _docking_distance(0.1f)
333void vrpn_Tracker_RazerHydra::_shared_init() {
349 _calibration_done[i] =
false;
353 memset(_old_position[i], 0,
sizeof(q_vec_type));
354 memset(_calibration_pose_conj[i], 0,
sizeof(q_type));
355 _calibration_pose_conj[i][Q_W] = 1.0;
361 if (status == HYDRA_REPORTING && _wasInGamepadMode)
364 <<
"Hydra was in gamepad mode when we started: switching back to gamepad mode.";
365 _ctrl->setGamepadMode();
378 if (!_data->connected()) {
390 case HYDRA_WAITING_FOR_CONNECT:
391 _waiting_for_connect();
394 case HYDRA_LISTENING_AFTER_CONNECT:
395 _listening_after_connect();
398 case HYDRA_LISTENING_AFTER_SET_FEATURE:
399 _listening_after_set_feature();
402 case HYDRA_REPORTING:
410 status = HYDRA_WAITING_FOR_CONNECT;
415 _calibration_done[i] =
false;
419 _data->reset_acceptor();
421 _ctrl->reset_acceptor();
422 return _ctrl->reconnect();
427void vrpn_Tracker_RazerHydra::_swap_channels() {
433void vrpn_Tracker_RazerHydra::_waiting_for_connect()
435 if (status != HYDRA_WAITING_FOR_CONNECT)
437 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_waiting_for_connect(): bad status\n");
440 if (_data->connected() && _ctrl->connected())
444 status = HYDRA_LISTENING_AFTER_CONNECT;
446 send_text_message() <<
"Listening to see if device is in motion controller mode.";
451 _wasInGamepadMode =
false;
455void vrpn_Tracker_RazerHydra::_listening_after_connect()
457 if (status != HYDRA_LISTENING_AFTER_CONNECT)
459 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_connect(): bad status\n");
462 if (!_data->connected() || !_ctrl->connected())
464 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_connect(): Data or control channel not connected\n");
471 _enter_motion_controller_mode();
475void vrpn_Tracker_RazerHydra::_listening_after_set_feature()
477 if (status != HYDRA_LISTENING_AFTER_SET_FEATURE)
479 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_set_feature(): bad status\n");
482 if (!_data->connected() || !_ctrl->connected())
484 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_listening_after_set_feature(): Data or control channel not connected\n");
492 <<
"Really sleepy device - won't start motion controller reports despite our earlier "
493 << _attempt <<
" attempt" << (_attempt > 1 ?
". " :
"s. ")
494 <<
" Will give it another try. "
495 <<
"If this doesn't work, unplug and replug device and restart the VRPN server.";
496#ifndef VRPN_HAVE_RELIABLE_INTERFACE_NUMBER
497 if ((_attempt % 2) == 0)
500 <<
"Switching control and data interface (some systems can't "
501 "tell the difference) and trying again to wake it.";
505 _enter_motion_controller_mode();
509void vrpn_Tracker_RazerHydra::_enter_motion_controller_mode()
511 if ( (status != HYDRA_LISTENING_AFTER_CONNECT) &&
512 (status != HYDRA_LISTENING_AFTER_SET_FEATURE) )
514 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_enter_motion_controller_mode(): bad status\n");
517 if (!_data->connected())
519 fprintf(stderr,
"vrpn_Tracker_RazerHydra::_enter_motion_controller_mode(): Control channel not connected\n");
524 _wasInGamepadMode =
true;
535 <<
"Hydra not in motion-controller mode - attempting to change modes. "
536 <<
"Please be sure that the left and right sensors are to the left and "
537 <<
"right sides of the base for automatic calibration to take place.";
540 _ctrl->setMotionControllerMode();
542 status = HYDRA_LISTENING_AFTER_SET_FEATURE;
546void vrpn_Tracker_RazerHydra::_report_for_sensor(
int sensorNum, vrpn_uint8 * data,
double )
551 static const double MM_PER_METER = 0.001;
552 static const double SCALE_INT16_TO_FLOAT_PLUSMINUS_1 = 1.0 / 32768.0;
553 static const double SCALE_UINT8_TO_FLOAT_0_TO_1 = 1.0 / 255.0;
554 const int channelOffset = sensorNum * 3;
558 const int buttonOffset = sensorNum * 8;
565 pos[0] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
566 pos[1] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
567 pos[2] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * MM_PER_METER;
572 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
573 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
574 d_quat[Q_Y] = -vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
575 d_quat[Q_Z] = -vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
580 _docked[sensorNum] = q_vec_magnitude(
pos) < _docking_distance;
581 if(_docked[sensorNum]) {
582 _calibration_done[sensorNum] =
true;
586 q_invert(_calibration_pose_conj[sensorNum],
d_quat);
591 if(
pos[1] > 0 ||
pos[2] > 0) {
592 _mirror[sensorNum] = -1;
594 _mirror[sensorNum] = 1;
598 q_vec_copy(tmp,
pos);
599 q_vec_scale(tmp, _mirror[sensorNum], tmp);
604 _sign_x[0] = (tmp[0] < 0) ? 1 : -1;
606 _sign_x[1] = (tmp[0] > 0) ? 1 : -1;
608 tmp[0] *= _sign_x[sensorNum];
610 q_vec_copy(_old_position[sensorNum], tmp);
613 if (_calibration_done[sensorNum]) {
617 q_mult(
d_quat,
d_quat, _calibration_pose_conj[sensorNum]);
620 pos[0] *= _sign_x[sensorNum];
623 q_vec_scale(
pos, _mirror[sensorNum],
pos);
625 if(!_docked[sensorNum]) {
627 q_vec_type v_direct, v_mirror, pos_inv;
628 q_vec_subtract(v_direct,
pos, _old_position[sensorNum]);
630 q_vec_invert(pos_inv,
pos);
631 q_vec_subtract(v_mirror, pos_inv, _old_position[sensorNum]);
633 double dist_direct = q_vec_magnitude(v_direct);
634 double dist_mirror = q_vec_magnitude(v_mirror);
638 if (dist_direct > dist_mirror) {
646 q_vec_copy(
pos, pos_inv);
647 _mirror[sensorNum] *= -1;
653 q_vec_copy(_old_position[sensorNum],
pos);
659 vrpn_uint8 buttonBits = vrpn_unbuffer_from_little_endian<vrpn_uint8>(data);
662 buttons[0 + buttonOffset] = (buttonBits & 0x20) != 0;
665 buttons[1 + buttonOffset] = (buttonBits & 0x04) != 0;
666 buttons[2 + buttonOffset] = (buttonBits & 0x08) != 0;
667 buttons[3 + buttonOffset] = (buttonBits & 0x02) != 0;
668 buttons[4 + buttonOffset] = (buttonBits & 0x10) != 0;
671 buttons[5 + buttonOffset] = (buttonBits & 0x01) != 0;
674 buttons[6 + buttonOffset] = (buttonBits & 0x40) != 0;
681 channel[0 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
682 channel[1 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_int16>(data) * SCALE_INT16_TO_FLOAT_PLUSMINUS_1;
685 channel[2 + channelOffset] = vrpn_unbuffer_from_little_endian<vrpn_uint8>(data) * SCALE_UINT8_TO_FLOAT_0_TO_1;
696 fprintf(stderr,
"vrpn_Tracker_RazerHydra: cannot write message: tossing\n");
A unique-ownership smart pointer, with the ability to transfer ownership, but only explicitly (aka,...
void reset(pointer p=pointer())
Deletes the owned object, if any, and takes ownership of a new object, if one is passed.
pointer release()
Returns the held pointer without performing the delete action.
vrpn_float64 last[vrpn_CHANNEL_MAX]
vrpn_float64 channel[vrpn_CHANNEL_MAX]
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...
Accepts only devices meeting two criteria. NOT SHORT-CIRCUIT. Another demonstration of acceptor compo...
Accepts any device with a particular interface number. Best in conjunction with vrpn_HidBooleanAndAcc...
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 ...
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...
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 the Nth device matching a given acceptor.
Accepts any device with the given vendor and product IDs.
Device supporting the Razer Hydra game controller as a tracker, analog device, and button device,...
virtual void mainloop()
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
vrpn_Tracker_RazerHydra(const char *name, vrpn_Connection *con=NULL)
~vrpn_Tracker_RazerHydra()
virtual int encode_to(char *buf)
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.
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
const unsigned int HYDRA_PRODUCT
const unsigned int HYDRA_VENDOR
const unsigned int HYDRA_CONTROL_INTERFACE
const unsigned int HYDRA_INTERFACE