13#define MAX_TIME_INTERVAL (2000000)
16 const char *port,
long baud,
int filtering_count):
44 fprintf(stderr,
"Resetting the 3DMouse...\n");
52 perror(
"3DMouse: Failed writing to 3DMouse");
67 fprintf(stderr,
"Got >=%d characters after reset\n", ret);
68 for (i = 0; i < ret; i++)
71 else fprintf(stderr,
"[0x%02X]",
_buffer[i]);
73 fprintf(stderr,
"\n");
82 perror(
" 3DMouse write failed");
91 if (ret != 2) fprintf(stderr,
" Got %d of 5 characters for status\n",ret);
93 fprintf(stderr,
" Control Unit test : ");
94 if (
_buffer[0] & 1) fprintf(stderr,
"success\n");
97 fprintf(stderr,
"fail\n");
101 fprintf(stderr,
" Processor test : ");
102 if (
_buffer[0] & 2) fprintf(stderr,
"success\n");
105 fprintf(stderr,
"fail\n");
109 fprintf(stderr,
" EPROM checksum test : ");
110 if (
_buffer[0] & 4) fprintf(stderr,
"success\n");
113 fprintf(stderr,
"fail\n");
117 fprintf(stderr,
" RAM checksum test : ");
118 if (
_buffer[0] & 8) fprintf(stderr,
"success\n");
121 fprintf(stderr,
"fail\n");
125 fprintf(stderr,
" Transmitter test : ");
126 if (
_buffer[0] & 16) fprintf(stderr,
"success\n");
129 fprintf(stderr,
"fail\n");
133 fprintf(stderr,
" Receiver test : ");
134 if (
_buffer[0] & 32) fprintf(stderr,
"success\n");
137 fprintf(stderr,
"fail\n");
141 fprintf(stderr,
" Serial Port test : ");
142 if (
_buffer[1] & 1) fprintf(stderr,
"success\n");
145 fprintf(stderr,
"fail\n");
149 fprintf(stderr,
" EEPROM test : ");
150 if (
_buffer[0] & 2) fprintf(stderr,
"success\n");
153 fprintf(stderr,
"fail\n");
159 fprintf(stderr,
"Bad status report from 3DMouse, retrying reset\n");
165 fprintf(stderr,
"3DMouse gives status (this is good)\n");
173 fprintf(stderr,
"Reset Completed (this is good)\n");
193 perror(
" 3DMouse write filtering count failed");
206 waittime.tv_usec = 0;
213 perror(
" 3DMouse write command failed");
220 perror(
" 3DMouse read failed (disconnected)");
226 if (
_count < 16)
return 0;
229 perror(
" 3DMouse read failed (wrong message)");
247 ax = (
_buffer[1] & 0x40) ? 0xFFE00000 : 0;
248 ax |= (long)(
_buffer[1] & 0x7f) << 14;
249 ax |= (long)(
_buffer[2] & 0x7f) << 7;
252 ay = (
_buffer[4] & 0x40) ? 0xFFE00000 : 0;
253 ay |= (long)(
_buffer[4] & 0x7f) << 14;
254 ay |= (long)(
_buffer[5] & 0x7f) << 7;
257 az = (
_buffer[7] & 0x40) ? 0xFFE00000 : 0;
258 az |= (long)(
_buffer[7] & 0x7f) << 14;
259 az |= (long)(
_buffer[8] & 0x7f) << 7;
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);
266 arx = (
_buffer[10] & 0x7f) << 7;
269 ary = (
_buffer[12] & 0x7f) << 7;
272 arz = (
_buffer[14] & 0x7f) << 7;
275 p =
static_cast<float>(arx / 40.0);
276 y =
static_cast<float>(ary / 40.0);
277 r =
static_cast<float>(arz / 40.0);
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);
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));
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;
331 fprintf(stderr,
"3DMouse failed, trying to reset (Try power cycle if more than 4 attempts made)\n");
339 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...
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]
virtual void send_report(void)
vrpn_Tracker_Serial(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=38400)
void print_latest_report(void)
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)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
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.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void vrpn_SleepMsecs(double dMilliSecs)
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_AWAITING_STATION
class VRPN_API vrpn_Connection