vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_3DMouse.C
Go to the documentation of this file.
1#include <ctype.h> // for isprint
2#include <math.h> // for cos, sin
3#include <stdio.h> // for fprintf, stderr, perror
4
5#include "vrpn_Serial.h" // for vrpn_write_characters, etc
6#include "vrpn_Shared.h" // for vrpn_SleepMsecs, timeval
8#include "vrpn_Types.h" // for vrpn_float64
9
11
12// max time between reports (usec)
13#define MAX_TIME_INTERVAL (2000000)
14
16 const char *port, long baud, int filtering_count):
17 vrpn_Tracker_Serial(name, c, port, baud),
18 vrpn_Button_Filter(name, c),
19 _filtering_count(filtering_count),
20 _numbuttons(5),
21 _count(0)
22{
25}
26
28{
29 int i;
30 for (i=0; i <_numbuttons; i++)
32}
33
35{
36}
37
39{
40 int ret, i;
41
43
44 fprintf(stderr, "Resetting the 3DMouse...\n");
45 if (vrpn_write_characters(serial_fd, (const unsigned char*)"*R", 2) == 2)
46 {
47 fprintf(stderr,".");
48 vrpn_SleepMsecs(1000.0*2); // Wait after each character to give it time to respond
49 }
50 else
51 {
52 perror("3DMouse: Failed writing to 3DMouse");
54 return;
55 }
56
57 fprintf(stderr,"\n");
58
59 // Get rid of the characters left over from before the reset
61
62 // Make sure that the tracker has stopped sending characters
63 vrpn_SleepMsecs(1000.0*2);
64
65 if ( (ret = vrpn_read_available_characters(serial_fd, _buffer, 80)) != 0)
66 {
67 fprintf(stderr, "Got >=%d characters after reset\n", ret);
68 for (i = 0; i < ret; i++)
69 {
70 if (isprint(_buffer[i])) fprintf(stderr,"%c",_buffer[i]);
71 else fprintf(stderr,"[0x%02X]",_buffer[i]);
72 }
73 fprintf(stderr, "\n");
74 vrpn_flush_input_buffer(serial_fd); // Flush what's left
75 }
76
77 // Asking for tracker status
78 if (vrpn_write_characters(serial_fd, (const unsigned char *) "*\x05", 2) == 2)
79 vrpn_SleepMsecs(1000.0*1); // Sleep for a second to let it respond
80 else
81 {
82 perror(" 3DMouse write failed");
84 return;
85 }
86
87 // Read Status
88 bool success = true;
89
91 if (ret != 2) fprintf(stderr, " Got %d of 5 characters for status\n",ret);
92
93 fprintf(stderr, " Control Unit test : ");
94 if (_buffer[0] & 1) fprintf(stderr, "success\n");
95 else
96 {
97 fprintf(stderr, "fail\n");
98 success = false;
99 }
100
101 fprintf(stderr, " Processor test : ");
102 if (_buffer[0] & 2) fprintf(stderr, "success\n");
103 else
104 {
105 fprintf(stderr, "fail\n");
106 success = false;
107 }
108
109 fprintf(stderr, " EPROM checksum test : ");
110 if (_buffer[0] & 4) fprintf(stderr, "success\n");
111 else
112 {
113 fprintf(stderr, "fail\n");
114 success = false;
115 }
116
117 fprintf(stderr, " RAM checksum test : ");
118 if (_buffer[0] & 8) fprintf(stderr, "success\n");
119 else
120 {
121 fprintf(stderr, "fail\n");
122 success = false;
123 }
124
125 fprintf(stderr, " Transmitter test : ");
126 if (_buffer[0] & 16) fprintf(stderr, "success\n");
127 else
128 {
129 fprintf(stderr, "fail\n");
130 success = false;
131 }
132
133 fprintf(stderr, " Receiver test : ");
134 if (_buffer[0] & 32) fprintf(stderr, "success\n");
135 else
136 {
137 fprintf(stderr, "fail\n");
138 success = false;
139 }
140
141 fprintf(stderr, " Serial Port test : ");
142 if (_buffer[1] & 1) fprintf(stderr, "success\n");
143 else
144 {
145 fprintf(stderr, "fail\n");
146 success = false;
147 }
148
149 fprintf(stderr, " EEPROM test : ");
150 if (_buffer[0] & 2) fprintf(stderr, "success\n");
151 else
152 {
153 fprintf(stderr, "fail\n");
154 success = false;
155 }
156
157 if (!success)
158 {
159 fprintf(stderr, "Bad status report from 3DMouse, retrying reset\n");
161 return;
162 }
163 else
164 {
165 fprintf(stderr, "3DMouse gives status (this is good)\n");
166 }
167
168 // Set filtering count if the constructor parameter said to.
169 if (_filtering_count > 1)
170 {
172 }
173 fprintf(stderr, "Reset Completed (this is good)\n");
174 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
175}
176
178{
179 char sBuf[16];
180
181 sBuf[0] = 0x2a;
182 sBuf[1] = 0x24;
183 sBuf[2] = 2;
184 sBuf[3] = 7;
185 sBuf[4] = count;
186
187 if (vrpn_write_characters(serial_fd, (const unsigned char*)sBuf, 5) == 5)
188 {
189 vrpn_SleepMsecs(1000.0*1);
190 }
191 else
192 {
193 perror(" 3DMouse write filtering count failed");
195 return false;
196 }
197
198 return true;
199}
200
202{
203 int ret; // Return value from function call to be checked
204 timeval waittime;
205 waittime.tv_sec = 2;
206 waittime.tv_usec = 0;
207
209 unsigned char tmpc;
210
211 if (vrpn_write_characters(serial_fd, (const unsigned char*)"*d", 2) !=2)
212 {
213 perror(" 3DMouse write command failed");
215 return 0;
216 }
218 if (ret < 0)
219 {
220 perror(" 3DMouse read failed (disconnected)");
222 return 0;
223 }
224
225 _count += ret;
226 if (_count < 16) return 0;
227 if (_count > 16)
228 {
229 perror(" 3DMouse read failed (wrong message)");
231 return 0;
232 }
233
234 _count = 0;
235
236 tmpc = _buffer[0];
237 if (tmpc & 32)
238 {
239 //printf("port%d: Out of Range\n", i);
240 //ret |= 1 << (3-i);
241 } else
242 {
243 long ax, ay, az; // integer form of absolute translational data
244 short arx, ary, arz; // integer form of absolute rotational data
245 float p, y, r;
246
247 ax = (_buffer[1] & 0x40) ? 0xFFE00000 : 0;
248 ax |= (long)(_buffer[1] & 0x7f) << 14;
249 ax |= (long)(_buffer[2] & 0x7f) << 7;
250 ax |= (_buffer[3] & 0x7f);
251
252 ay = (_buffer[4] & 0x40) ? 0xFFE00000 : 0;
253 ay |= (long)(_buffer[4] & 0x7f) << 14;
254 ay |= (long)(_buffer[5] & 0x7f) << 7;
255 ay |= (_buffer[6] & 0x7f);
256
257 az = (_buffer[7] & 0x40) ? 0xFFE00000 : 0;
258 az |= (long)(_buffer[7] & 0x7f) << 14;
259 az |= (long)(_buffer[8] & 0x7f) << 7;
260 az |= (_buffer[9] & 0x7f);
261
262 pos[0] = static_cast<float>(ax / 100000.0 * 2.54);
263 pos[2] = static_cast<float>(ay / 100000.0 * 2.54);
264 pos[1] = -static_cast<float>(az / 100000.0f * 2.54);
265
266 arx = (_buffer[10] & 0x7f) << 7;
267 arx += (_buffer[11] & 0x7f);
268
269 ary = (_buffer[12] & 0x7f) << 7;
270 ary += (_buffer[13] & 0x7f);
271
272 arz = (_buffer[14] & 0x7f) << 7;
273 arz += (_buffer[15] & 0x7f);
274
275 p = static_cast<float>(arx / 40.0); // pitch
276 y = static_cast<float>(ary / 40.0); // yaw
277 r = static_cast<float>(arz / 40.0); // roll
278
279 p = static_cast<float>(p * VRPN_PI / 180);
280 y = static_cast<float>(y * VRPN_PI / 180);
281 r = static_cast<float>((360-r) * VRPN_PI / 180);
282
283 float cosp2 = static_cast<float>(cos(p/2));
284 float cosy2 = static_cast<float>(cos(y/2));
285 float cosr2 = static_cast<float>(cos(r/2));
286 float sinp2 = static_cast<float>(sin(p/2));
287 float siny2 = static_cast<float>(sin(y/2));
288 float sinr2 = static_cast<float>(sin(r/2));
289
290 d_quat[0] = cosr2*sinp2*cosy2 + sinr2*cosp2*siny2;
291 d_quat[1] = sinr2*cosp2*cosy2 + cosr2*sinp2*siny2;
292 d_quat[2] = cosr2*cosp2*siny2 + sinr2*sinp2*cosy2;
293 d_quat[3] = cosr2*cosp2*cosy2 + sinr2*sinp2*siny2;
294
295 }
296
297 buttons[0] = tmpc & 16; // Mouse stand button
298 buttons[1] = tmpc & 8; // Suspend button
299 buttons[2] = tmpc & 4; // Left button
300 buttons[3] = tmpc & 2; // Middle button
301 buttons[4] = tmpc & 1; // Right button
302 }
303
305
307 bufcount = 0;
308
309#ifdef VERBOSE2
311#endif
312
313 return 1;
314}
315
317{
319
320 switch(status)
321 {
325 if (get_report()) send_report();
326 break;
328 reset();
329 break;
331 fprintf(stderr, "3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
332 if (serial_fd >= 0)
333 {
335 serial_fd = -1;
336 }
338 {
339 fprintf(stderr,"vrpn_Tracker_3DMouse::mainloop(): Cannot Open serial port\n");
341 return;
342 }
344 break;
345 default:
346 break;
347 }
348}
349
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
vrpn_int32 num_buttons
Definition: vrpn_Button.h:48
virtual void report_changes(void)
Definition: vrpn_Button.C:423
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:46
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
unsigned char _buffer[2048]
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
virtual void mainloop()
Called once through each main loop iteration to handle updates.
vrpn_Tracker_3DMouse(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int filtering_count=1)
bool set_filtering_count(int count)
virtual void reset()
Reset the tracker.
virtual void clear_values(void)
char portname[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:151
virtual void send_report(void)
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
void print_latest_report(void)
Definition: vrpn_Tracker.C:325
#define VRPN_API
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:651
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:348
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:438
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition: vrpn_Serial.C:54
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void vrpn_SleepMsecs(double dMilliSecs)
Definition: vrpn_Shared.C:166
#define VRPN_PI
Definition: vrpn_Shared.h:13
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36