22#define INCHES_TO_METERS (2.54/100.0)
23static const bool VRPN_LIBERTY_DEBUG =
false;
24static const bool VRPN_LIBERTY_DEBUGA =
false;
27 const char *port,
long baud,
int enable_filtering,
int numstations,
28 const char *additional_reset_commands,
int whoamilen) :
39 if (additional_reset_commands == NULL) {
51 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG] Constructed Liberty Object\n");
70 const char *timestring;
71 const char *buttonstring;
72 const char *analogstring;
87 sprintf(outstring,
"O%d,2,7%s%s%s,0\015", sensor+1, timestring,
88 buttonstring, analogstring);
90 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: %s \n",outstring);
92 strlen(outstring)) == (
int)strlen(outstring)) {
131 char outstring1[64],outstring3[64];
147 fprintf(stderr,
"[DEBUG] Beginning Reset");
151 reset[resetLen++] = (char) (13);
152 reset[resetLen++] =
'F';
153 reset[resetLen++] =
'0';
154 reset[resetLen++] = (char) (13);
168 reset[resetLen++] = (char) (25);
169 reset[resetLen++] = (char) (13);
171 reset[resetLen++] =
'P';
173 sprintf(errmsg,
"Resetting the tracker (attempt %d)",
num_resets);
175 for (i = 0; i < resetLen; i++) {
180 perror(
"Liberty: Failed writing to tracker");
193 fprintf(stderr,
"\n");
200 unsigned char scrap[80];
202 sprintf(errmsg,
"Got >=%d characters after reset",ret);
204 for (i = 0; i < ret; i++) {
205 if (isprint(scrap[i])) {
206 fprintf(stderr,
"%c",scrap[i]);
208 fprintf(stderr,
"[0x%02X]",scrap[i]);
211 fprintf(stderr,
"\n");
218 char statusCommand[2];
219 statusCommand[0]=(char)(22);
220 statusCommand[1]=(char)(13);
225 perror(
" Liberty write failed");
236 fprintf(stderr,
" Got %d of %d characters for status\n",ret,
whoami_len);
239 statusmsg[ret] =
'\0';
246 if ( (statusmsg[0]!=
'0') ) {
248 fprintf(stderr,
" Liberty: status is (");
249 for (i = 0; i < ret; i++) {
250 if (isprint(statusmsg[i])) {
251 fprintf(stderr,
"%c",statusmsg[i]);
253 fprintf(stderr,
"[0x%02X]",statusmsg[i]);
256 fprintf(stderr,
"\n)\n");
261printf(
"LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
286 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Enabling filtering\n");
289 (
const unsigned char *)
"X0.2,0.2,0.8,0.8\015", 17) == 17) {
292 perror(
" Liberty write position filter failed");
297 (
const unsigned char *)
"Y0.2,0.2,0.8,0.8\015", 17) == 17) {
300 perror(
" Liberty write orientation filter failed");
305 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Disabling filtering\n");
308 (
const unsigned char *)
"X0,1,0,0\015", 9) == 9) {
311 perror(
" Liberty write position filter failed");
316 (
const unsigned char *)
"Y0,1,0,0\015", 9) == 9) {
319 perror(
" Liberty write orientation filter failed");
339 printf(
" Liberty writing extended reset commands...\n");
350 next_line = strtok(add_cmd_copy,
"\015");
351 while (next_line != NULL) {
352 if (next_line[0] ==
'*') {
353 seconds_to_wait = atoi(&next_line[1]);
354 fprintf(stderr,
" ...sleeping %d seconds\n",seconds_to_wait);
357 sprintf(string_to_send,
"%s\015", next_line);
358 fprintf(stderr,
" ...sending command: %s\n", string_to_send);
360 (
const unsigned char *)string_to_send,strlen(string_to_send));
362 next_line = strtok(next_line+strlen(next_line)+1,
"\015");
371 sprintf(outstring1,
"F1\r");
373 strlen(outstring1)) == (
int)strlen(outstring1)) {
374 fprintf(stderr,
" Liberty set to binary mode\n");
379 sprintf(outstring3,
"C\r");
381 strlen(outstring3)) != (
int)strlen(outstring3)) {
382 perror(
" Liberty write failed");
386 fprintf(stderr,
" Liberty set to continuous mode\n");
396 char clear_timestamp_cmd[] =
"Q0\r";
401 strlen(clear_timestamp_cmd)) != (
int)strlen(clear_timestamp_cmd)) {
442 unsigned char *bufptr;
469 else if (ret != -1) {
470 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Missed Second Sync Char\n");
487 sprintf(errmsg,
"While syncing (looking for 'LY' or 'PA' or 'LU', "
491 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",
buffer[0],
buffer[1]);
495 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Getting Report - Got LY or PA or LU\n");
522 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Awaiting Station - Got Station (%i) \n",
buffer[2]);
527 sprintf(errmsg,
"Bad sensor # (%d) in record, re-syncing",
d_sensor);
556 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,
"[DEBUG]: Error Reading Report\n");
563 if (VRPN_LIBERTY_DEBUG) fprintf(stderr,
"[DEBUG]: Don't have full report (%i of %i)\n",
bufcount,
REPORT_LEN);
584 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
586 VRPN_MSG_INFO(
"Not 'LY' or 'PA' or 'LU' in record, re-syncing");
593 VRPN_MSG_INFO(
"No space character at end of report, re-syncing\n");
595 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have space at end of report, got (%c) sensor %i\n",
buffer[
bufcount-1],
d_sensor);
603 if (VRPN_LIBERTY_DEBUGA) fprintf(stderr,
"[DEBUG]:Error Flag %i\n",
buffer[4]);
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);
664 vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
vrpn_Connection * d_connection
Connection that this object talks to.
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_Tracker_Liberty(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS0", long baud=115200, int enable_filtering=1, int numstations=vrpn_LIBERTY_MAX_STATIONS, const char *additional_reset_commands=NULL, int whoamilen=195)
The constructor is given the name of the tracker (the name of the sender it should use),...
vrpn_Button_Server * stylus_buttons[vrpn_LIBERTY_MAX_STATIONS]
struct timeval liberty_zerotime
int report_length(int sensor)
Augments the basic Liberty report length.
struct timeval reset_time
int add_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
Add a stylus (with button) to one of the sensors.
int set_sensor_output_format(int sensor)
Augments the basic Liberty format.
virtual void reset()
Reset the tracker.
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
vrpn_Tracker_Serial(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=38400)
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)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
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
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_AWAITING_STATION
const int vrpn_LIBERTY_MAX_STATIONS
const int vrpn_LIBERTY_MAX_WHOAMI_LEN
class VRPN_API vrpn_Connection
class VRPN_API vrpn_Button_Server