22#if defined(VRPN_USE_LIBUSB_1_0)
26static const bool VRPN_LIBERTYHS_METRIC_UNITS =
true;
27static const bool VRPN_LIBERTYHS_DEBUG =
false;
28static const bool VRPN_LIBERTYHS_DEBUGA =
false;
31 long baud,
int enable_filtering,
int numstations,
32 int receptoridx,
const char *additional_reset_commands,
int whoamilen) :
41 if (additional_reset_commands == NULL) {
47 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG] Constructed LibertyHS Object\n");
53 fprintf(stderr,
" interrupting continuous print output mode\n");
54 char pollCommand =
'P';
80 sprintf(outstring,
"O*,2,7,8,9,0\015");
82 sprintf(outstring,
"O%d,2,7,8,9,0\015", sensor+1);
83 int len = strlen(outstring);
86 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: %s \n",outstring);
89 fprintf(stderr,
"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: %s\n",
90 libusb_strerror(
static_cast<libusb_error
>(ret)));
92 fprintf(stderr,
"vrpn_Tracker_LibertyHS::libusb_bulk_transfer(): Could not send: code %d\n",
127 int ret = libusb_bulk_transfer(
_device_handle, LIBERTYHS_WRITE_EP | LIBUSB_ENDPOINT_OUT,
128 (vrpn_uint8*)data, len, &sent_len, 50);
131 fprintf(stderr,
"vrpn_Tracker_LibertyHS::write_usb_data(): LIBUSB ERROR '%i'\n",ret);
141 int ret = libusb_bulk_transfer(
_device_handle, LIBERTYHS_READ_EP | LIBUSB_ENDPOINT_IN,
142 (vrpn_uint8*)data, maxlen, &
read_len, timeout);
183 char outstring1[64],outstring2[64],outstring3[64],outstring4[64];
199 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG] Beginning Reset");
204 reset[resetLen++] =
'F';
205 reset[resetLen++] =
'0';
206 reset[resetLen++] = (char) (13);
210 reset[resetLen++] = (char) (25);
211 reset[resetLen++] = (char) (13);
214 reset[resetLen++] =
'P';
216 sprintf(errmsg,
"Resetting the tracker (attempt %d)",
num_resets);
218 for (i = 0; i < resetLen; i++) {
223 perror(
"Liberty: Failed writing to tracker");
233 fprintf(stderr,
"\n");
240 unsigned char scrap[80];
242 sprintf(errmsg,
"Got >=%d characters after reset",ret);
244 for (i = 0; i < ret; i++) {
245 if (isprint(scrap[i])) {
246 fprintf(stderr,
"%c",scrap[i]);
248 fprintf(stderr,
"[0x%02X]",scrap[i]);
251 fprintf(stderr,
"\n");
256 char statusCommand[2];
257 statusCommand[0]=(char)(22);
258 statusCommand[1]=(char)(13);
263 perror(
" LibertyHS write failed (WhoAmI command)");
273 fprintf(stderr,
" Got %d of %d characters for status\n",ret,
whoami_len);
276 if ( (statusmsg[0]!=
'0') ) {
279 statusmsg[ret] =
'\0';
281 fprintf(stderr,
" LibertyHS: status is (");
282 for (i = 0; i < ret; i++) {
283 if (isprint(statusmsg[i])) {
284 fprintf(stderr,
"%c",statusmsg[i]);
286 fprintf(stderr,
"[0x%02X]",statusmsg[i]);
289 fprintf(stderr,
"\n)\n");
290 VRPN_MSG_ERROR(
"Bad status report from LibertyHS, retrying reset");
314 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Enabling filtering\n");
316 if (
write_usb_data(
const_cast<char*
>(
"X0.2,0.2,0.8,0.8\015"), 17) == 17) {
319 perror(
" LibertyHS write position filter failed");
323 if (
write_usb_data(
const_cast<char*
>(
"Y0.2,0.2,0.8,0.8\015"), 17) == 17) {
326 perror(
" LibertyHS write orientation filter failed");
331 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Disabling filtering\n");
336 perror(
" LibertyHS write position filter failed");
343 perror(
" LibertyHS write orientation filter failed");
360 int seconds_to_wait, count;
362 printf(
" LibertyHS writing extended reset commands...\n");
373 next_line = strtok(add_cmd_copy,
"\015");
375 while (next_line != NULL) {
376 if (next_line[0] ==
'*') {
377 seconds_to_wait = atoi(&next_line[1]);
378 fprintf(stderr,
" ...sleeping %d seconds\n",seconds_to_wait);
381 sprintf(string_to_send,
"%s\015", next_line);
382 fprintf(stderr,
" ...sending command: %s\n", string_to_send);
386 next_line = strtok(next_line+strlen(next_line)+1,
"\015");
396 sprintf(outstring1,
"@B0\r");
397 if (
write_usb_data(outstring1, strlen(outstring1)) == (
int)strlen(outstring1)) {
398 fprintf(stderr,
"\n Tracker USB buffering mode disabled\n");
403 if (VRPN_LIBERTYHS_METRIC_UNITS) {
404 sprintf(outstring2,
"U1\r");
405 if (
write_usb_data(outstring2, strlen(outstring2)) == (
int)strlen(outstring2)) {
406 fprintf(stderr,
" LibertyHS set to metric units\n");
410 fprintf(stderr,
" LibertyHS set to English units\n");
414 sprintf(outstring3,
"F1\r");
415 if (
write_usb_data(outstring3, strlen(outstring3)) == (
int)strlen(outstring3)) {
416 fprintf(stderr,
" LibertyHS set to binary mode\n\n");
422 fprintf(stderr,
"\nCould not launch the %i requested markers\n",
num_stations);
426 fprintf(stderr,
"\nAll %i markers are ready!\n",
num_stations);
430 sprintf(outstring4,
"C\r");
431 if (
write_usb_data(outstring4, strlen(outstring4)) != (
int)strlen(outstring4)) {
432 perror(
" LibertyHS write failed");
436 fprintf(stderr,
"\n LibertyHS set to continuous mode\n\n");
443 char clear_timestamp_cmd[] =
"Q0\r";
446 strlen(clear_timestamp_cmd)) != (
int)strlen(clear_timestamp_cmd)) {
487 unsigned char *bufptr;
501 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Missed First Sync Char, read_len = %i\n",
read_len);
519 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Getting Report - Got LU\n");
524 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUGA] While syncing: Getting Report - Not LU, Got Character %c \n",
buffer[
sync_index]);
535 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Awaiting Station - Got Station (%i) \n",
buffer[
sync_index +
bufcount]);
538 sprintf(errmsg,
"Bad sensor # (%d) in record, re-syncing",
d_sensor + 1);
561 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Don't have full report (%i of %i)\n",
577 if (VRPN_LIBERTYHS_DEBUG) fprintf(stderr,
"[DEBUG]: Got full report\n");
580 if (VRPN_LIBERTYHS_DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have 'LU' at beginning");
588 VRPN_MSG_INFO(
"No space character at end of report, re-syncing\n");
589 if (VRPN_LIBERTYHS_DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",
600 if (VRPN_LIBERTYHS_DEBUGA) fprintf(stderr,
"[DEBUG]:Error Flag %i\n",
buffer[
sync_index + 4]);
623 pos[0] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
624 pos[1] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
625 pos[2] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr) * convFactor;
629 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
630 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
631 d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
632 d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
641 struct timeval delta_time;
644 vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
647 delta_time.tv_sec = (long)(read_time / 1000);
648 vrpn_uint32 read_time_milliseconds = read_time - delta_time.tv_sec * 1000;
649 delta_time.tv_usec = (long)(read_time_milliseconds * 1000);
656 frame_count = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
703 mapmsg[0] = (char) (21);
705 mapmsg[2] = (char) (13);
711 perror(
" LibertyHS write failed");
727 int marker_found = 0;
728 unsigned char mask = (char) (1);
730 if (mask & markermap[10]) {
732 fprintf(stderr,
"Tracker #%i launched\n",bit);
743 fprintf(stderr,
"\nDetect and launch %i markers:\n\n",
num_stations);
750 if (marker_found > 0) {
751 fprintf(stderr,
"\nWARNING: %i markers are already launched! If you want to change markers,\n",marker_found);
752 fprintf(stderr,
" turn off the SEU (System Electronics Unit) and run this application again.\n");
760 launchmsg[2] = (char) (13);
766 fprintf(stderr,
"\n--> PLACE A NEW POWERED UP MARKER UNDER RECEPTOR #%i\n ",
receptor_index);
767 for (
int i = 10; i > 0 ; i--) {
768 fprintf(stderr,
"%i...",i);
773 fprintf(stderr,
" sending LAUNCH MARKER command\n DON'T MOVE THE TRACKER!\n\n");
777 perror(
"LibertyHS: Failed writing launch marker command to tracker");
struct timeval liberty_zerotime
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 write_usb_data(void *data, int len)
Writes len bytes from data buffer to USB device.
int launch_markers()
Returns the number of detected and lauched trackers.
int set_sensor_output_format(int sensor=-1)
Augments the basic LibertyHS format.
~vrpn_Tracker_LibertyHS()
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...
virtual void reset()
Reset the tracker.
int report_length(int sensor)
Augments the basic LibertyHS report length.
void flush_usb_data()
Empties the USB read buffer.
virtual int get_report(void)
Gets reports if some are available, returns 0 if not, 1 if complete report(s).
int test_markers()
Launches num_stations markers using receptor receptor_index to detect them.
vrpn_Tracker_USB(const char *name, vrpn_Connection *c, vrpn_uint16 vendor, vrpn_uint16 product, long baud=115200)
vrpn_uint8 buffer[VRPN_TRACKER_USB_BUF_SIZE]
virtual void send_report(void)
struct libusb_device_handle * _device_handle
struct timeval watchdog_timestamp
void print_latest_report(void)
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
Header containing macros formerly duplicated in a lot of implementation files.
#define VRPN_MSG_INFO(msg)
#define VRPN_MSG_ERROR(msg)
#define VRPN_MSG_WARNING(msg)
void vrpn_SleepMsecs(double dMilliSecs)
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
void vrpn_strcpy(char(&to)[charCount], const char *pSrc)
Null-terminated-string copy function that both guarantees not to overrun the buffer and guarantees th...
#define vrpn_gettimeofday
#define VRPN_INCHES_TO_METERS
#define VRPN_TRACKER_USB_BUF_SIZE
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_LIBERTYHS_MAX_STATIONS
const int vrpn_LIBERTYHS_MAX_MARKERMAP_LEN
const int vrpn_LIBERTYHS_MAX_WHOAMI_LEN
class VRPN_API vrpn_Connection