31 #define INCHES_TO_METERS (2.54/100.0) 34 const char *port,
long baud,
int enable_filtering,
int numstations,
35 const char *additional_reset_commands,
int is900_timestamps) :
37 do_filter(enable_filtering),
39 do_is900_timestamps(is900_timestamps),
45 if (additional_reset_commands == NULL) {
91 const char *timestring;
92 const char *buttonstring;
93 const char *analogstring;
112 sprintf(outstring,
"O%d,2,11%s%s%s,0\015", sensor+1, timestring,
113 buttonstring, analogstring);
115 strlen(outstring)) == (
int)strlen(outstring)) {
172 unsigned char reset[10];
192 reset[resetLen++] = (
unsigned char) (13);
195 reset[resetLen++] =
'Y';
198 reset[resetLen++] =
't';
210 reset[resetLen++] = (
unsigned char) (25);
212 reset[resetLen++] =
'c';
214 sprintf(errmsg,
"Resetting the tracker (attempt %d)",
num_resets);
216 for (i = 0; i < resetLen; i++) {
221 perror(
"Fastrak: 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");
260 perror(
" Fastrak write failed");
266 unsigned char statusmsg[56];
272 if ( (ret != 55) && (ret != 54) ) {
274 " Got %d of 55 characters for status (54 expected for IS900)\n",ret);
276 if ( (statusmsg[0]!=
'2') || (statusmsg[ret-1]!=(
char)(10)) ) {
278 statusmsg[55] =
'\0';
279 fprintf(stderr,
" Fastrak: status is (");
280 for (i = 0; i < ret; i++) {
281 if (isprint(statusmsg[i])) {
282 fprintf(stderr,
"%c",statusmsg[i]);
284 fprintf(stderr,
"[0x%02X]",statusmsg[i]);
287 fprintf(stderr,
"\n)\n");
312 sprintf(outstring,
"e1,0\r");
314 strlen(outstring)) == (
int)strlen(outstring)) {
329 (
const unsigned char *)
"x0.2,0.2,0.8,0.8\015", 17) == 17) {
332 perror(
" Fastrak write position filter failed");
337 (
const unsigned char *)
"v0.2,0.2,0.8,0.8\015", 17) == 17) {
340 perror(
" Fastrak write orientation filter failed");
359 printf(
" Fastrak writing extended reset commands...\n");
363 add_cmd_copy[
sizeof(add_cmd_copy)-1] =
'\0';
371 next_line = strtok(add_cmd_copy,
"\015");
372 while (next_line != NULL) {
373 if (next_line[0] ==
'*') {
374 seconds_to_wait = atoi(&next_line[1]);
375 fprintf(stderr,
" ...sleeping %d seconds\n",seconds_to_wait);
378 sprintf(string_to_send,
"%s\015", next_line);
379 fprintf(stderr,
" ...sending command: %s\n", string_to_send);
381 (
const unsigned char *)string_to_send,strlen(string_to_send));
383 next_line = strtok(next_line+strlen(next_line)+1,
"\015");
396 perror(
" Fastrak write failed");
400 fprintf(stderr,
" Fastrak set to continuous mode\n");
410 char clear_timestamp_cmd[] =
"MT\015MZ\015";
415 strlen(clear_timestamp_cmd)) != (
int)strlen(clear_timestamp_cmd)) {
458 unsigned char *bufptr;
476 sprintf(errmsg,
"While syncing (looking for '0', " 511 sprintf(errmsg,
"Bad sensor # (%d) in record, re-syncing",
d_sensor);
573 VRPN_MSG_INFO(
"No space character at end of report, re-syncing");
602 d_quat[Q_W] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
603 d_quat[Q_X] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
604 d_quat[Q_Y] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
605 d_quat[Q_Z] = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
615 struct timeval delta_time;
618 vrpn_float32 read_time = vrpn_unbuffer_from_little_endian<vrpn_float32>(bufptr);
621 delta_time.tv_sec = (long)(read_time / 1000);
622 read_time -= delta_time.tv_sec * 1000;
623 delta_time.tv_usec = (long)(read_time * 1000);
641 value=*(++bufptr)-
'0';
645 value = ( (*bufptr) >> i) & 1;
665 unsigned char raw_lr = *bufptr;
667 unsigned char raw_tb = *bufptr;
761 double c0Min,
double c0Low,
double c0Hi,
double c0Max,
762 double c1Min,
double c1Low,
double c1Hi,
double c1Max)
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
void vrpn_SleepMsecs(double dMsecs)
class VRPN_API vrpn_Button_Server
class VRPN_API vrpn_Clipping_Analog_Server
int add_is900_button(const char *button_device_name, int sensor, int numbuttons=5)
Add an IS900 button device to one of the sensors This allows configuration of an InterSense IS-900.
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...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Makes public the protected base class function.
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
virtual void reset()
Reset the tracker.
vrpn_Tracker_Fastrak(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int enable_filtering=1, int numstations=vrpn_FASTRAK_MAX_STATIONS, const char *additional_reset_commands=NULL, int is900_timestamps=0)
The constructor is given the name of the tracker (the name of the sender it should use),...
Generic connection class not specific to the transport mechanism.
virtual void mainloop()
For this server, the user must normally call report() or report_changes() directly....
struct timeval reset_time
#define VRPN_MSG_WARNING(msg)
int report_length(int sensor)
Augments the basic Fastrak report length to include IS900 features if needed.
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...
int set_sensor_output_format(int sensor)
Augments the basic Fastrak format to include IS900 features if needed.
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int add_is900_analog(const char *analog_device_name, int sensor, double c0Min=-1, double c0Low=0, double c0Hi=0, double c0Max=1, double c1Min=-1, double c1Low=0, double c1Hi=0, double c1Max=1)
Add the analog part of an IS900 joystick device to one of the sensors This allows configuration of an...
const int vrpn_TRACKER_PARTIAL
vrpn_Connection * d_connection
Connection that this object talks to.
const int vrpn_TRACKER_SYNCING
vrpn_Button_Server * is900_buttons[vrpn_FASTRAK_MAX_STATIONS]
#define VRPN_MSG_INFO(msg)
struct timeval is900_zerotime
int add_fastrak_stylus_button(const char *button_device_name, int sensor, int numbuttons=1)
const int vrpn_TRACKER_AWAITING_STATION
#define vrpn_gettimeofday
int setClipValues(int channel, double min, double lowzero, double highzero, double max)
Set the clipping values for the specified channel. min maps to -1, values between lowzero and highzer...
vrpn_Clipping_Analog_Server * is900_analogs[vrpn_FASTRAK_MAX_STATIONS]
void print_latest_report(void)
int setChannelValue(int channel, double value)
This method should be used to set the value of a channel. It will be scaled and clipped as described ...
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
const int vrpn_FASTRAK_MAX_STATIONS
vrpn_int32 setNumChannels(vrpn_int32 sizeRequested)
Sets the size of the array; returns the size actually set. (May be clamped to vrpn_CHANNEL_MAX) This ...
#define VRPN_MSG_ERROR(msg)