vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Tracker_TrivisioColibri.C
Go to the documentation of this file.
1
2//
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;
49 try { sensorList = new TrivisioSensor[numSensors]; }
50 catch (...) {
51 printf("vrpn_Tracker_TrivisioColibri::vrpn_Tracker_TrivisioColibri: Out of memory\n");
53 return;
54 }
55 int sensorCount = colibriGetDeviceList(sensorList, numSensors);
56
57 // If sensorCount == -1, there are more sensors connected than we specified, which is fine.
58 // Else, sensorCount == the number of sensors connected, which is all we can use.
59 if (sensorCount < 0) {
60 num_sensors = numSensors;
61 }
62 else {
63 num_sensors = sensorCount;
64 }
65
66
67 // Print info
68 if (num_sensors < 1) {
69 printf("Warning: No Colibri sensors found\n");
71 return;
72 }
73
74 printf("Using %d Colibri sensors\n", num_sensors);
75 for (int i = 0; i < num_sensors; i++) {
76 printf("%s:\t %s (FW %d.%d)\n", sensorList[i].dev, sensorList[i].ID,
77 sensorList[i].FWver, sensorList[i].FWsubver);
78 }
79 printf("\n\n");
80
81
82 // From the sample code:
83 //
84 // Diagonal matrices with diagonal element .68 yields approx 20Hz
85 // bandwidth @ 100Hz
86 //
87 float Ka[9] = { 0.68f, 0.00f, 0.00f,
88 0.00f, 0.68f, 0.00f,
89 0.00f, 0.00f, 0.68f };
90 float Kg[9] = { 0.68f, 0.00f, 0.00f,
91 0.00f, 0.68f, 0.00f,
92 0.00f, 0.00f, 0.68f };
93
94
95 // Create the array of device handles
96 try { imu = new void*[num_sensors]; }
97 catch (...) {
98 printf("vrpn_Tracker_TrivisioColibri::vrpn_Tracker_TrivisioColibri: Out of memory\n");
100 return;
101 }
102
103 // Create device handles and configure them
104 for (int i = 0; i < num_sensors; i++) {
105 imu[i] = colibriCreate(bufLen);
106
107 if (colibriOpen(imu[i], 0, sensorList[i].dev) < 0) {
108 printf("Warning: Could not access Colibri device on %s\n", sensorList[i]);
109 continue;
110 }
111
112 struct ColibriConfig conf;
113
114 // Taken from sample code
115 colibriGetConfig(imu[i], &conf);
116 conf.raw = 0;
117 conf.freq = Hz;
118 conf.sensor = (ColibriConfig::Sensor)1023;
119 conf.ascii = 0;
120 colibriSetConfig(imu[i], &conf);
121
122 colibriSetKa(imu[i], Ka);
123 colibriSetKaStatus(imu[i], 1);
124 colibriSetKg(imu[i], Kg);
125 colibriSetKgStatus(imu[i], 1);
126 colibriSetJitterStatus(imu[i], 1);
127
128 // Print info
129 char id[8];
130
131 printf("Colibri IMU %d\n", i);
132 colibriGetID(imu, id);
133 printf("\tDevice ID: %s\n", id);
134 printf("\tSensor config: %d\n", conf.sensor);
135 printf("\tMagnetic div: %d\n", (unsigned)conf.magDiv);
136 printf("\tFrequency: %d\n", conf.freq);
137 printf("\tASCII output: %d\n", conf.ascii);
138 printf("\tAutoStart: %d\n", conf.autoStart);
139 printf("\tRAW mode: %d\n", conf.raw);
140 printf("\tJitter reduction: %d\n", colibriGetJitterStatus(imu[i]));
141 printf("\n\n");
142 }
143
144
145 // Start the devices
146 for (int i = 0; i < num_sensors; i++) {
147 colibriStart(imu[i]);
148 }
149
150
151 // VRPN stuff
153}
154
156{
157 for (int i = 0; i < num_sensors; i++) {
158 colibriStop(imu[i]);
159 colibriClose(imu[i]);
160 }
161
162 try {
163 delete[] imu;
164 } catch (...) {
165 fprintf(stderr, "vrpn_Tracker_TrivisioColibri::~vrpn_Tracker_TrivisioColibri(): delete failed\n");
166 return;
167 }
168}
169
171{
172 // Call the generic server mainloop, since we are a server
174
175 // Get latest data
176 get_report();
177}
178
180{
182
183 for (int i = 0; i < num_sensors; i++) {
184 TrivisioIMUData data;
185 colibriGetData(imu[i], &data);
186
187 // The sensor number
188 d_sensor = i;
189
190 // No need to fill in position, as we don´t get position information
191
192 // The orientation
193 d_quat[0] = data.q_x;
194 d_quat[1] = data.q_y;
195 d_quat[2] = data.q_z;
196 d_quat[3] = data.q_w;
197
198 // Send the data
199 send_report();
200 }
201}
202
204{
205 if (d_connection) {
206 char msgbuf[1000];
207 int len = encode_to(msgbuf);
208 if (d_connection->pack_message(len, timestamp, position_m_id, d_sender_id, msgbuf,
210 fprintf(stderr, "Tracker: cannot write message: tossing\n");
211 }
212 }
213}
214
215#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...
Generic connection class not specific to the transport mechanism.
vrpn_Tracker_TrivisioColibri(const char *name, vrpn_Connection *c, int numSensors=1, int Hz=60, int bufLen=0)
virtual void mainloop()
This function should be called each time through the main loop of the server code....
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_int32 num_sensors
struct timeval timestamp
vrpn_int32 position_m_id
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
const int vrpn_TRACKER_FAIL