4 #ifdef VRPN_INCLUDE_INTERSENSE 10 #include <sys/types.h> 20 #include <sys/ioctl.h> 23 #include <netinet/in.h> 33 #define MAX_TIME_INTERVAL (5000000) // max time between reports (usec) 34 #define INCHES_TO_METERS (2.54/100.0) 35 #define PI (3.14159265358979323846) 36 #define DEG_TO_RAD (PI/180.) 44 sprintf(msg,
"%s (Unknown series:", msg);
46 case ISD_PRECISION_SERIES:
47 sprintf(msg,
"%s (Precision series:", msg);
49 case ISD_INTERTRAX_SERIES:
50 sprintf(msg,
"%s (InterTrax series:", msg);
55 sprintf(msg,
"%s Unknown model)", msg);
59 sprintf(msg,
"%s IS300)", msg);
63 sprintf(msg,
"%s IS600)", msg);
67 sprintf(msg,
"%s IS900)", msg);
71 sprintf(msg,
"%s IS1200)", msg);
75 sprintf(msg,
"%s InterTrax)", msg);
79 sprintf(msg,
"%s InterTrax2)", msg);
82 case ISD_INTERTRAX_LS:
83 sprintf(msg,
"%s InterTrax LS)", msg);
86 case ISD_INTERTRAX_LC:
87 sprintf(msg,
"%s InterTrax LC)", msg);
91 sprintf(msg,
"%s InertiaCube2)", msg);
95 sprintf(msg,
"%s InertiaCube2 Pro)", msg);
99 sprintf(msg,
"%s InertiaCube3)", msg);
102 sprintf(msg,
"%s\n", msg);
107 int commPort,
const char *additional_reset_commands,
108 int is900_timestamps,
int reset_at_start) :
109 vrpn_Tracker(name,c), do_is900_timestamps(is900_timestamps),
110 m_reset_at_start(reset_at_start)
112 #ifdef VRPN_INCLUDE_INTERSENSE 118 if (additional_reset_commands == NULL) {
126 for (i = 0; i < ISD_MAX_STATIONS; i++) {
132 m_Handle = ISD_OpenTracker(NULL, commPort, FALSE, FALSE);
136 sprintf(errStr,
"Failed to open tracker '%s' on COM%d: ISLIB_OpenTracker returned -1",name,commPort);
145 for (i = 0; i < ISD_MAX_STATIONS; i++) {
147 sprintf(errStr,
"Failed to reset sensor %d on tracker '%s' on COM%d",i, name,commPort);
160 fprintf(stderr,errStr);
164 fprintf(stderr,
"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
171 #ifdef VRPN_INCLUDE_INTERSENSE 178 for (i = 0; i < ISD_MAX_STATIONS; i++) {
209 ISD_Boresight(
m_Handle, station+1,
true);
227 sprintf(errStr,
"Warning: Your tracker doesn't seem to support the quaternion format - couldn't set station config for Station%d. ",station+1);
242 sprintf(errStr,
"Warning: Your tracker doesn't seem to support the IS900 timestamps - couldn't set station config for Station%d. ",station+1);
253 sprintf(errStr,
"Warning: Your tracker doesn't seem to support the IS900 buttons/analogs - couldn't set station config for Station%d. ",station+1);
265 #ifdef VRPN_INCLUDE_INTERSENSE 273 sprintf(errStr,
"InterSense: Failed to open tracker '%s' on COM%d: ISD_OpenTracker returned -1",
d_servicename,
m_CommPort);
274 fprintf(stderr,errStr);
292 for (i = 0; i < ISD_MAX_STATIONS; i++) {
302 printf(
" Intersense writing extended reset commands...\n");
305 sprintf(errStr,
"Warning: Your tracker failed executing the additional command string. ");
318 char clear_timestamp_cmd[] =
"MT\015MZ\015";
319 if(!ISD_SendScript(
m_Handle,clear_timestamp_cmd))
321 sprintf(errStr,
"Warning: Your tracker failed executing the additional command string. ");
336 fprintf(stderr,
"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
346 #ifdef VRPN_INCLUDE_INTERSENSE 348 ISD_TRACKING_DATA_TYPE data;
351 if(ISD_GetTrackingData(
m_Handle,&data)) {
352 for(
int station=0;station<ISD_MAX_STATIONS;station++) {
353 if(data.Station[station].NewData == TRUE) {
365 vrpn_float32 read_time = data.Station[station].TimeStamp;
367 struct timeval delta_time;
370 delta_time.tv_sec = (long)(read_time / 1000);
371 read_time -= delta_time.tv_sec * 1000;
372 delta_time.tv_usec = (long)(read_time * 1000);
420 pos[0] = data.Station[station].Position[0];
421 pos[1] = data.Station[station].Position[1];
422 pos[2] = data.Station[station].Position[2];
425 d_quat[0] = data.Station[station].Quaternion[1];
426 d_quat[1] = data.Station[station].Quaternion[2];
427 d_quat[2] = data.Station[station].Quaternion[3];
428 d_quat[3] = data.Station[station].Quaternion[0];
432 angles[0] =
DEG_TO_RAD*data.Station[station].Euler[0];
433 angles[1] =
DEG_TO_RAD*data.Station[station].Euler[1];
434 angles[2] =
DEG_TO_RAD*data.Station[station].Euler[2];
436 q_from_euler(
d_quat, angles[0], angles[1], angles[2]);
456 #ifdef VRPN_INCLUDE_INTERSENSE 464 fprintf(stderr,
"InterSense: cannot write message: tossing\n");
509 VRPN_MSG_WARNING(
"Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
527 if ( (sensor < 0) || (sensor >= ISD_MAX_STATIONS) ) {
558 double c0Min,
double c0Low,
double c0Hi,
double c0Max,
559 double c1Min,
double c1Low,
double c1Hi,
double c1Max)
562 if ( (sensor < 0) || (sensor >= ISD_MAX_STATIONS) ) {
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
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...
const int vrpn_TRACKER_REPORT_READY
vrpn_Tracker_InterSense(const char *name, vrpn_Connection *c, int commPort, const char *additional_reset_commands=NULL, int is900_timestamps=0, int reset_at_start=0)
char * d_servicename
Name of this device, not including the connection part.
class VRPN_API vrpn_Button_Server
class VRPN_API vrpn_Clipping_Analog_Server
virtual void get_report(void)
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...
int register_server_handlers(void)
void getTrackerInfo(char *msg)
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...
vrpn_Clipping_Analog_Server * is900_analogs[ISD_MAX_STATIONS]
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.
ISD_TRACKER_INFO_TYPE m_TrackerInfo
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. This mainloop() only takes care of the things any server object should do.
#define VRPN_MSG_WARNING(msg)
const int vrpn_TRACKER_FAIL
ISD_STATION_INFO_TYPE m_StationInfo[ISD_MAX_STATIONS]
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_PARTIAL
ISD_TRACKER_HANDLE m_Handle
vrpn_Connection * d_connection
Connection that this object talks to.
virtual void send_report(void)
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
const int vrpn_TRACKER_SYNCING
#define VRPN_MSG_INFO(msg)
~vrpn_Tracker_InterSense()
virtual int encode_to(char *buf)
vrpn_Button_Server * is900_buttons[ISD_MAX_STATIONS]
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_int32 d_sender_id
Sender ID registered with the connection.
struct timeval is900_zerotime
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)
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)
virtual void mainloop()
This function should be called each time through the main loop of the server code. It polls for a report from the tracker and sends it if there is one. It will reset the tracker if there is no data from it for a few seconds.
int set_sensor_output_format(int sensor)
Augments the basic Fastrak format to include IS900 features if needed.