vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
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
15const double vrpn_inertiamouse::Vel_Decay = 0.66;
16
17namespace {
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
28 const char* port,
29 int baud_rate)
30 : vrpn_Serial_Analog (name, c, port, baud_rate)
31 , vrpn_Button_Filter (name, c)
34 , expected_chars_ (1)
35 , bufcount_ (0)
36 , null_radius_ (0)
37 , dcb_ ()
38 , lp_ ()
39{
42
43 vel_ = new double[numchannels_];
44
45 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
46
48
50}
51
52// factory method
54vrpn_inertiamouse::create (const char* name,
56 const char* port,
57 int baud_rate)
58{
59 vrpn_inertiamouse *ret = NULL;
60 try { ret = new vrpn_inertiamouse(name, c, port, baud_rate); }
61 catch (...) { return NULL; }
62 return ret;
63}
64
65
67{
68 int i;
69
70 for (i = 0; i < numbuttons_; i++) {
72 }
73 for (i = 0; i < numchannels_; i++) {
75 }
76}
77
79{
82
84
85 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
86 return 0;
87}
88
89
91{
92 int ret; // Return value from function call to be checked
93
94 //
95 // If we're SYNCing, then the next character we get should be the
96 // start of a report. If we recognize it, go into READing mode
97 // and tell how many characters we expect total. If we don't
98 // recognize it, then we must have misinterpreted a command or
99 // something; reset the intertiamouse and start over
100 //
101 if (status_ == STATUS_SYNCING) {
102 // Try to get a character. If none, just return.
104 if (ret != 1) { return 0; }
105
106 switch (buffer_[0]) {
107 case 'D':
108 expected_chars_ = 25;
109 break;
110 case 'B':
111 expected_chars_ = 4;
112 break;
113
114 default:
115 fprintf(stderr, "vrpn_inertiamouse: Unknown command (%c), resetting\n", buffer_[0]);
117 return 0;
118 }
119
120 bufcount_ = 1;
122
124 }
125
129 if (ret == -1) {
130 send_text_message("vrpn_inertiamouse: Error reading",
131 timestamp,
134 return 0;
135 }
136 bufcount_ += ret;
137
138 if (bufcount_ < expected_chars_) { // Not done -- go back for more
139 return 0;
140 }
141
142 if (buffer_[expected_chars_ - 1] != '\n') {
144 send_text_message("vrpn_inertiamouse: No newline in record",
145 timestamp,
147 return 0;
148 }
149
150 switch ( buffer_[0] ) {
151 case 'D':
152 {
153 int i;
154 int nextchar;
155 for (i = 0, nextchar = 0; i < numchannels_; ++i) {
156 int packet;
157 packet = (buffer_[++nextchar] & 0xf0) << 8;
158 packet |= (buffer_[++nextchar] & 0xf0) << 4;
159 packet |= (buffer_[++nextchar] & 0xf0);
160 packet |= (buffer_[++nextchar] & 0xf0) >> 4;
161
162 int chnl = (packet >> 10) & 7;
163 if (chnl >= Channels) {
165 send_text_message("vrpn_inertiamouse: Too-large channel value",
166 timestamp,
168 return 0;
169 }
170 int acc = packet & 0x3ff; // 10 bits
171
172 // normalize to interval [-1,1]
173 // just a guess, block dc later
174 double normval = ((double)(acc - 256) /
175 (double)256);
176
177// normval *= 1.5;
178
179 // update rotation data
180 if ( (chnl == 4) || (chnl == 5) ) {
181 channel[chnl] = normval;
182 break;
183 }
184 normval = dcb_[chnl].filter (normval);
185 normval = lp_[chnl].filter (normval);
186
187 double dt = 0.25;
188
189 // update velocity and position only when button[0] pressed
190 if (buttons[0]) {
191
192 double pos = vel_[chnl] * dt + normval * dt * dt / 2;
193
194 vel_[chnl] += normval*dt;
195 if(fabs (vel_[chnl]) < dt/2.0)
196 vel_[chnl] *= 0.90;
197// else
198// if (fabs (vel_[chnl]) > 1.0)
199// vel_[chnl] *= 1.0 / fabs (vel_[chnl]);
200
201 channel[chnl] = pos;
202// channel[chnl] *= 0.95;
203 } else {
204 vel_[chnl] = 0.0;
205 channel[chnl] = 0.0;
206 }
207 }
208 }
209 break;
210 case 'B':
211 buttons[0] = ((buffer_[1] & 1) != 0);
212 buttons[1] = ((buffer_[1] & 2) != 0);
213 break;
214 default:
215 fprintf(stderr, "vrpn_inertiamouse: Unknown [internal] command (%c), resetting\n", buffer_[0]);
217 return 0;
218 }
219
220 //
221 // Done with the decoding, send the reports and go back to syncing
222 //
223
226 bufcount_ = 0;
227
228 return 1; // We got a full report.
229}
230
239
240void vrpn_inertiamouse::report(vrpn_uint32 class_of_service)
241{
244
245 vrpn_Analog::report(class_of_service);
247}
248
249// This routine is called each time through the server's main loop. It
250// will take a course of action depending on the current status of the
251// intertiamouse, either trying to reset it or trying to get a reading
252// from it.
254{
256
257 switch(status_) {
258 case STATUS_RESETTING:
259 reset();
260 break;
261
262 case STATUS_SYNCING:
263 case STATUS_READING:
264 // Keep getting reports until all full reports are read.
265 while (get_report());
266 break;
267
268 default:
269 fprintf(stderr, "vrpn_inertiamouse: Unknown mode (internal error)\n");
270 break;
271 }
272}
vrpn_float64 last[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:39
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition vrpn_Analog.h:38
struct timeval timestamp
Definition vrpn_Analog.h:41
vrpn_int32 num_channel
Definition vrpn_Analog.h:40
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
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
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
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 report_changes(void)
vrpn_Button_Filter(const char *, vrpn_Connection *c=NULL)
vrpn_int32 num_buttons
Definition vrpn_Button.h:48
struct timeval timestamp
Definition vrpn_Button.h:49
virtual void report_changes(void)
unsigned char lastbuttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:46
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
vrpn_Serial_Analog(const char *name, vrpn_Connection *connection, const char *port, int baud=9600, int bits=8, vrpn_SER_PARITY parity=vrpn_SER_PARITY_NONE, bool rts_flow=false)
static vrpn_inertiamouse * create(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
virtual void report(vrpn_uint32 class_of_service=vrpn_CONNECTION_LOW_LATENCY)
send report whether or not changed
virtual void clear_values(void)
virtual void mainloop()
Called once through each main loop iteration to handle updates.
static const double Vel_Decay
vrpn_inertiamouse(const char *name, vrpn_Connection *c, const char *port, int baud_rate)
lowpass lp_[Channels]
virtual int get_report(void)
Try to read a report from the device. Returns 1 if complete report received, 0 otherwise....
dcblocker dcb_[Channels]
unsigned char buffer_[512]
virtual int reset(void)
struct timeval timestamp
#define STATUS_SYNCING
#define MAX_TIME_INTERVAL
#define STATUS_READING
#define STATUS_RESETTING
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99