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