vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_TrivisioColibri.C
Go to the documentation of this file.
1 //
3 // Name: vrpn_Tracker_TrivisioColibri.C
4 //
5 // Author: David Borland
6 // Institut d'Investigacions Biomèdiques August Pi i Sunyer (IDIBAPS)
7 // Virtual Embodiment and Robotic Re-Embodiment (VERE) Project – 257695
8 //
9 // Description: VRPN tracker class for Trivisio Colibri device.
10 //
12 
14 
15 #ifdef VRPN_USE_TRIVISIOCOLIBRI
16 
17 // XXX: Some horrible hackery here...
18 //
19 // The TravisioTypes.h header file has an #if WIN32 test. This is problematic, because
20 // WIN32 is not guaranteed to be defined to a value of 1 on Windows. Thus, this test can fail
21 // inadvertently. For example, #define WIN32 or #define WIN32 WIN32 will both produce
22 // unwanted results, but for different reasons. #define WIN32 will cause the #if WIN32
23 // test to fail because WIN32 has not been assigned a value. #define WIN32 WIN32 will cause
24 // #if WIN32 to produce a compiler error because WIN32 does not have an integer value.
25 //
26 // The correct thing to do would be to fix TrivisioTypes.h to use #ifdef _WIN32, as _WIN32 is
27 // always defined on Windows. However, so that users don't have to manually edit the
28 // Trivisio SDK source files, we redefine WIN32 to have a value of 1, include the Trivisio
29 // headers, and then redefine WIN32 with no value, which is what windows.h does...
30 //
31 #ifdef _WIN32
32 # ifdef WIN32
33 # undef WIN32
34 # endif
35 # define WIN32 1
36 # include VRPN_TRIVISIOCOLIBRI_H
37 # undef WIN32
38 # define WIN32
39 #else
40 # include VRPN_TRIVISIOCOLIBRI_H
41 #endif
42 
44  int numSensors, int Hz, int bufLen) :
45  vrpn_Tracker(name, c)
46 {
47  // Query the number of connected devices
48  struct TrivisioSensor* sensorList = new TrivisioSensor[numSensors];
49  int sensorCount = colibriGetDeviceList(sensorList, numSensors);
50 
51  // If sensorCount == -1, there are more sensors connected than we specified, which is fine.
52  // Else, sensorCount == the number of sensors connected, which is all we can use.
53  if (sensorCount < 0) {
54  num_sensors = numSensors;
55  }
56  else {
57  num_sensors = sensorCount;
58  }
59 
60 
61  // Print info
62  if (num_sensors < 1) {
63  printf("Warning: No Colibri sensors found\n");
65  return;
66  }
67 
68  printf("Using %d Colibri sensors\n", num_sensors);
69  for (int i = 0; i < num_sensors; i++) {
70  printf("%s:\t %s (FW %d.%d)\n", sensorList[i].dev, sensorList[i].ID,
71  sensorList[i].FWver, sensorList[i].FWsubver);
72  }
73  printf("\n\n");
74 
75 
76  // From the sample code:
77  //
78  // Diagonal matrices with diagonal element .68 yields approx 20Hz
79  // bandwidth @ 100Hz
80  //
81  float Ka[9] = { 0.68f, 0.00f, 0.00f,
82  0.00f, 0.68f, 0.00f,
83  0.00f, 0.00f, 0.68f };
84  float Kg[9] = { 0.68f, 0.00f, 0.00f,
85  0.00f, 0.68f, 0.00f,
86  0.00f, 0.00f, 0.68f };
87 
88 
89  // Create the array of device handles
90  imu = new void*[num_sensors];
91 
92  // Create device handles and configure them
93  for (int i = 0; i < num_sensors; i++) {
94  imu[i] = colibriCreate(bufLen);
95 
96  if (colibriOpen(imu[i], 0, sensorList[i].dev) < 0) {
97  printf("Warning: Could not access Colibri device on %s\n", sensorList[i]);
98  continue;
99  }
100 
101  struct ColibriConfig conf;
102 
103  // Taken from sample code
104  colibriGetConfig(imu[i], &conf);
105  conf.raw = 0;
106  conf.freq = Hz;
107  conf.sensor = (ColibriConfig::Sensor)1023;
108  conf.ascii = 0;
109  colibriSetConfig(imu[i], &conf);
110 
111  colibriSetKa(imu[i], Ka);
112  colibriSetKaStatus(imu[i], 1);
113  colibriSetKg(imu[i], Kg);
114  colibriSetKgStatus(imu[i], 1);
115  colibriSetJitterStatus(imu[i], 1);
116 
117  // Print info
118  char id[8];
119 
120  printf("Colibri IMU %d\n", i);
121  colibriGetID(imu, id);
122  printf("\tDevice ID: %s\n", id);
123  printf("\tSensor config: %d\n", conf.sensor);
124  printf("\tMagnetic div: %d\n", (unsigned)conf.magDiv);
125  printf("\tFrequency: %d\n", conf.freq);
126  printf("\tASCII output: %d\n", conf.ascii);
127  printf("\tAutoStart: %d\n", conf.autoStart);
128  printf("\tRAW mode: %d\n", conf.raw);
129  printf("\tJitter reduction: %d\n", colibriGetJitterStatus(imu[i]));
130  printf("\n\n");
131  }
132 
133 
134  // Start the devices
135  for (int i = 0; i < num_sensors; i++) {
136  colibriStart(imu[i]);
137  }
138 
139 
140  // VRPN stuff
142 }
143 
145 {
146  for (int i = 0; i < num_sensors; i++) {
147  colibriStop(imu[i]);
148  colibriClose(imu[i]);
149  }
150 
151  delete [] imu;
152 }
153 
155 {
156  // Call the generic server mainloop, since we are a server
157  server_mainloop();
158 
159  // Get latest data
160  get_report();
161 }
162 
164 {
166 
167  for (int i = 0; i < num_sensors; i++) {
168  TrivisioIMUData data;
169  colibriGetData(imu[i], &data);
170 
171  // The sensor number
172  d_sensor = i;
173 
174  // No need to fill in position, as we don´t get position information
175 
176  // The orientation
177  d_quat[0] = data.q_x;
178  d_quat[1] = data.q_y;
179  d_quat[2] = data.q_z;
180  d_quat[3] = data.q_w;
181 
182  // Send the data
183  send_report();
184  }
185 }
186 
188 {
189  if (d_connection) {
190  char msgbuf[1000];
191  int len = encode_to(msgbuf);
194  fprintf(stderr, "Tracker: cannot write message: tossing\n");
195  }
196  }
197 }
198 
199 #endif
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...
virtual void mainloop()
This function should be called each time through the main loop of the server code....
int register_server_handlers(void)
Definition: vrpn_Tracker.C:318
Generic connection class not specific to the transport mechanism.
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_TrivisioColibri(const char *name, vrpn_Connection *c, int numSensors=1, int Hz=60, int bufLen=0)
vrpn_Connection * d_connection
Connection that this object talks to.
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...
vrpn_int32 num_sensors
Definition: vrpn_Tracker.h:114
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_int32 d_sender_id
Sender ID registered with the connection.
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80