15#define MPSS_PER_G (9.80665)
18 float g_range,
float ar_range)
58 aligned.packet = packet;
61 if (packet.
header != 0xAA55) {
62 fprintf(stderr,
"vrpn_Tracker_Crossbow: Received packet with invalid header $%02X%02X (should be $AA55)\n",
63 aligned.bytes[0], aligned.bytes[1]);
68 vrpn_uint16 checksum = 0;
69 for (
int i = 2; i < 22; i++)
70 checksum += aligned.bytes[i];
74 fprintf(stderr,
"vrpn_Tracker_Crossbow: Received packet with invalid checksum $%04X (should be $%04X)\n",
83 struct timeval timeout;
86 timeout.tv_usec = 500000;
90 fprintf(stderr,
"vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
103 unsigned char echo =
'G';
128 if (!rv || (
buffer[1] != 0x55)) {
164 fprintf(stderr,
"vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
171 unsigned char recv_buf[8];
172 struct timeval timeout;
205 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
210 if ((recv_buf[0] !=
'H') || (recv_buf[1] != 255) || (recv_buf[7] != 255)) {
211 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow gave unexpected ping response\n");
216 if (recv_buf[6] != ((recv_buf[2] + recv_buf[3] + recv_buf[4] + recv_buf[5]) & 0xFF)) {
217 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow gave invalid serial number checksum\n");
222 const char *bufptr =
reinterpret_cast<const char *
>(&recv_buf[2]);
227 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
231 }
while (*recv_buf != 255);
233 int curSize = 4, curLen = 0;
236 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Out of memory\n");
242 fprintf(stderr,
"vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
246 if (*recv_buf !=
'$')
249 if (curLen == curSize)
251 }
while (*recv_buf !=
'$');
254 if (curLen == curSize)
267 unsigned char buffer =
'R';
268 struct timeval timeout;
280 fprintf(stderr,
"vrpn_Tracker_Crossbow: Crossbow device not responding to ping\n");
284 if (num_samples < 100) {
285 fprintf(stderr,
"vrpn_Tracker_Crossbow: Must recalibrate using at least 100 samples\n");
288 else if (num_samples > 25599) {
289 fprintf(stderr,
"vrpn_Tracker_Crossbow: Capping recalibration at 25,500 samples\n");
299 buffer[1] = (
unsigned char) (num_samples / 100);
308 struct timeval timeout;
309 timeout.tv_sec = 120;
312 fprintf(stderr,
"vrpn_Tracker_Crossbow: Failed to recalibrate device\n");
335 return data * scale / 32768;
343 memset(
pos, 0,
sizeof(
pos));
351 memset(
vel, 0,
sizeof(
vel));
376 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
381 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
386 fprintf(stderr,
"Tracker: cannot write message: tossing\n");
389 fprintf(stderr,
"Tracker: No valid connection\n");
398 double sinP = sin(pitch / 2.0);
399 double cosP = cos(pitch / 2.0);
400 double sinR = sin(roll / 2.0);
401 double cosR = cos(roll / 2.0);
403 destQuat[Q_X] = sinP * cosR;
404 destQuat[Q_Y] = cosP * sinR;
405 destQuat[Q_Z] = -sinP * sinR;
406 destQuat[Q_W] = cosP * cosR;
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
float convert_scalar(vrpn_int16 data, float scale) const
void unbuffer_packet(raw_packet &dest, unsigned char *buffer)
int validate_packet(const raw_packet &packet)
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),...
void process_packet(const raw_packet &packet)
int get_report()
Gets a report if one is available, returns 0 if not, 1 if complete report.
vrpn_uint32 device_serial
void recalibrate(vrpn_uint16 num_samples=20000)
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
void reset()
Reset the tracker.
vrpn_uint32 get_serial_number()
void xb_quat_from_euler(q_type destQuat, double pitch, double roll) const
const char * get_version_string()
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
char portname[VRPN_TRACKER_BUF_SIZE]
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
vrpn_Tracker_Serial(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=38400)
virtual int encode_to(char *buf)
virtual int encode_acc_to(char *buf)
virtual int encode_vel_to(char *buf)
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_set_rts(int comm)
int vrpn_close_commport(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
int vrpn_clear_rts(int comm)
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...
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.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
#define VRPN_DEGREES_TO_RADIANS
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_AWAITING_STATION
class VRPN_API vrpn_Connection