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."
186 d_hydra->reconnect();
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_Analog(const char *name, vrpn_Connection *c=NULL)
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 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 ...
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...
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.
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)
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
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