vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Tracker_isense.C
Go to the documentation of this file.
1
3
4#ifdef VRPN_INCLUDE_INTERSENSE
5#include <time.h>
6#include <math.h>
7#include <stdlib.h>
8#include <stdio.h>
9#include <string.h>
10#include <sys/types.h>
11#include <sys/stat.h>
12#include <fcntl.h>
13#include <ctype.h>
14
15#ifdef linux
16#include <termios.h>
17#endif
18
19#ifndef _WIN32
20#include <sys/ioctl.h>
21#include <sys/time.h>
22#include <unistd.h>
23#include <netinet/in.h>
24#endif
25
26#include "vrpn_Tracker.h"
27#include "vrpn_Serial.h"
28#include "vrpn_Shared.h"
29#include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
30#include "quat.h"
31#include "isense.c"
32
33#define MAX_TIME_INTERVAL (5000000) // max time between reports (usec)
34
36{
37 sprintf(msg, "Port%d (Intersense lib %g) (Firmware Rev %g)", m_TrackerInfo.Port, m_TrackerInfo.LibVersion, m_TrackerInfo.FirmwareRev);
38 switch(m_TrackerInfo.TrackerType) {
39 case ISD_NONE:
40 sprintf(msg, "%s (Unknown series:", msg);
41 break;
42 case ISD_PRECISION_SERIES:
43 sprintf(msg, "%s (Precision series:", msg);
44 break;
45 case ISD_INTERTRAX_SERIES:
46 sprintf(msg, "%s (InterTrax series:", msg);
47 break;
48 }
49 switch(m_TrackerInfo.TrackerModel) {
50 case ISD_UNKNOWN:
51 sprintf(msg, "%s Unknown model)", msg);
52 break;
53
54 case ISD_IS300:
55 sprintf(msg, "%s IS300)", msg);
56 break;
57
58 case ISD_IS600:
59 sprintf(msg, "%s IS600)", msg);
60 break;
61
62 case ISD_IS900:
63 sprintf(msg, "%s IS900)", msg);
64 break;
65
66 case ISD_IS1200:
67 sprintf(msg, "%s IS1200)", msg);
68 break;
69
70 case ISD_INTERTRAX:
71 sprintf(msg, "%s InterTrax)", msg);
72 break;
73
74 case ISD_INTERTRAX_2:
75 sprintf(msg, "%s InterTrax2)", msg);
76 break;
77
78 case ISD_INTERTRAX_LS:
79 sprintf(msg, "%s InterTrax LS)", msg);
80 break;
81
82 case ISD_INTERTRAX_LC:
83 sprintf(msg, "%s InterTrax LC)", msg);
84 break;
85
86 case ISD_ICUBE2:
87 sprintf(msg, "%s InertiaCube2)", msg);
88 break;
89
90 case ISD_ICUBE2_PRO:
91 sprintf(msg, "%s InertiaCube2 Pro)", msg);
92 break;
93
94 case ISD_ICUBE3:
95 sprintf(msg, "%s InertiaCube3)", msg);
96 break;
97 }
98 sprintf(msg, "%s\n", msg);
99}
100
103 int commPort, const char *additional_reset_commands,
104 int is900_timestamps, int reset_at_start) :
105vrpn_Tracker(name,c), do_is900_timestamps(is900_timestamps),
106m_reset_at_start(reset_at_start)
107{
108#ifdef VRPN_INCLUDE_INTERSENSE
109 char errStr[1024];
110 int i;
111
113
114 if (additional_reset_commands == NULL) {
115 sprintf(add_reset_cmd, "");
116 } else {
117 vrpn_strcpy(add_reset_cmd, additional_reset_commands);
118 }
119
120 // Initially, set to no buttons or analogs on the stations. The
121 // methods to add buttons and analogs must be called to add them.
122 for (i = 0; i < ISD_MAX_STATIONS; i++) {
123 is900_buttons[i] = NULL;
124 is900_analogs[i] = NULL;
125 }
126
127 m_CommPort = commPort;
128 m_Handle = ISD_OpenTracker(NULL, commPort, FALSE, FALSE);
129
130 if(m_Handle == -1)
131 {
132 sprintf(errStr,"Failed to open tracker '%s' on COM%d: ISLIB_OpenTracker returned -1",name,commPort);
134 return;
135 }
136
137 // ISD_TRACKER_INFO_TYPE trackerInfo;
138
139 ISD_GetTrackerConfig(m_Handle,&m_TrackerInfo,FALSE);
140
141 for (i = 0; i < ISD_MAX_STATIONS; i++) {
143 sprintf(errStr,"Failed to reset sensor %d on tracker '%s' on COM%d",i, name,commPort);
145 return;
146 }
147 }
148
149
150 //what is the update rate of this tracker?
151 //we might want to set the update rate of the mainloop to based on this value.
152 //for now we just print it out.
153 getTrackerInfo(errStr);
155 VRPN_MSG_INFO(errStr);
156 fprintf(stderr,errStr);
157
159#else
160 fprintf(stderr,"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
162#endif
163}
164
166{
167#ifdef VRPN_INCLUDE_INTERSENSE
168
169 int i;
170
171 ISD_CloseTracker(m_Handle);
172
173 // Delete any button and analog devices that were created
174 for (i = 0; i < ISD_MAX_STATIONS; i++) {
175 if (is900_buttons[i]) {
176 try {
177 delete is900_buttons[i];
178 } catch (...) {
179 fprintf(stderr, "vrpn_Tracker_InterSense::~vrpn_Tracker_InterSense(): delete failed\n");
180 return;
181 }
182 is900_buttons[i] = NULL;
183 }
184 if (is900_analogs[i]) {
185 try {
186 delete is900_analogs[i];
187 } catch (...) {
188 fprintf(stderr, "vrpn_Tracker_InterSense::~vrpn_Tracker_InterSense(): delete failed\n");
189 return;
190 }
191 is900_analogs[i] = NULL;
192 }
193 }
194#endif
195}
196
204
206{
207 char errStr[1024];
208
209 //Get the info about the station
210 ISD_GetStationConfig(m_Handle,&m_StationInfo[station],station+1,0);
211
212 //ISD_ResetHeading(m_Handle,station+1); //Not sure if this is needed
213 // nahon@virtools.com -
215 ISD_Boresight(m_Handle, station+1, true); // equivalent to ISD_ResetHeading for itrax, see isense.h
216
217
218 // First, try to set the orientation reporting format to quaternions if possible.
219 // But, some models of intersense trackers (like the intertrax series) will only report
220 // in euler angles
221
222 // Try to set the tracker to report in quaternion format
223 // (to avoid gimbal lock)
224 // nahon@virtools.com : Let's try, even if we are not a precision series
225 // It seems that this is OK with Intertrax2, what would happen to old Intertrax ?
226// if (m_TrackerInfo.TrackerType == ISD_PRECISION_SERIES &&
227 if (
228 m_StationInfo[station].AngleFormat == ISD_EULER)
229 {
230 m_StationInfo[station].AngleFormat = ISD_QUATERNION;
231 if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
232 {
233 sprintf(errStr,"Warning: Your tracker doesn't seem to support the quaternion format - couldn't set station config for Station%d. ",station+1);
235 VRPN_MSG_WARNING(errStr);
236
237 m_StationInfo[station].AngleFormat = ISD_EULER;
238 }
239 }
240
241 // do is900 special things
242 // IS900 states (timestamp, button, analog).
243 if(m_TrackerInfo.TrackerModel == ISD_IS900) {
245 m_StationInfo[station].TimeStamped = TRUE;
246 if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
247 {
248 sprintf(errStr,"Warning: Your tracker doesn't seem to support the IS900 timestamps - couldn't set station config for Station%d. ",station+1);
250 VRPN_MSG_WARNING(errStr);
251 m_StationInfo[station].TimeStamped = FALSE;
252 }
253 }
254
255 if (is900_buttons[station] || is900_analogs[station]) {
256 m_StationInfo[station].GetInputs = TRUE;
257 if(!ISD_SetStationConfig(m_Handle,&m_StationInfo[station],station+1,FALSE))
258 {
259 sprintf(errStr,"Warning: Your tracker doesn't seem to support the IS900 buttons/analogs - couldn't set station config for Station%d. ",station+1);
261 VRPN_MSG_WARNING(errStr);
262 m_StationInfo[station].GetInputs = FALSE;
263 }
264 }
265 }
266 return 0;
267}
268
270{
271#ifdef VRPN_INCLUDE_INTERSENSE
272 char errStr[1024];
273 int i;
274
275 m_Handle = ISD_OpenTracker(NULL,m_CommPort,FALSE,FALSE);
276
277 if(m_Handle == -1)
278 {
279 sprintf(errStr,"InterSense: Failed to open tracker '%s' on COM%d: ISD_OpenTracker returned -1",d_servicename,m_CommPort);
280 fprintf(stderr,errStr);
282 VRPN_MSG_ERROR(errStr);
283
285 }
286 else
287 {
288
289 //--------------------------------------------------------------------
290 // Now that the tracker has given a valid status report, set all of
291 // the parameters the way we want them. We rely on power-up setting
292 // based on the receiver select switches to turn on the receivers that
293 // the user wants.
294 //--------------------------------------------------------------------
295
296 // Set output format for each of the possible stations.
297
298 for (i = 0; i < ISD_MAX_STATIONS; i++) {
300 return;
301 }
302 }
303
304 // Send the additional reset commands, if any, to the tracker.
305 // These commands come in lines, with character \015 ending each
306 // line.
307 if (strlen(add_reset_cmd) > 0) {
308 printf(" Intersense writing extended reset commands...\n");
309 if(!ISD_SendScript(m_Handle,add_reset_cmd))
310 {
311 sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
313 VRPN_MSG_WARNING(errStr);
314 }
315 }
316
317 // If we are using the IS-900 timestamps, clear the timer on the device and
318 // store the time when we cleared it. First, drain any characters in the output
319 // buffer to ensure we're sending right away. Then, send the reset command and
320 // store the time that we sent it, plus the estimated time for the characters to
321 // get across the serial line to the device at the current baud rate.
322 // Set time units to milliseconds (MT) and reset the time (MZ).
324 char clear_timestamp_cmd[] = "MT\015MZ\015";
325 if(!ISD_SendScript(m_Handle,clear_timestamp_cmd))
326 {
327 sprintf(errStr,"Warning: Your tracker failed executing the additional command string. ");
329 VRPN_MSG_WARNING(errStr);
330 }
331
333 }
334
335 // Done with reset.
336 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
337 VRPN_MSG_WARNING("Reset Completed (this is good)");
338
339 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
340 }
341#else
342 fprintf(stderr,"Intersense library not compiled into this version. Use Fastrak driver for IS-600/900 or recompile with VRPN_INCLUDE_INTERSENSE defined\n");
344#endif
345}
346
347// This function will get the reports from the intersense dll, then
348// put that report into the time, sensor, pos and quat fields, and
349// finally call send_report to send it.
351{
352#ifdef VRPN_INCLUDE_INTERSENSE
353 q_vec_type angles;
354 ISD_TRACKING_DATA_TYPE data;
355 int i;
356
357 if(ISD_GetTrackingData(m_Handle,&data)) {
358 for(int station=0;station<ISD_MAX_STATIONS;station++) {
359 if(data.Station[station].NewData == TRUE) {
360
361 d_sensor = station;
362
363 //--------------------------------------------------------------------
364 // If we are doing IS900 timestamps, decode the time, add it to the
365 // time we zeroed the tracker, and update the report time. Remember
366 // to convert the MILLIseconds from the report into MICROseconds and
367 // seconds.
368 //--------------------------------------------------------------------
369
371 vrpn_float32 read_time = data.Station[station].TimeStamp;
372
373 struct timeval delta_time; // Time since the clock was reset
374
375 // Convert from the float in MILLIseconds to the struct timeval
376 delta_time.tv_sec = (long)(read_time / 1000); // Integer trunction to seconds
377 read_time -= delta_time.tv_sec * 1000; // Subtract out what we just counted
378 delta_time.tv_usec = (long)(read_time * 1000); // Convert remainder to MICROseconds
379
380 // Store the current time
382 } else {
383 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
384 }
385
386 //--------------------------------------------------------------------
387 // If this sensor has an IS900 button on it, decode
388 // the button values into the button device and mainloop the button
389 // device so that it will report any changes. Each button is stored
390 // in one bit of the byte, with the lowest-numbered button in the
391 // lowest bit.
392 //--------------------------------------------------------------------
393
394 if (is900_buttons[station]) {
395 for (i = 0; i < is900_buttons[station]->number_of_buttons(); i++) {
396 is900_buttons[station]->set_button(i, data.Station[station].ButtonState[i]);
397 }
398 is900_buttons[station]->mainloop();
399 }
400
401 //--------------------------------------------------------------------
402 // If this sensor has an IS900 analog on it, decode the analog values
403 // into the analog device and mainloop the analog device so that it
404 // will report any changes. The first byte holds the unsigned char
405 // representation of left/right. The second holds up/down. For each,
406 // 0 means min (left or rear), 127 means center and 255 means max.
407 //--------------------------------------------------------------------
408
409 if (is900_analogs[station]) {
410 // Normalize the values to the range -1 to 1
411 is900_analogs[station]->setChannelValue(0, (data.Station[station].AnalogData[0] - 127) / 128.0);
412 is900_analogs[station]->setChannelValue(1, (data.Station[station].AnalogData[1] - 127) / 128.0);
413
414 // Report the new values
415 is900_analogs[station]->report_changes();
416 is900_analogs[station]->mainloop();
417 }
418
419 // Copy the tracker data into our internal storage before sending
420 // (no unit problem as the Position vector is already in meters, see ISD_STATION_STATE_TYPE)
421 // Watch: For some reason, to get consistent rotation and translation axis permutations,
422 // we need non direct mapping.
423 // RMT: Based on a report from Christian Odom, it seems like the Quaternions in the
424 // Isense are QXYZ, whereas in Quatlib (and VRPN) they are XYZQ. Once these
425 // are switched correctly, the positions can be read without strange swapping.
426 pos[0] = data.Station[station].Position[0];
427 pos[1] = data.Station[station].Position[1];
428 pos[2] = data.Station[station].Position[2];
429
430 if(m_StationInfo[station].AngleFormat == ISD_QUATERNION) {
431 d_quat[0] = data.Station[station].Quaternion[1];
432 d_quat[1] = data.Station[station].Quaternion[2];
433 d_quat[2] = data.Station[station].Quaternion[3];
434 d_quat[3] = data.Station[station].Quaternion[0];
435 } else {
436 // Just return Euler for now...
437 // nahon@virtools needs to convert to radians
438 angles[0] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[0];
439 angles[1] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[1];
440 angles[2] = VRPN_DEGREES_TO_RADIANS*data.Station[station].Euler[2];
441
442 q_from_euler(d_quat, angles[0], angles[1], angles[2]);
443 }
444
445 // have to just send it now
447// fprintf(stderr, "sending message len %d\n", len);
448 send_report();
449
450 //printf("Isense %f, %f, %f\n",pos[0],pos[1],pos[2]);
451 //printf("Isense a:%f, %f, %f : ",angles[0],angles[1],angles[2]); //if the tracker reports a quat, these will be garbage
452 //printf("q: %f, %f, %f, %f\n",d_quat[0],d_quat[1],d_quat[2],d_quat[3]);
453 }
454 }
455
456 }
457#endif
458}
459
461{
462#ifdef VRPN_INCLUDE_INTERSENSE
463 // Send the message on the connection
464 if (d_connection)
465 {
466 char msgbuf[1000];
467 int len = encode_to(msgbuf);
468 if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf,
470 fprintf(stderr,"InterSense: cannot write message: tossing\n");
471 }
472 }
473#endif
474}
475
476
477
478// This function should be called each time through the main loop
479// of the server code. It polls for a report from the tracker and
480// sends it if there is one. It will reset the tracker if there is
481// no data from it for a few seconds.
482
484{
485 // Call the generic server mainloop, since we are a server
487
488 switch (status) {
492 {
493 // It turns out to be important to get the report before checking
494 // to see if it has been too long since the last report. This is
495 // because there is the possibility that some other device running
496 // in the same server may have taken a long time on its last pass
497 // through mainloop(). Trackers that are resetting do this. When
498 // this happens, you can get an infinite loop -- where one tracker
499 // resets and causes the other to timeout, and then it returns the
500 // favor. By checking for the report here, we reset the timestamp
501 // if there is a report ready (ie, if THIS device is still operating).
502
503 get_report();
504
505 // Ready for another report
507 }
508 break;
509
511 reset();
512 break;
513
515 VRPN_MSG_WARNING("Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
517 break;
518 }
519}
520
521
529
530int vrpn_Tracker_InterSense::add_is900_button(const char *button_device_name, int sensor, int numbuttons)
531{
532 // Make sure this is a valid sensor
533 if ( (sensor < 0) || (sensor >= ISD_MAX_STATIONS) ) {
534 return -1;
535 }
536
537 // Add a new button device and set the pointer to point at it.
538 try { is900_buttons[sensor] = new vrpn_Button_Server(button_device_name, d_connection, numbuttons); }
539 catch (...) {
540 VRPN_MSG_ERROR("Cannot open button device");
541 return -1;
542 }
543
544 // Send a new station-format command to the tracker so it will report the button states.
545 return set_sensor_output_format(sensor);
546}
547
548
562
563int vrpn_Tracker_InterSense::add_is900_analog(const char *analog_device_name, int sensor,
564 double c0Min, double c0Low, double c0Hi, double c0Max,
565 double c1Min, double c1Low, double c1Hi, double c1Max)
566{
567 // Make sure this is a valid sensor
568 if ( (sensor < 0) || (sensor >= ISD_MAX_STATIONS) ) {
569 return -1;
570 }
571
572 // Add a new analog device and set the pointer to point at it.
573 try { is900_analogs[sensor] = new vrpn_Clipping_Analog_Server(analog_device_name, d_connection); }
574 catch (...) {
575 VRPN_MSG_ERROR("Cannot open analog device");
576 return -1;
577 }
578
579 // Set the analog to have two channels, and set its channels to 0 to start with
580 is900_analogs[sensor]->setNumChannels(2);
581 is900_analogs[sensor]->setChannelValue(0, 0.0);
582 is900_analogs[sensor]->setChannelValue(1, 0.0);
583
584 // Set the scaling on the two channels.
585 is900_analogs[sensor]->setClipValues(0, c0Min, c0Low, c0Hi, c0Max);
586 is900_analogs[sensor]->setClipValues(1, c1Min, c1Low, c1Hi, c1Max);
587
588 // Send a new station-format command to the tracker so it will report the analog status
589 return set_sensor_output_format(sensor);
590}
591#endif
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
char * d_servicename
Name of this device, not including the connection part.
Generic connection class not specific to the transport mechanism.
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.
ISD_TRACKER_INFO_TYPE m_TrackerInfo
vrpn_Clipping_Analog_Server * is900_analogs[ISD_MAX_STATIONS]
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...
virtual void send_report(void)
int set_sensor_output_format(int sensor)
Augments the basic Fastrak format to include IS900 features if needed.
ISD_STATION_INFO_TYPE m_StationInfo[ISD_MAX_STATIONS]
virtual void get_report(void)
virtual void mainloop()
This function should be called each time through the main loop of the server code....
ISD_TRACKER_HANDLE m_Handle
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)
vrpn_Button_Server * is900_buttons[ISD_MAX_STATIONS]
virtual int encode_to(char *buf)
int register_server_handlers(void)
vrpn_float64 d_quat[4]
vrpn_Tracker(const char *name, vrpn_Connection *c=NULL, const char *tracker_cfg_file_name=NULL)
vrpn_int32 d_sensor
vrpn_float64 pos[3]
struct timeval timestamp
vrpn_int32 position_m_id
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
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)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition vrpn_Shared.C:54
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
Definition vrpn_Shared.h:99
#define VRPN_DEGREES_TO_RADIANS
Definition vrpn_Shared.h:15
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_REPORT_READY
const int vrpn_TRACKER_AWAITING_STATION
class VRPN_API vrpn_Clipping_Analog_Server
class VRPN_API vrpn_Button_Server