vrpn  07.33
Virtual Reality Peripheral Network
vrpn_inertiamouse.C
Go to the documentation of this file.
1 /*
2  * vrpn_inertiamouse.C
3  */
4 
5 #include <math.h> // for fabs
6 #include <stdio.h> // for fprintf, stderr, NULL
7 #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
8 
9 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
10 #include "vrpn_Serial.h"
11 #include "vrpn_inertiamouse.h"
12 
13 #undef VERBOSE
14 
15 const double vrpn_inertiamouse::Vel_Decay = 0.66;
16 
17 namespace {
18  enum {
19  STATUS_RESETTING = -1, // Resetting the device
20  STATUS_SYNCING = 0, // Looking for the first character of report
21  STATUS_READING = 1, // Looking for the rest of the report
22 
23  MAX_TIME_INTERVAL = 2000000 // max time between reports (usec)
24  };
25 } // namespace
27  vrpn_Connection * c,
28  const char* port,
29  int baud_rate)
30  : vrpn_Serial_Analog (name, c, port, baud_rate)
31  , vrpn_Button_Filter (name, c)
32  , numbuttons_ (Buttons)
33  , numchannels_ (Channels)
34  , null_radius_ (0)
35  , bufcount_ (0)
36  , expected_chars_ (1)
37  , dcb_ ()
38  , lp_ ()
39 {
42 
43  vel_ = new double[numchannels_];
44  if (vel_ == NULL) {
45  fprintf(stderr,"vrpn_inertiamouse::vrpn_inertiamouse(): Out of memory\n");
46  }
47 
48  clear_values();
49 
51 }
52 
53 // factory method
55 vrpn_inertiamouse::create (const char* name,
56  vrpn_Connection * c,
57  const char* port,
58  int baud_rate)
59 {
60  return new vrpn_inertiamouse (name, c, port, baud_rate);
61 }
62 
63 
65 {
66  int i;
67 
68  for (i = 0; i < numbuttons_; i++) {
70  }
71  for (i = 0; i < numchannels_; i++) {
73  }
74 }
75 
77 {
78  clear_values();
80 
82 
83  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
84  return 0;
85 }
86 
87 
89 {
90  int ret; // Return value from function call to be checked
91 
92  //
93  // If we're SYNCing, then the next character we get should be the
94  // start of a report. If we recognize it, go into READing mode
95  // and tell how many characters we expect total. If we don't
96  // recognize it, then we must have misinterpreted a command or
97  // something; reset the intertiamouse and start over
98  //
99  if (status_ == STATUS_SYNCING) {
100  // Try to get a character. If none, just return.
102  if (ret != 1) { return 0; }
103 
104  switch (buffer_[0]) {
105  case 'D':
106  expected_chars_ = 25;
107  break;
108  case 'B':
109  expected_chars_ = 4;
110  break;
111 
112  default:
113  fprintf(stderr, "vrpn_inertiamouse: Unknown command (%c), resetting\n", buffer_[0]);
115  return 0;
116  }
117 
118  bufcount_ = 1;
119  vrpn_gettimeofday (&timestamp, NULL);
120 
122  }
123 
125  &buffer_[bufcount_],
127  if (ret == -1) {
128  send_text_message("vrpn_inertiamouse: Error reading",
129  timestamp,
132  return 0;
133  }
134  bufcount_ += ret;
135 
136  if (bufcount_ < expected_chars_) { // Not done -- go back for more
137  return 0;
138  }
139 
140  if (buffer_[expected_chars_ - 1] != '\n') {
142  send_text_message("vrpn_inertiamouse: No newline in record",
143  timestamp,
145  return 0;
146  }
147 
148  switch ( buffer_[0] ) {
149  case 'D':
150  {
151  int i;
152  int nextchar;
153  for (i = 0, nextchar = 0; i < numchannels_; ++i) {
154  int packet;
155  packet = (buffer_[++nextchar] & 0xf0) << 8;
156  packet |= (buffer_[++nextchar] & 0xf0) << 4;
157  packet |= (buffer_[++nextchar] & 0xf0);
158  packet |= (buffer_[++nextchar] & 0xf0) >> 4;
159 
160  int chnl = (packet >> 10) & 7;
161  if (chnl >= Channels) {
163  send_text_message("vrpn_inertiamouse: Too-large channel value",
164  timestamp,
166  return 0;
167  }
168  int acc = packet & 0x3ff; // 10 bits
169 
170  // normalize to interval [-1,1]
171  // just a guess, block dc later
172  double normval = ((double)(acc - 256) /
173  (double)256);
174 
175 // normval *= 1.5;
176 
177  // update rotation data
178  if ( (chnl == 4) || (chnl == 5) ) {
179  channel[chnl] = normval;
180  break;
181  }
182  normval = dcb_[chnl].filter (normval);
183  normval = lp_[chnl].filter (normval);
184 
185  double dt = 0.25;
186 
187  // update velocity and position only when button[0] pressed
188  if (buttons[0]) {
189 
190  double pos = vel_[chnl] * dt + normval * dt * dt / 2;
191 
192  vel_[chnl] += normval*dt;
193  if(fabs (vel_[chnl]) < dt/2.0)
194  vel_[chnl] *= 0.90;
195 // else
196 // if (fabs (vel_[chnl]) > 1.0)
197 // vel_[chnl] *= 1.0 / fabs (vel_[chnl]);
198 
199  channel[chnl] = pos;
200 // channel[chnl] *= 0.95;
201  } else {
202  vel_[chnl] = 0.0;
203  channel[chnl] = 0.0;
204  }
205  }
206  }
207  break;
208  case 'B':
209  buttons[0] = ((buffer_[1] & 1) != 0);
210  buttons[1] = ((buffer_[1] & 2) != 0);
211  break;
212  default:
213  fprintf(stderr, "vrpn_inertiamouse: Unknown [internal] command (%c), resetting\n", buffer_[0]);
215  return 0;
216  }
217 
218  //
219  // Done with the decoding, send the reports and go back to syncing
220  //
221 
222  report_changes();
224  bufcount_ = 0;
225 
226  return 1; // We got a full report.
227 }
228 
229 void vrpn_inertiamouse::report_changes(vrpn_uint32 class_of_service)
230 {
233 
234  vrpn_Analog::report_changes(class_of_service);
236 }
237 
238 void vrpn_inertiamouse::report(vrpn_uint32 class_of_service)
239 {
242 
243  vrpn_Analog::report(class_of_service);
245 }
246 
247 // This routine is called each time through the server's main loop. It
248 // will take a course of action depending on the current status of the
249 // intertiamouse, either trying to reset it or trying to get a reading
250 // from it.
252 {
253  server_mainloop();
254 
255  switch(status_) {
256  case STATUS_RESETTING:
257  reset();
258  break;
259 
260  case STATUS_SYNCING:
261  case STATUS_READING:
262  // Keep getting reports until all full reports are read.
263  while (get_report());
264  break;
265 
266  default:
267  fprintf(stderr, "vrpn_inertiamouse: Unknown mode (internal error)\n");
268  break;
269  }
270 }
virtual int reset(void)
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
virtual void report_changes(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report only if something has changed (for servers) Optionally, tell what time to stamp the val...
Definition: vrpn_Analog.C:71
static vrpn_inertiamouse * create(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
virtual int get_report(void)
Try to read a report from the device. Returns 1 if complete report received, 0 otherwise....
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
#define STATUS_SYNCING
vrpn_inertiamouse(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
struct timeval timestamp
double filter(double s)
Generic connection class not specific to the transport mechanism.
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY, const struct timeval time=vrpn_ANALOG_NOW)
Send a report whether something has changed or not (for servers) Optionally, tell what time to stamp ...
Definition: vrpn_Analog.C:94
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
virtual void report_changes(void)
Definition: vrpn_Button.C:382
dcblocker dcb_[Channels]
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
double filter(double s)
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
static const double Vel_Decay
virtual void mainloop()
Called once through each main loop iteration to handle updates.
int send_text_message(const char *msg, struct timeval timestamp, vrpn_TEXT_SEVERITY type=vrpn_TEXT_NORMAL, vrpn_uint32 level=0)
Sends a NULL-terminated text message from the device d_sender_id.
virtual void clear_values(void)
struct timeval timestamp
Definition: vrpn_Button.h:48
#define STATUS_RESETTING
#define MAX_TIME_INTERVAL
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
unsigned char buffer_[512]
lowpass lp_[Channels]
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65
#define STATUS_READING
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39