22 #define INCHES_TO_METERS (2.54/100.0) 27 const char *port,
long baud,
int enable_filtering,
int numstations,
28 const char *additional_reset_commands,
int whoamilen) :
30 do_filter(enable_filtering),
34 got_single_sync_char(0)
39 if (additional_reset_commands == NULL) {
51 if (
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 (
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);
243 if ( (statusmsg[0]!=
'0') ) {
246 statusmsg[ret] =
'\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");
261 printf(
"LIBERTY LATUS STATUS (whoami):\n%s\n\n",statusmsg);
286 if (
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 (
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");
343 add_cmd_copy[
sizeof(add_cmd_copy)-1] =
'\0';
351 next_line = strtok(add_cmd_copy,
"\015");
352 while (next_line != NULL) {
353 if (next_line[0] ==
'*') {
354 seconds_to_wait = atoi(&next_line[1]);
355 fprintf(stderr,
" ...sleeping %d seconds\n",seconds_to_wait);
358 sprintf(string_to_send,
"%s\015", next_line);
359 fprintf(stderr,
" ...sending command: %s\n", string_to_send);
361 (
const unsigned char *)string_to_send,strlen(string_to_send));
363 next_line = strtok(next_line+strlen(next_line)+1,
"\015");
372 sprintf(outstring1,
"F1\r");
374 strlen(outstring1)) == (
int)strlen(outstring1)) {
375 fprintf(stderr,
" Liberty set to binary mode\n");
380 sprintf(outstring3,
"C\r");
382 strlen(outstring3)) != (
int)strlen(outstring3)) {
383 perror(
" Liberty write failed");
387 fprintf(stderr,
" Liberty set to continuous mode\n");
397 char clear_timestamp_cmd[] =
"Q0\r";
402 strlen(clear_timestamp_cmd)) != (
int)strlen(clear_timestamp_cmd)) {
443 unsigned char *bufptr;
470 else if (ret != -1) {
471 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Missed Second Sync Char\n");
488 sprintf(errmsg,
"While syncing (looking for 'LY' or 'PA' or 'LU', " 492 if (
DEBUG) fprintf(stderr,
"[DEBUGA]: Getting Report - Not LY or PA or LU, Got Character %c %c \n",
buffer[0],
buffer[1]);
496 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Getting Report - Got LY or PA or LU\n");
523 if (
DEBUG) fprintf(stderr,
"[DEBUG]: Awaiting Station - Got Station (%i) \n",
buffer[2]);
528 sprintf(errmsg,
"Bad sensor # (%d) in record, re-syncing",
d_sensor);
557 if (
DEBUGA) fprintf(stderr,
"[DEBUG]: Error Reading Report\n");
585 if (
DEBUGA) fprintf(stderr,
"[DEBUG]: Don't have LY or PA or 'LU' at beginning");
587 VRPN_MSG_INFO(
"Not 'LY' or 'PA' or 'LU' in record, re-syncing");
594 VRPN_MSG_INFO(
"No space character at end of report, re-syncing\n");
604 if (
DEBUGA) fprintf(stderr,
"[DEBUG]:Error Flag %i\n",
buffer[4]);
631 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
632 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
633 d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
634 d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
643 struct timeval delta_time;
646 vrpn_uint32 read_time = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
649 delta_time.tv_sec = (long)(read_time / 1000);
650 vrpn_uint32 read_time_milliseconds = read_time_milliseconds = read_time - delta_time.tv_sec * 1000;
651 delta_time.tv_usec = (long)(read_time_milliseconds * 1000);
665 vrpn_uint32 button_status = vrpn_unbuffer_from_little_endian<vrpn_uint32>(bufptr);
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
vrpn_Button_Server * stylus_buttons[vrpn_LIBERTY_MAX_STATIONS]
void vrpn_SleepMsecs(double dMsecs)
class VRPN_API vrpn_Button_Server
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),...
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
struct timeval watchdog_timestamp
Generic connection class not specific to the transport mechanism.
#define VRPN_MSG_WARNING(msg)
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
const int vrpn_TRACKER_FAIL
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
struct timeval liberty_zerotime
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
const int vrpn_TRACKER_PARTIAL
vrpn_Connection * d_connection
Connection that this object talks to.
const int vrpn_TRACKER_SYNCING
#define VRPN_MSG_INFO(msg)
const int vrpn_LIBERTY_MAX_STATIONS
int add_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
Add a stylus (with button) to one of the sensors.
struct timeval reset_time
const int vrpn_LIBERTY_MAX_WHOAMI_LEN
const int vrpn_TRACKER_AWAITING_STATION
int report_length(int sensor)
Augments the basic Liberty report length.
#define vrpn_gettimeofday
void print_latest_report(void)
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
virtual void reset()
Reset the tracker.
#define VRPN_MSG_ERROR(msg)
int set_sensor_output_format(int sensor)
Augments the basic Liberty format.