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 }
vrpn_Serial_Analog
Definition: vrpn_Analog.h:63
vrpn_inertiamouse::dcb_
dcblocker dcb_[Channels]
Definition: vrpn_inertiamouse.h:142
vrpn_Button::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:422
vrpn_BaseClass.h
vrpn_Button_Filter::report_changes
virtual void report_changes(void)
Definition: vrpn_Button.C:382
vrpn_Analog::channel
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
vrpn_inertiamouse::bufcount_
int bufcount_
Definition: vrpn_inertiamouse.h:133
vrpn_inertiamouse::mainloop
virtual void mainloop()
Called once through each main loop iteration to handle updates.
Definition: vrpn_inertiamouse.C:251
vrpn_Analog::report
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
vrpn_inertiamouse::create
static vrpn_inertiamouse * create(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
Definition: vrpn_inertiamouse.C:55
vrpn_inertiamouse::reset
virtual int reset(void)
Definition: vrpn_inertiamouse.C:76
STATUS_SYNCING
#define STATUS_SYNCING
Definition: vrpn_3DMicroscribe.C:27
vrpn_Analog::timestamp
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_inertiamouse::vel_
double * vel_
Definition: vrpn_inertiamouse.h:140
vrpn_inertiamouse::lp_
lowpass lp_[Channels]
Definition: vrpn_inertiamouse.h:143
vrpn_Serial.h
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Button::num_buttons
vrpn_int32 num_buttons
Definition: vrpn_Button.h:47
vrpn_Button::buttons
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:44
vrpn_inertiamouse::status_
int status_
Definition: vrpn_inertiamouse.h:127
vrpn_inertiamouse::Vel_Decay
static const double Vel_Decay
Definition: vrpn_inertiamouse.h:102
dcblocker::filter
double filter(double s)
Definition: vrpn_inertiamouse.h:40
vrpn_TEXT_ERROR
@ vrpn_TEXT_ERROR
Definition: vrpn_BaseClass.h:103
vrpn_flush_input_buffer
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
vrpn_inertiamouse::Channels
@ Channels
Definition: vrpn_inertiamouse.h:98
vrpn_Shared.h
vrpn_inertiamouse::report
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
Definition: vrpn_inertiamouse.C:238
vrpn_inertiamouse::timestamp
struct timeval timestamp
Definition: vrpn_inertiamouse.h:138
vrpn_Button::timestamp
struct timeval timestamp
Definition: vrpn_Button.h:48
vrpn_inertiamouse::vrpn_inertiamouse
vrpn_inertiamouse(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
Definition: vrpn_inertiamouse.C:26
vrpn_Serial_Analog::serial_fd
int serial_fd
Definition: vrpn_Analog.h:72
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
MAX_TIME_INTERVAL
#define MAX_TIME_INTERVAL
Definition: vrpn_3DMicroscribe.C:29
vrpn_inertiamouse::numchannels_
int numchannels_
Definition: vrpn_inertiamouse.h:129
vrpn_Analog::num_channel
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
lowpass::filter
double filter(double s)
Definition: vrpn_inertiamouse.h:77
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
STATUS_RESETTING
#define STATUS_RESETTING
Definition: vrpn_3DMicroscribe.C:26
vrpn_Button::lastbuttons
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
vrpn_read_available_characters
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
vrpn_inertiamouse::get_report
virtual int get_report(void)
Try to read a report from the device. Returns 1 if complete report received, 0 otherwise....
Definition: vrpn_inertiamouse.C:88
vrpn_Analog::last
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:39
vrpn_inertiamouse.h
vrpn_BaseClassUnique::send_text_message
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.
Definition: vrpn_BaseClass.C:568
vrpn_inertiamouse
Definition: vrpn_inertiamouse.h:94
vrpn_inertiamouse::numbuttons_
int numbuttons_
Definition: vrpn_inertiamouse.h:128
vrpn_Analog::report_changes
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
vrpn_inertiamouse::clear_values
virtual void clear_values(void)
Definition: vrpn_inertiamouse.C:64
STATUS_READING
#define STATUS_READING
Definition: vrpn_3DMicroscribe.C:28
vrpn_inertiamouse::expected_chars_
int expected_chars_
Definition: vrpn_inertiamouse.h:131
vrpn_inertiamouse::buffer_
unsigned char buffer_[512]
Definition: vrpn_inertiamouse.h:132
vrpn_BaseClassUnique::server_mainloop
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
Definition: vrpn_BaseClass.C:603
vrpn_Button_Filter
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:65