vrpn  07.33
Virtual Reality Peripheral Network
vrpn_UNC_Joystick.C
Go to the documentation of this file.
1 #include <stdio.h> // for fprintf, stderr, perror, etc
2 #include <string.h> // for strlen
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_SleepMsecs, etc
7 #include "vrpn_Types.h" // for vrpn_float64, vrpn_int32
8 #include "vrpn_UNC_Joystick.h"
9 
10 // This class runs the UNC custom serial joystick. It includes two
11 // buttons, a slider, and two 3-axis joysticks. It is based on a
12 // single-board computer. This driver is based on the px_sjoy.c
13 // code.
14 
15 static const vrpn_float64 JoyScale[] = {1019, 200, 200, 350, 200, 200, 350};
16 
18  vrpn_Connection * c, char * portname,int baud,
19  vrpn_float64 update_rate):
20  vrpn_Serial_Analog(name, c, portname, baud),
21  vrpn_Button_Filter(name, c)
22 {
23  num_buttons = 2; // Has 2 buttons
24  num_channel = 7; // Has a slider and two 3-axis joysticks
25  if (update_rate != 0) {
26  MAX_TIME_INTERVAL = (long)(1000000/update_rate);
27  } else {
28  MAX_TIME_INTERVAL = -1;
29  }
31 }
32 
33 void vrpn_Joystick::report(struct timeval current_time)
34 {
35  // Store the current time in the structures
36  vrpn_Analog::timestamp.tv_sec = current_time.tv_sec;
37  vrpn_Analog::timestamp.tv_usec = current_time.tv_usec;
38  vrpn_Button::timestamp.tv_sec = current_time.tv_sec;
39  vrpn_Button::timestamp.tv_usec = current_time.tv_usec;
40 
41  vrpn_Button::report_changes(); // report any button event;
42 
43  // Send the message on the connection;
45  char msgbuf[1000];
46  vrpn_int32 len = vrpn_Analog::encode_to(msgbuf);
50  fprintf(stderr,"UNC Joystick: Cannot pack message, tossing\n");
51  }
52  } else {
53  fprintf(stderr,"UNC Joystick: No valid Connection\n");
54  }
55 }
56 
58  struct timeval current_time;
59  vrpn_gettimeofday(&current_time, NULL);
60 
61  // Since we are a server, call the generic server mainloop()
63 
64  switch (status) {
65 
68  {
69  // As long as we keep getting reports, send them.
70  // XXX Ideally, this time would be the time of the first
71  // character read from the serial port for this report.
72  while (get_report()) {
73  vrpn_gettimeofday(&current_time, NULL);
74  report(current_time);
75  }
76 
77  // If it has been longer than the requested report interval
78  // since we sent a report, send a complete report
79  // anyway (a repeat of the previous one).
80  if ( (vrpn_TimevalDuration(current_time, vrpn_Analog::timestamp)
81  > MAX_TIME_INTERVAL) && (MAX_TIME_INTERVAL != -1) ) {
82 
83  // send out the last report again;
84  report(current_time);
85  }
86  }
87  break;
88 
90  case vrpn_ANALOG_FAIL:
91  reset();
92  break;
93  }
94 }
95 
96 /****************************************************************************/
97 /* Send request for status to joysticks and reads response.
98  * Resets all of the rest values for the analog channels.
99  */
101  char request[256];
102  int write_rt, bytesread;
103 
104  fprintf(stderr, "Going into vrpn_Joystick::reset()\n");
105 
106  /* Request baseline report for comparison */
107  request[0] = 'r';
108  request[1] = 0;
109  write_rt = vrpn_write_characters(serial_fd, (unsigned char*)request, strlen(request));
110  if (write_rt < 0) {
111  fprintf(stderr, "vrpn_Joystick::reset: write failed\n");
113  return;
114  }
115  vrpn_SleepMsecs(1000.0*1);
116 
117  // Read the report and parse each entry. For each one, have
118  // the parse routine store the restvalue, so that it will be
119  // subtracted from subsequent readings.
120  bytesread= vrpn_read_available_characters(serial_fd, serialbuf, 16);
121  if (bytesread != 16) {
122  fprintf(stderr, "vrpn_Joystick::reset: got only %d char from \
123 joystick, should be 16, trying again\n", bytesread);
125  } else {
126  // Parse the analog reports in the serialbuf and reset each
127  // channel's rest value. The button report is not
128  // parsed.
129  for (int i=0; i< num_channel; i++) {
130  parse(i*2, 1);
131  }
133 
134  /* Request only send each channel when state has changed */
135  request[0] = 'j';
136  vrpn_write_characters(serial_fd, (unsigned char*)request, strlen(request));
137  }
138 }
139 
140 // SYNCING means that we are looking for a character that has
141 // the high bit clear (the first character of a report has this,
142 // while the second has this bit set).
143 // PARTIAL means that we have gotten the first character and are
144 // looking for the second character (which will have the high bit
145 // set).
147  int bytesread = 0;
148 
149  if (status == vrpn_ANALOG_SYNCING) {
150  bytesread =vrpn_read_available_characters(serial_fd, serialbuf, 1);
151  if (bytesread == -1) {
152  perror("vrpn_Joystick::get_report() 1st read failed");
153  return 0;
154  }
155  if (bytesread == 1) {
156  if ((serialbuf[0] >> 7) == 0) {
158  } else {
159  fprintf(stderr,"vrpn_Joystick::get_report(): Bad 1st byte (re-syncing)\n");
160  return 0;
161  }
162  }
163  }
164 
165  // Since we are not syncing, we must be in partial mode and
166  // looking for the second character.
167  bytesread = vrpn_read_available_characters(serial_fd, serialbuf+1, 1);
168  if (bytesread == -1) {
169  perror("vrpn_Joystick::get_report() 2nd read failed");
170  return 0;
171  }
172  if (bytesread == 0) {
173  return 0;
174  }
175  // If the high bit is not set, then what we have here is a
176  // first character, not a second. Go ahead and put it into
177  // the first byte and continue on in partial mode. Scary but
178  // true: this seems to get all of the reports accurately,
179  // when tossing it out loses reports. Looks like there are
180  // sometimes duplicate first characters in the report ?!?!?
181  if ( (serialbuf[1] >> 7) == 0 ) { // Should have high bit set
182  serialbuf[0] = serialbuf[1];
184  return 0;
185  }
186 
187  parse(0); // Parse starting at the beginning of the serialbuf.
189  return 1; // Indicates that we have gotten a report
190 }
191 
192 /****************************************************************************/
193 /* Decodes bytes as follows:
194  First byte of set received (from high order bit down):
195  -bit == 0 to signify first byte
196  -empty bit
197  -3 bit channel label
198  -3 bits (out of 10) of channel reading (high-order bits)
199  Second byte of set received (from high order bit down):
200  -bit == 1 to signify second byte
201  -7 bits (out of 10) of channel reading (low-order bits)
202 */
203 
204 void vrpn_Joystick::parse (int index, int reset_rest_pos)
205 {
206  unsigned int temp;
207  static const unsigned int mask1 = 7, mask2 = 127, left = 1, right = 2;
208 
209  int chan; // Channel number extracted from report
210  int value; // Integer value extracted from report
211 
212  // Isolate the channel part of the first byte by shifting it
213  // to the lowest bits and then masking off the rest.
214  chan = serialbuf[index] >> 3;
215  chan = chan & mask1;
216 
217  // If channel number is > 7, this is illegal.
218  if (chan > 7) {
219  fprintf(stderr,"vrpn_Joystick::parse(): Invalid channel (%d)\n",chan);
221  return;
222  }
223 
224  // Channel 7 is the button report.
225  if (chan == 7) {
226  /******************************************************************
227  * The least most significant bit should be the left button and the next
228  * should be the right button. In the original reading the two bits are
229  * opposite this. We swap the two least significant bits. */;
230 
231  buttons[0] = static_cast<unsigned char>(!(serialbuf[index+1] & left)?1:0);
232  buttons[1] = static_cast<unsigned char>(!(serialbuf[index+1] & right)?1:0);
233 #ifdef VERBOSE
234  printf("Joystick::parse: Buttons: %d %d\n",
235  buttons[1], buttons[0]);
236 #endif
237  return;
238  }
239 
240  // The other channels are the analog channels [0..6]
241  // Strip off the high-order 3 bits from the first byte and
242  // shift them up to their proper location.
243  // Then strip off check bit from the second byte and insert
244  // the value bits to form the full value.
245  temp = serialbuf[index] & mask1; /* isolate value bits */
246  temp = temp << 7; /* position value bits */
247  value = (serialbuf[index+1] & mask2) | temp;
248 
249  // If we're resetting, set the rest positions to this value.
250  // Don't do this for the slider (channel 0), since it is not
251  // auto-centering.
252  if (reset_rest_pos) {
253  if (chan == 0) {
254  restval[chan] = 512;
255  } else {
256  restval[chan]= value;
257  }
258  }
259 
260  // Subtract the current value from the rest value to form the
261  // relative location, then scale to get into the range
262  // [-0.5 --- 0.5]. Clamp to make sure we are in this range.
263  channel[chan] = (restval[chan] - value)/JoyScale[chan];
264  if (channel[chan] > 0.5) { channel[chan] = 0.5; }
265  if (channel[chan] < -0.5) { channel[chan] = -0.5; }
266 
267  // The slider should be inverted, to match the conventional
268  // reading.
269  if (chan == 0) {channel[chan] = -channel[chan];}
270 
271 #ifdef VERBOSE
272  printf("Joystick::parse: channel[%d] = %f\n", chan, channel[chan]);
273 #endif
274 }
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...
const int vrpn_ANALOG_PARTIAL
Definition: vrpn_Analog.h:21
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_int32 channel_m_id
Definition: vrpn_Analog.h:42
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.
vrpn_Joystick(char *name, vrpn_Connection *c, char *portname, int baud, double)
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(struct timeval current_time)
vrpn_Connection * d_connection
Connection that this object talks to.
void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
struct timeval timestamp
Definition: vrpn_Button.h:48
const int vrpn_ANALOG_SYNCING
Definition: vrpn_Analog.h:19
#define MAX_TIME_INTERVAL
void parse(int, int reset_rest_pos=0)
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
const int vrpn_ANALOG_FAIL
Definition: vrpn_Analog.h:23
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
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
struct timeval timestamp
Definition: vrpn_Analog.h:41