vrpn
07.33
Virtual Reality Peripheral Network
|
Go to the documentation of this file.
15 #define DEGREES_TO_RADIANS (3.1415926535897 / 180)
17 #define MPSS_PER_G (9.80665)
20 float g_range,
float ar_range) :
vrpn_Tracker_Serial(name, c, port, baud), just_read_something(0),
21 lin_accel_range(g_range), ang_accel_range(ar_range), device_version(0), device_serial(0)
55 aligned.packet = packet;
58 if (packet.
header != 0xAA55) {
59 fprintf(stderr,
"vrpn_Tracker_Crossbow: Received packet with invalid header $%02X%02X (should be $AA55)\n",
60 aligned.bytes[0], aligned.bytes[1]);
65 vrpn_uint16 checksum = 0;
66 for (
int i = 2; i < 22; i++)
67 checksum += aligned.bytes[i];
71 fprintf(stderr,
"vrpn_Tracker_Crossbow: Received packet with invalid checksum $%04X (should be $%04X)\n",
80 struct timeval timeout;
83 timeout.tv_usec = 500000;
87 fprintf(stderr,
"vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
100 unsigned char echo =
'G';
125 if (!rv || (
buffer[1] != 0x55)) {
161 fprintf(stderr,
"vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
168 unsigned char recv_buf[8];
169 struct timeval timeout;
174 #if 0 // doesn't help
202 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
207 if ((recv_buf[0] !=
'H') || (recv_buf[1] != 255) || (recv_buf[7] != 255)) {
208 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow gave unexpected ping response\n");
213 if (recv_buf[6] != ((recv_buf[2] + recv_buf[3] + recv_buf[4] + recv_buf[5]) & 0xFF)) {
214 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow gave invalid serial number checksum\n");
219 const char *bufptr =
reinterpret_cast<const char *
>(&recv_buf[2]);
224 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
228 }
while (*recv_buf != 255);
230 int curSize = 4, curLen = 0;
233 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Out of memory\n");
239 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
243 if (*recv_buf !=
'$')
246 if (curLen == curSize)
248 }
while (*recv_buf !=
'$');
251 if (curLen == curSize)
264 unsigned char buffer =
'R';
265 struct timeval timeout;
277 fprintf(stderr,
"vrpn_Tracker_Crossbow: Crossbow device not responding to ping\n");
281 if (num_samples < 100) {
282 fprintf(stderr,
"vrpn_Tracker_Crossbow: Must recalibrate using at least 100 samples\n");
285 else if (num_samples > 25599) {
286 fprintf(stderr,
"vrpn_Tracker_Crossbow: Capping recalibration at 25,500 samples\n");
296 buffer[1] = (
unsigned char) (num_samples / 100);
305 struct timeval timeout;
306 timeout.tv_sec = 120;
309 fprintf(stderr,
"vrpn_Tracker_Crossbow: Failed to recalibrate device\n");
332 return data * scale / 32768;
340 memset(
pos, 0,
sizeof(
pos));
348 memset(
vel, 0,
sizeof(
vel));
373 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
378 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
383 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
386 fprintf(stderr,
"Tracker: No valid connection\n");
395 double sinP = sin(pitch / 2.0);
396 double cosP = cos(pitch / 2.0);
397 double sinR = sin(roll / 2.0);
398 double cosR = cos(roll / 2.0);
400 destQuat[Q_X] = sinP * cosR;
401 destQuat[Q_Y] = cosP * sinR;
402 destQuat[Q_Z] = -sinP * sinR;
403 destQuat[Q_W] = cosP * cosR;
char portname[VRPN_TRACKER_BUF_SIZE]
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_Tracker_Crossbow(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS0", long baud=38400, float g_range=2.0f, float ar_range=100.0f)
The constructor is given the name of the tracker (the name of the sender it should use),...
virtual int encode_to(char *buf)
vrpn_uint32 device_serial
void process_packet(const raw_packet &packet)
int validate_packet(const raw_packet &packet)
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
int vrpn_set_rts(int comm)
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
virtual int encode_vel_to(char *buf)
float convert_scalar(vrpn_int16 data, float scale) const
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
void recalibrate(vrpn_uint16 num_samples=20000)
void reset()
Reset the tracker.
const char * get_version_string()
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
vrpn_Connection * d_connection
Connection that this object talks to.
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
void vrpn_SleepMsecs(double dMsecs)
const int vrpn_TRACKER_PARTIAL
vrpn_int32 d_sender_id
Sender ID registered with the connection.
int get_report()
Gets a report if one is available, returns 0 if not, 1 if complete report.
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
virtual int encode_acc_to(char *buf)
Generic connection class not specific to the transport mechanism.
const int vrpn_TRACKER_FAIL
void xb_quat_from_euler(q_type destQuat, double pitch, double roll) const
void unbuffer_packet(raw_packet &dest, unsigned char *buffer)
#define vrpn_gettimeofday
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_close_commport(int comm)
const int vrpn_TRACKER_SYNCING
vrpn_uint32 get_serial_number()
const int vrpn_TRACKER_AWAITING_STATION
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
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.
#define DEGREES_TO_RADIANS
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
int vrpn_clear_rts(int comm)