vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
vrpn_Tracker_Crossbow.C
Go to the documentation of this file.
1// vrpn_Tracker_Crossbow.h
2// This file contains the implementation for a Crossbow RGA300CA Tracker.
3
4#include <math.h> // for cos, sin
5#include <stdio.h> // for fprintf, stderr, NULL
6#include <stdlib.h> // for realloc, free
7#include <string.h> // for memset
8
9#include "quat.h" // for q_from_euler, Q_W, Q_X, Q_Y, etc
10#include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
11#include "vrpn_Serial.h"
13
14// Conversion multiplier from Gs to meters-per-second-per-second (~9.8 m/s^2 on Earth)
15#define MPSS_PER_G (9.80665)
16
17vrpn_Tracker_Crossbow::vrpn_Tracker_Crossbow(const char *name, vrpn_Connection *c, const char *port, long baud,
18 float g_range, float ar_range)
19 : vrpn_Tracker_Serial(name, c, port, baud)
20 , lin_accel_range(g_range)
21 , ang_accel_range(ar_range)
22 , device_serial(0)
23 , device_version(0)
24 , just_read_something(0)
25{
26}
27
29 if (device_version) {
30 free(device_version);
32 }
33}
34
35// Retrieves a raw_packet from an incoming byte array, and even flips endianness as necessary.
36void vrpn_Tracker_Crossbow::unbuffer_packet(raw_packet &dest, unsigned char *buffer) {
44 vrpn_unbuffer(&buffer, &dest.timer);
49}
50
52
53 // Allow accessing the packet as a string of bytes
54 union {
55 raw_packet packet;
56 vrpn_uint8 bytes[sizeof(raw_packet)];
57 } aligned;
58 aligned.packet = packet;
59
60 // Check the header for the magic number
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]);
64 return 1;
65 }
66
67 // Now calculate the expected checksum
68 vrpn_uint16 checksum = 0;
69 for (int i = 2; i < 22; i++)
70 checksum += aligned.bytes[i];
71
72 // And compare the two checksum values.
73 if (checksum != packet.checksum) {
74 fprintf(stderr, "vrpn_Tracker_Crossbow: Received packet with invalid checksum $%04X (should be $%04X)\n",
75 packet.checksum, checksum);
76 return 1;
77 }
78
79 return 0;
80}
81
83 struct timeval timeout;
84
85 timeout.tv_sec = 0;
86 timeout.tv_usec = 500000; // Half a second
87
88 switch (status) {
90 fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
91 return 0;
92
94 int rv;
95
96 // Quit early if we just got a packet
99 return 0;
100 }
101
102 // Request a packet from the device
103 unsigned char echo = 'G';
105
107 // Return early if no characters are available
108 if (!rv)
109 return 0;
110
111 // Return early if we aren't at the start of a packet header
112 if (*buffer != 0xAA) {
113 return 0;
114 }
115
116 bufcount = 1;
117
119
120 // Fall through to next state
121 }
122
124 int rv;
125
126 if (bufcount == 1) {
127 rv = vrpn_read_available_characters(serial_fd, buffer + 1, 1, &timeout);
128 if (!rv || (buffer[1] != 0x55)) {
129 buffer[0] = 0;
130 bufcount = 0;
132 return 0;
133 }
134 bufcount++;
135 }
136
137 // Try to read the rest of the packet. Return early if we haven't got a full packet yet.
139 bufcount += rv;
140 if (bufcount < 24)
141 return 0;
142
143 raw_packet new_data;
144 unbuffer_packet(new_data, &buffer[0]);
145
146 // Ensure the packet is valid
147 if (validate_packet(new_data)) {
150 return 0;
151 }
152
153 // Prepare the new report
154 process_packet(new_data);
155
156 bufcount = 0;
157 memset(buffer, 0, 24);
160 return 1;
161 }
162
163 default:
164 fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
165 return 0;
166 }
167}
168
170 const char *cmd;
171 unsigned char recv_buf[8];
172 struct timeval timeout;
173
174 timeout.tv_sec = 1;
175 timeout.tv_usec = 0;
176
177#if 0 // doesn't help
178 // First, take the comm port offline for a second
180 vrpn_SleepMsecs(1000);
182#endif
183
186
187 // Try resetting by toggling the RTS line of the serial port
189 vrpn_SleepMsecs(750);
191 vrpn_SleepMsecs(250);
193
195
196 cmd = "P";
197 vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 1);
198 vrpn_SleepMsecs(50); // Sleep long enough to stop receiving data
200
201 cmd = "RSv";
202 vrpn_write_characters(serial_fd, reinterpret_cast<const unsigned char*> (cmd), 3);
204 if (vrpn_read_available_characters(serial_fd, recv_buf, 8, &timeout) != 8) {
205 fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
207 return;
208 }
209
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");
213 return;
214 }
215
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");
219 return;
220 }
221
222 const char *bufptr = reinterpret_cast<const char *>(&recv_buf[2]);
223 vrpn_unbuffer(&bufptr, &device_serial);
224
225 if (0) do {
226 if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
227 fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
229 return;
230 }
231 } while (*recv_buf != 255);
232
233 int curSize = 4, curLen = 0;
234 device_version = (char *) realloc(device_version, curSize * sizeof(char));
235 if (device_version == NULL) {
236 fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Out of memory\n");
238 return;
239 }
240 do {
241 if (!vrpn_read_available_characters(serial_fd, recv_buf, 1, &timeout)) {
242 fprintf(stderr, "vrpn_Tracker_Crossbow::reset: Crossbow not responding to stimulus\n");
244 return;
245 }
246 if (*recv_buf != '$')
247 device_version[curLen++] = *recv_buf;
248
249 if (curLen == curSize)
250 device_version = (char *) realloc(device_version, curSize *= 2);
251 } while (*recv_buf != '$');
252
253 // Now null-terminate the version string, expanding it one last time if necessary
254 if (curLen == curSize)
255 device_version = (char *) realloc(device_version, ++curSize);
256
257 device_version[curLen] = 0;
258
259 //printf("Serial %u\tVersion '%s'\n", device_serial, device_version);
260
263
264}
265
267 unsigned char buffer = 'R';
268 struct timeval timeout;
269
271
272 timeout.tv_sec = 1;
273 timeout.tv_usec = 0;
274
275 while (vrpn_read_available_characters(serial_fd, &buffer, 1, &timeout) == 1) {
276 if (buffer == 'H')
277 return;
278 }
279
280 fprintf(stderr, "vrpn_Tracker_Crossbow: Crossbow device not responding to ping\n");
281}
282
283void vrpn_Tracker_Crossbow::recalibrate(vrpn_uint16 num_samples) {
284 if (num_samples < 100) {
285 fprintf(stderr, "vrpn_Tracker_Crossbow: Must recalibrate using at least 100 samples\n");
286 return;
287 }
288 else if (num_samples > 25599) {
289 fprintf(stderr, "vrpn_Tracker_Crossbow: Capping recalibration at 25,500 samples\n");
290 num_samples = 25500;
291 }
292
295
296 // Prepare zero command
297 unsigned char buffer[2];
298 buffer[0] = 'z';
299 buffer[1] = (unsigned char) (num_samples / 100);
300
303 vrpn_SleepMsecs(50);
304
305 // Wait for affirmative response.
306 // Allow two minutes before timing out.
307 // Even 25500 samples should make it with a few seconds to spare.
308 struct timeval timeout;
309 timeout.tv_sec = 120;
310 timeout.tv_usec = 0;
311 if (!vrpn_read_available_characters(serial_fd, buffer, 1, &timeout) || *buffer != 'Z') {
312 fprintf(stderr, "vrpn_Tracker_Crossbow: Failed to recalibrate device\n");
313 }
314}
315
317 if (!device_serial)
318 reset();
319 return device_serial;
320}
321
323 if (!device_version)
324 reset();
325 return device_version;
326}
327
328
331}
332
333// Utility function to convert packed inputs into decoded floating-point equivalents.
334float vrpn_Tracker_Crossbow::convert_scalar(vrpn_int16 data, float scale) const {
335 return data * scale / 32768; // 2^15
336}
337
339 // Use the current time for a timestamp.
341
342 // Clear the position record.
343 memset(pos, 0, sizeof(pos));
344
345 // Calculate the current orientation. (We don't know yaw, so report 0.)
346 double pitch = convert_scalar(packet.pitch_angle, 180.0f) * VRPN_DEGREES_TO_RADIANS;
347 double roll = convert_scalar(packet.roll_angle, 180.0f) * VRPN_DEGREES_TO_RADIANS;
348 xb_quat_from_euler(d_quat, pitch, roll);
349
350 // Clear the linear velocity; we don't know it.
351 memset(vel, 0, sizeof(vel));
352
353 // Calculate the current angular velocity from yaw rate
354 // It's in degrees per second, so convert to radians per second.
355 q_from_euler(vel_quat, convert_scalar(packet.yaw_rate, 1.5f * ang_accel_range) * VRPN_DEGREES_TO_RADIANS, 0, 0);
356 vel_quat_dt = 1;
357
358 // Calculate the current acceleration vector
359 acc[0] = convert_scalar(packet.accel_x, 1.5f * lin_accel_range) * MPSS_PER_G;
360 acc[1] = convert_scalar(packet.accel_y, 1.5f * lin_accel_range) * MPSS_PER_G;
361 acc[2] = convert_scalar(packet.accel_z, 1.5f * lin_accel_range) * MPSS_PER_G;
362
363 //printf("RawAccel = %hd\tG-Range = %f\tDerivedZ = %f\n", packet.accel_z, lin_accel_range, acc[2]);
364
365 // The angular acceleration vector is nil. (0001 / 1)
366 acc_quat[0] = acc_quat[1] = acc_quat[2] = 0;
367 acc_quat[3] = acc_quat_dt = 1;
368}
369
371 // Send the message on the connection
372 if (d_connection) {
373 char msgbuf[1000];
374 int len = encode_to(msgbuf);
376 fprintf(stderr,"Tracker: cannot write message: tossing\n");
377 }
378
379 len = encode_acc_to(msgbuf);
381 fprintf(stderr,"Tracker: cannot write message: tossing\n");
382 }
383
384 len = encode_vel_to(msgbuf);
386 fprintf(stderr,"Tracker: cannot write message: tossing\n");
387 }
388 } else {
389 fprintf(stderr,"Tracker: No valid connection\n");
390 }
391}
392
393// Creates orientation quaternion from pitch and roll Euler angles.
394// Crossbow angles are given such that p' = yaw(pitch(roll(p))),
395// but quatlib angles assume p' = roll(pitch(yaw(p))).
396// Hence the apparent reimplementation of q_from_euler.
397void vrpn_Tracker_Crossbow::xb_quat_from_euler(q_type destQuat, double pitch, double roll) const {
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);
402
403 destQuat[Q_X] = sinP * cosR;
404 destQuat[Q_Y] = cosP * sinR;
405 destQuat[Q_Z] = -sinP * sinR;
406 destQuat[Q_W] = cosP * cosR;
407}
408
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Generic connection class not specific to the transport mechanism.
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...
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.
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.
void xb_quat_from_euler(q_type destQuat, double pitch, double roll) const
unsigned char buffer[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:155
char portname[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:151
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
vrpn_float64 acc_quat[4]
Definition: vrpn_Tracker.h:98
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:552
vrpn_float64 vel_quat[4]
Definition: vrpn_Tracker.h:96
vrpn_float64 d_quat[4]
Definition: vrpn_Tracker.h:95
vrpn_int32 velocity_m_id
Definition: vrpn_Tracker.h:81
virtual int encode_acc_to(char *buf)
Definition: vrpn_Tracker.C:604
vrpn_float64 vel_quat_dt
Definition: vrpn_Tracker.h:97
vrpn_float64 vel[3]
Definition: vrpn_Tracker.h:96
vrpn_float64 acc_quat_dt
Definition: vrpn_Tracker.h:99
vrpn_int32 accel_m_id
Definition: vrpn_Tracker.h:82
virtual int encode_vel_to(char *buf)
Definition: vrpn_Tracker.C:577
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_float64 acc[3]
Definition: vrpn_Tracker.h:98
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80
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.
Definition: vrpn_Serial.C:651
int vrpn_set_rts(int comm)
Definition: vrpn_Serial.C:368
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_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
Definition: vrpn_Serial.C:488
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
Definition: vrpn_Serial.C:465
int vrpn_clear_rts(int comm)
Definition: vrpn_Serial.C:402
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...
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.
Definition: vrpn_Shared.C:321
void vrpn_SleepMsecs(double dMilliSecs)
Definition: vrpn_Shared.C:166
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:99
#define VRPN_DEGREES_TO_RADIANS
Definition: vrpn_Shared.h:15
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
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
#define MPSS_PER_G