7 #define M_PI 3.14159265358979323846 18 #define MAX_TIME_INTERVAL (2000000) 19 #define INCHES_TO_METERS (2.54/100.0) 22 const char *port,
long baud,
int filtering_count):
25 _filtering_count(filtering_count),
50 fprintf(stderr,
"Resetting the 3DMouse...\n");
58 perror(
"3DMouse: Failed writing to 3DMouse");
73 fprintf(stderr,
"Got >=%d characters after reset\n", ret);
74 for (i = 0; i < ret; i++)
77 else fprintf(stderr,
"[0x%02X]",
_buffer[i]);
79 fprintf(stderr,
"\n");
88 perror(
" 3DMouse write failed");
97 if (ret != 2) fprintf(stderr,
" Got %d of 5 characters for status\n",ret);
99 fprintf(stderr,
" Control Unit test : ");
100 if (
_buffer[0] & 1) fprintf(stderr,
"success\n");
103 fprintf(stderr,
"fail\n");
107 fprintf(stderr,
" Processor test : ");
108 if (
_buffer[0] & 2) fprintf(stderr,
"success\n");
111 fprintf(stderr,
"fail\n");
115 fprintf(stderr,
" EPROM checksum test : ");
116 if (
_buffer[0] & 4) fprintf(stderr,
"success\n");
119 fprintf(stderr,
"fail\n");
123 fprintf(stderr,
" RAM checksum test : ");
124 if (
_buffer[0] & 8) fprintf(stderr,
"success\n");
127 fprintf(stderr,
"fail\n");
131 fprintf(stderr,
" Transmitter test : ");
132 if (
_buffer[0] & 16) fprintf(stderr,
"success\n");
135 fprintf(stderr,
"fail\n");
139 fprintf(stderr,
" Receiver test : ");
140 if (
_buffer[0] & 32) fprintf(stderr,
"success\n");
143 fprintf(stderr,
"fail\n");
147 fprintf(stderr,
" Serial Port test : ");
148 if (
_buffer[1] & 1) fprintf(stderr,
"success\n");
151 fprintf(stderr,
"fail\n");
155 fprintf(stderr,
" EEPROM test : ");
156 if (
_buffer[0] & 2) fprintf(stderr,
"success\n");
159 fprintf(stderr,
"fail\n");
165 fprintf(stderr,
"Bad status report from 3DMouse, retrying reset\n");
171 fprintf(stderr,
"3DMouse gives status (this is good)\n");
179 fprintf(stderr,
"Reset Completed (this is good)\n");
199 perror(
" 3DMouse write filtering count failed");
212 waittime.tv_usec = 0;
219 perror(
" 3DMouse write command failed");
226 perror(
" 3DMouse read failed (disconnected)");
232 if (
_count < 16)
return 0;
235 perror(
" 3DMouse read failed (wrong message)");
253 ax = (
_buffer[1] & 0x40) ? 0xFFE00000 : 0;
254 ax |= (long)(
_buffer[1] & 0x7f) << 14;
255 ax |= (long)(
_buffer[2] & 0x7f) << 7;
258 ay = (
_buffer[4] & 0x40) ? 0xFFE00000 : 0;
259 ay |= (long)(
_buffer[4] & 0x7f) << 14;
260 ay |= (long)(
_buffer[5] & 0x7f) << 7;
263 az = (
_buffer[7] & 0x40) ? 0xFFE00000 : 0;
264 az |= (long)(
_buffer[7] & 0x7f) << 14;
265 az |= (long)(
_buffer[8] & 0x7f) << 7;
268 pos[0] = static_cast<float>(ax / 100000.0 * 2.54);
269 pos[2] = static_cast<float>(ay / 100000.0 * 2.54);
270 pos[1] = -static_cast<float>(az / 100000.0f * 2.54);
272 arx = (
_buffer[10] & 0x7f) << 7;
275 ary = (
_buffer[12] & 0x7f) << 7;
278 arz = (
_buffer[14] & 0x7f) << 7;
281 p = static_cast<float>(arx / 40.0);
282 y = static_cast<float>(ary / 40.0);
283 r = static_cast<float>(arz / 40.0);
285 p = static_cast<float>(p *
M_PI / 180);
286 y = static_cast<float>(y *
M_PI / 180);
287 r = static_cast<float>((360-r) *
M_PI / 180);
289 float cosp2 = static_cast<float>(cos(p/2));
290 float cosy2 = static_cast<float>(cos(y/2));
291 float cosr2 = static_cast<float>(cos(r/2));
292 float sinp2 = static_cast<float>(sin(p/2));
293 float siny2 = static_cast<float>(sin(y/2));
294 float sinr2 = static_cast<float>(sin(r/2));
296 d_quat[0] = cosr2*sinp2*cosy2 + sinr2*cosp2*siny2;
297 d_quat[1] = sinr2*cosp2*cosy2 + cosr2*sinp2*siny2;
298 d_quat[2] = cosr2*cosp2*siny2 + sinr2*sinp2*cosy2;
299 d_quat[3] = cosr2*cosp2*cosy2 + sinr2*sinp2*siny2;
337 fprintf(stderr,
"3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
345 fprintf(stderr,
"vrpn_Tracker_3DMouse::mainloop(): Cannot Open serial port\n");
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_close_commport(int comm)
void vrpn_SleepMsecs(double dMsecs)
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.
virtual void clear_values(void)
unsigned char _buffer[2048]
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Tracker_3DMouse(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=19200, int filtering_count=1)
Generic connection class not specific to the transport mechanism.
virtual void send_report(void)
const int vrpn_TRACKER_FAIL
virtual void reset()
Reset the tracker.
const int vrpn_TRACKER_RESETTING
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
const int vrpn_TRACKER_PARTIAL
char portname[VRPN_TRACKER_BUF_SIZE]
const int vrpn_TRACKER_SYNCING
virtual void mainloop()
Called once through each main loop iteration to handle updates.
const int vrpn_TRACKER_AWAITING_STATION
void print_latest_report(void)
bool set_filtering_count(int count)
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.