vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Wanda.C
Go to the documentation of this file.
1 #include <stdio.h> // for fprintf, stderr, NULL
2 #include <stdlib.h> // for getenv
3 
4 #include "vrpn_Connection.h" // for vrpn_CONNECTION_LOW_LATENCY, etc
5 #include "vrpn_Serial.h"
6 #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
7 #include "vrpn_Types.h" // for vrpn_float64, vrpn_int32
8 #include "vrpn_Wanda.h"
9 
10 // This will cause the value to be set when the program is run, before
11 // main() is called. It will be the same for all vrpn_Wanda instances.
12 int vrpn_Wanda::dbug_wanda = getenv("DBUG_WANDA") ? 1 : 0;
13 
14 // Returns time in seconds in a double.
15 inline double the_time()
16 {
17  struct timeval ts;
18  vrpn_gettimeofday(&ts, NULL);
19  return (double)(ts.tv_sec + ts.tv_usec/1e6);
20 }
21 
22 void
23 print_bits( char *buf, int num_bytes )
24 {
25  for(int i=0;i<num_bytes;i++) {
26  for(int b=7;b>=0;b--)
27  fprintf(stderr,"%d ", (((1 << b) & ((int)buf[i])) ? 1 : 0));
28  }
29  fprintf(stderr,"\n");
30 }
31 
32 void print_bits(unsigned char *buf, int n) { print_bits((char *)buf, n); }
33 
34 //
35 // Note: Wanda works at 1200 baud, 1 stopbit, CS7 (character size), and no parity
36 //
38  vrpn_Connection * c, char * portname,int baud,
39  vrpn_float64 update_rate):
40  vrpn_Serial_Analog(name, c, portname, baud, 7), vrpn_Button_Filter(name, c),
41  bytesread(0),
42  first(1),
43  index(0)
44 {
45  num_buttons = 3; // Wanda has 3 buttons
46  num_channel = 2;
47  for(int i=0; i<num_channel; i++) resetval[i] = -1;
48  if (update_rate != 0)
49  MAX_TIME_INTERVAL = (long)(1000000/update_rate);
50  else MAX_TIME_INTERVAL = -1;
52 
53  // reset buttons & channels
54  buttons[0] = buttons[1] = buttons[2] = 0;
55  channel[0] = channel[1] = 0;
56 
57  // Set the time
58  last_val_timestamp = the_time();
59 }
60 
61 void
63 {
64  if (dbug_wanda) {
65  fprintf(stderr, "buttons = %d %d %d\n", int(buttons[0]), int(buttons[1]), int(buttons[2]));
66  }
67  vrpn_Button::report_changes(); // report any button event;
68 }
69 
70 
71 void
73 {
74  last_val_timestamp = the_time();
75  if (dbug_wanda) {
76  fprintf(stderr, "vals = %lf %lf\n", channel[0], channel[1]);
77  }
78 
79  // Send the message on the connection;
81  char msgbuf[1000];
82  vrpn_int32 len = vrpn_Analog::encode_to(msgbuf);
83 #ifdef VERBOSE
85 #endif
89  fprintf(stderr,"Tracker: cannot write message: tossing\n");
90  }
91  } else {
92  fprintf(stderr,"Tracker Fastrak: No valid connection\n");
93  }
94 }
95 
97 {
99 
100  if (first) {
101  bytesread += vrpn_read_available_characters(serial_fd,buffer+bytesread,1024-bytesread);
102 
103  if (bytesread < 2) return;
104 
105  if (bytesread > 2) {
106  fprintf(stderr,"wanda huh? expected 2 characters on opening (got %d)\n", bytesread);
107  } else {
108  if (buffer[0] == 'M' && buffer[1] == '3') {
109  fprintf(stderr,"Read init message from wanda\n");
110  } else {
111  fprintf(stderr,"vrpn_Wanda: ERROR, expected 'M3' from wanda...\n");
112  }
113  }
114  bytesread = 0;
115  first = 0;
116  return;
117  }
118 
119  int new_button_info = 0;
120  int new_valuator_info = 0;
121 
122  // read available characters into end of buffer
123 // bytesread += vrpn_read_available_characters(serial_fd,buffer+bytesread,1024-bytesread);
124  int num_read = vrpn_read_available_characters(serial_fd,buffer+bytesread,1024-bytesread);
125 #if 1
126  if (dbug_wanda)
127  if (num_read > 0)
128  print_bits(buffer+bytesread, num_read);
129 #endif
130  bytesread += num_read;
131 
132  // handling synching
133  while( index == 0 && bytesread > 0 && !(buffer[0] & (1<<6)) ) {
134  fprintf(stderr,"synching wanda\n");
135  for(int i=0;i<bytesread-1;i++)
136  buffer[i] = buffer[i+1];
137 
138  bytesread--;
139  }
140 
141 #if 1
142  double curtime = the_time();
143  // XXX - hack. why doesn't wanda send a record when the user
144  // XXX - isn't pushing the joystick ball? we weren't getting
145  // XXX - the last record
146 // if (bytesread == 3 && index == 3 && curtime - last_val_timestamp > 0.2) {
147  if (curtime - last_val_timestamp > 0.2) {
148  int new_valuator_info = 0;
149 
150  if (channel[0] != 0) {
151  channel[0] = 0;
152  new_valuator_info = 1;
153  }
154  if (channel[1] != 0) {
155  channel[1] = 0;
156  new_valuator_info = 1;
157  }
158 
159  if (new_valuator_info) {
160  if (dbug_wanda) {
161  fprintf(stderr, "timeout: %lf\n", curtime - last_val_timestamp);
162  }
164  }
165  }
166 #endif
167 
168  // process all data in buffer
169 #if 1
170  if (dbug_wanda)
171  if (num_read > 0)
172  fprintf(stderr, "\t(bytesread = %d)\n", bytesread);
173 #endif
174 
175  while( bytesread >= 3 && index < bytesread ) {
176  // as soon as 3 bytes are available, report them...
177  if (index == 0 && bytesread >= 3) {
178  // update valuators & buttons #0 & #2
179 
180  signed char x = static_cast<char>((buffer[1] | ((buffer[0]&3) << 6)));
181  signed char y = static_cast<char>((buffer[2] | ((buffer[0]&12) << 4)));
182  double xd = -((double)x) / 34.0; // XXX - what is max range? 34?
183  double yd = ((double)y) / 34.0;
184 
185  if (xd > 1.0) xd = 1.0;
186  if (xd <-1.0) xd =-1.0;
187  if (yd > 1.0) yd = 1.0;
188  if (yd <-1.0) yd =-1.0;
189 
190  if (channel[0] != xd) {
191  channel[0] = xd;
192  new_valuator_info = 1;
193  }
194  if (channel[1] != yd) {
195  channel[1] = yd;
196  new_valuator_info = 1;
197  }
198 
199 
200  // decode button data
201  int blue_val = (buffer[0] & (1<<4)) ? 1 : 0; // blue button
202  int red_val = (buffer[0] & (1<<5)) ? 1 : 0; // red button
203 
204  if (blue_val != buttons[0]) {
205  buttons[0] = static_cast<unsigned char>(blue_val);
206  new_button_info = 1;
207  }
208  if (red_val != buttons[2]) {
209  buttons[2] = static_cast<unsigned char>(red_val);
210  new_button_info = 1;
211  }
212 
213 
214  index = 3;
215  }
216 
217  // if index is at 3 & there are more bytes & the next byte
218  // isn't the start of a new record, then process button info
219  if (index == 3 && bytesread > 3 && !(buffer[3] & (1<<6))) {
220  int new_val = (buffer[3]) ? 1 : 0; // yellow button
221 
222  if (new_val != buttons[1]) {
223  buttons[1] = static_cast<unsigned char>(new_val);
224  new_button_info = 1;
225  }
226 
227  index = 4;
228  }
229 
230  if (new_button_info)
232 
233  if (new_valuator_info)
235 
236  // if next byte is start of new record, shift bytes
237  if (index == 4 || (index == 3 && bytesread > 3 && (buffer[3] & (1<<6)))) {
238  for(int i=index;i<bytesread;i++)
239  buffer[i - index] = buffer[i];
240  bytesread -= index;
241  index = 0;
242  }
243  }
244 }
void print_bits(char *buf, int num_bytes)
Definition: vrpn_Wanda.C:23
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
unsigned char buffer[1024]
Definition: vrpn_Analog.h:75
double the_time()
Definition: vrpn_Wanda.C:15
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_int32 channel_m_id
Definition: vrpn_Analog.h:42
void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Wanda.C:96
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
Generic connection class not specific to the transport mechanism.
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
void report_new_button_info()
Definition: vrpn_Wanda.C:62
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_Wanda(char *name, vrpn_Connection *c, char *portname, int baud, double)
Definition: vrpn_Wanda.C:37
#define MAX_TIME_INTERVAL
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65
vrpn_int32 d_sender_id
Sender ID registered with the connection.
virtual vrpn_int32 encode_to(char *buf)
Definition: vrpn_Analog.C:54
const int vrpn_ANALOG_RESETTING
Definition: vrpn_Analog.h:22
void print(void)
Definition: vrpn_Analog.C:42
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
void report_new_valuator_info()
Definition: vrpn_Wanda.C:72
struct timeval timestamp
Definition: vrpn_Analog.h:41