22 #if defined(VRPN_USE_LIBUSB_1_0) 26 #define INCHES_TO_METERS (2.54f/100.0f) 32 long baud,
int enable_filtering,
int numstations,
33 int receptoridx,
const char *additional_reset_commands,
int whoamilen) :
35 do_filter(enable_filtering),
37 receptor_index(receptoridx),
40 sync_index(-1), read_len(0)
42 if (additional_reset_commands == NULL) {
48 if (
DEBUG) fprintf(stderr,
"[DEBUG] Constructed LibertyHS Object\n");
54 fprintf(stderr,
" interrupting continuous print output mode\n");
55 char pollCommand =
'P';
81 sprintf(outstring,
"O*,2,7,8,9,0\015");
83 sprintf(outstring,
"O%d,2,7,8,9,0\015", sensor+1);
84 int len = strlen(outstring);
87 if (
DEBUG) fprintf(stderr,
"[DEBUG]: %s \n",outstring);
89 #ifdef libusb_strerror 90 fprintf(stderr,
"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: %s\n",
91 libusb_strerror(static_cast<libusb_error>(ret)));
93 fprintf(stderr,
"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: code %d\n",
128 int ret = libusb_bulk_transfer(
_device_handle, LIBERTYHS_WRITE_EP | LIBUSB_ENDPOINT_OUT,
129 (vrpn_uint8*)data, len, &sent_len, 50);
132 fprintf(stderr,
"vrpn_Tracker_LibertyHS::write_usb_data(): LIBUSB ERROR '%i'\n",ret);
142 int ret = libusb_bulk_transfer(
_device_handle, LIBERTYHS_READ_EP | LIBUSB_ENDPOINT_IN,
143 (vrpn_uint8*)data, maxlen, &
read_len, timeout);
184 char outstring1[64],outstring2[64],outstring3[64],outstring4[64];
200 if (
DEBUG) fprintf(stderr,
"[DEBUG] Beginning Reset");
205 reset[resetLen++] =
'F';
206 reset[resetLen++] =
'0';
207 reset[resetLen++] = (char) (13);
211 reset[resetLen++] = (char) (25);
212 reset[resetLen++] = (char) (13);
215 reset[resetLen++] =
'P';
217 sprintf(errmsg,
"Resetting the tracker (attempt %d)",
num_resets);
219 for (i = 0; i < resetLen; i++) {
224 perror(
"Liberty: Failed writing to tracker");
234 fprintf(stderr,
"\n");
241 unsigned char scrap[80];
243 sprintf(errmsg,
"Got >=%d characters after reset",ret);
245 for (i = 0; i < ret; i++) {
246 if (isprint(scrap[i])) {
247 fprintf(stderr,
"%c",scrap[i]);
249 fprintf(stderr,
"[0x%02X]",scrap[i]);
252 fprintf(stderr,
"\n");
257 char statusCommand[2];
258 statusCommand[0]=(char)(22);
259 statusCommand[1]=(char)(13);
264 perror(
" LibertyHS write failed (WhoAmI command)");
274 fprintf(stderr,
" Got %d of %d characters for status\n",ret,
whoami_len);
277 if ( (statusmsg[0]!=
'0') ) {
280 statusmsg[ret] =
'\0';
282 fprintf(stderr,
" LibertyHS: status is (");
283 for (i = 0; i < ret; i++) {
284 if (isprint(statusmsg[i])) {
285 fprintf(stderr,
"%c",statusmsg[i]);
287 fprintf(stderr,
"[0x%02X]",statusmsg[i]);
290 fprintf(stderr,
"\n)\n");
291 VRPN_MSG_ERROR(
"Bad status report from LibertyHS, retrying reset");
315 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Enabling filtering\n");
320 perror(
" LibertyHS write position filter failed");
327 perror(
" LibertyHS write orientation filter failed");
332 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Disabling filtering\n");
337 perror(
" LibertyHS write position filter failed");
344 perror(
" LibertyHS write orientation filter failed");
361 int seconds_to_wait, count;
363 printf(
" LibertyHS writing extended reset commands...\n");
374 next_line = strtok(add_cmd_copy,
"\015");
376 while (next_line != NULL) {
377 if (next_line[0] ==
'*') {
378 seconds_to_wait = atoi(&next_line[1]);
379 fprintf(stderr,
" ...sleeping %d seconds\n",seconds_to_wait);
382 sprintf(string_to_send,
"%s\015", next_line);
383 fprintf(stderr,
" ...sending command: %s\n", string_to_send);
387 next_line = strtok(next_line+strlen(next_line)+1,
"\015");
397 sprintf(outstring1,
"@B0\r");
398 if (
write_usb_data(outstring1, strlen(outstring1)) == (
int)strlen(outstring1)) {
399 fprintf(stderr,
"\n Tracker USB buffering mode disabled\n");
405 sprintf(outstring2,
"U1\r");
406 if (
write_usb_data(outstring2, strlen(outstring2)) == (
int)strlen(outstring2)) {
407 fprintf(stderr,
" LibertyHS set to metric units\n");
411 fprintf(stderr,
" LibertyHS set to English units\n");
415 sprintf(outstring3,
"F1\r");
416 if (
write_usb_data(outstring3, strlen(outstring3)) == (
int)strlen(outstring3)) {
417 fprintf(stderr,
" LibertyHS set to binary mode\n\n");
423 fprintf(stderr,
"\nCould not launch the %i requested markers\n",
num_stations);
427 fprintf(stderr,
"\nAll %i markers are ready!\n",
num_stations);
431 sprintf(outstring4,
"C\r");
432 if (
write_usb_data(outstring4, strlen(outstring4)) != (
int)strlen(outstring4)) {
433 perror(
" LibertyHS write failed");
437 fprintf(stderr,
"\n LibertyHS set to continuous mode\n\n");
444 char clear_timestamp_cmd[] =
"Q0\r";
447 strlen(clear_timestamp_cmd)) != (
int)strlen(clear_timestamp_cmd)) {
488 unsigned char *bufptr;
502 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Missed First Sync Char, read_len = %i\n",
read_len);
520 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Getting Report - Got LU\n");
525 if (
DEBUG) fprintf(stderr,
"[DEBUGA] While syncing: Getting Report - Not LU, Got Character %c \n",
buffer[
sync_index]);
539 sprintf(errmsg,
"Bad sensor # (%d) in record, re-syncing",
d_sensor + 1);
562 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Don't have full report (%i of %i)\n",
578 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Got full report\n");
581 if (
DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have 'LU' at beginning");
589 VRPN_MSG_INFO(
"No space character at end of report, re-syncing\n");
590 if (
DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",
624 pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
625 pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
626 pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
630 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
631 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
632 d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
633 d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
642 struct timeval delta_time;
645 vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
648 delta_time.tv_sec = (long)(read_time / 1000);
649 vrpn_uint32 read_time_milliseconds = read_time - delta_time.tv_sec * 1000;
650 delta_time.tv_usec = (long)(read_time_milliseconds * 1000);
657 frame_count = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
704 mapmsg[0] = (char) (21);
706 mapmsg[2] = (char) (13);
712 perror(
" LibertyHS write failed");
728 int marker_found = 0;
729 unsigned char mask = (char) (1);
731 if (mask & markermap[10]) {
733 fprintf(stderr,
"Tracker #%i launched\n",bit);
744 fprintf(stderr,
"\nDetect and launch %i markers:\n\n",
num_stations);
751 if (marker_found > 0) {
752 fprintf(stderr,
"\nWARNING: %i markers are already launched! If you want to change markers,\n",marker_found);
753 fprintf(stderr,
" turn off the SEU (System Electronics Unit) and run this application again.\n");
761 launchmsg[2] = (char) (13);
767 fprintf(stderr,
"\n--> PLACE A NEW POWERED UP MARKER UNDER RECEPTOR #%i\n ",
receptor_index);
768 for (
int i = 10; i > 0 ; i--) {
769 fprintf(stderr,
"%i...",i);
774 fprintf(stderr,
" sending LAUNCH MARKER command\n DON'T MOVE THE TRACKER!\n\n");
778 perror(
"LibertyHS: Failed writing launch marker command to tracker");
void vrpn_SleepMsecs(double dMsecs)
#define VRPN_TRACKER_USB_BUF_SIZE
virtual void send_report(void)
Header containing macros formerly duplicated in a lot of implementation files.
struct timeval watchdog_timestamp
int set_sensor_output_format(int sensor=-1)
Augments the basic LibertyHS format.
int launch_markers()
Returns the number of detected and lauched trackers.
const int vrpn_LIBERTYHS_MAX_WHOAMI_LEN
Generic connection class not specific to the transport mechanism.
#define VRPN_MSG_WARNING(msg)
int write_usb_data(void *data, int len)
Writes len bytes from data buffer to USB device.
const int vrpn_TRACKER_FAIL
int report_length(int sensor)
Augments the basic LibertyHS report length.
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
const int vrpn_TRACKER_PARTIAL
struct libusb_device_handle * _device_handle
struct timeval liberty_zerotime
const int vrpn_TRACKER_SYNCING
const int vrpn_LIBERTYHS_MAX_MARKERMAP_LEN
void flush_usb_data()
Empties the USB read buffer.
const int vrpn_LIBERTYHS_MAX_STATIONS
#define VRPN_MSG_INFO(msg)
virtual void reset()
Reset the tracker.
vrpn_uint8 buffer[VRPN_TRACKER_USB_BUF_SIZE]
#define vrpn_gettimeofday
int read_usb_data(void *data, int maxlen, unsigned int timeout=50)
Reads at most maxlen bytes from USB device and copy them into data buffer. Returns the number of read...
~vrpn_Tracker_LibertyHS()
void print_latest_report(void)
vrpn_Tracker_LibertyHS(const char *name, vrpn_Connection *c, long baud=115200, int enable_filtering=1, int numstations=vrpn_LIBERTYHS_MAX_STATIONS, int receptoridx=1, const char *additional_reset_commands=NULL, int whoamilen=288)
The constructor is given the name of the tracker (the name of the sender it should use)...
int test_markers()
Launches num_stations markers using receptor receptor_index to detect them.
virtual int get_report(void)
Gets reports if some are available, returns 0 if not, 1 if complete report(s).
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
#define VRPN_MSG_ERROR(msg)