vrpn 07.35
Virtual Reality Peripheral Network
Loading...
Searching...
No Matches
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.
12int vrpn_Wanda::dbug_wanda = getenv("DBUG_WANDA") ? 1 : 0;
13
14// Returns time in seconds in a double.
15inline 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
22void
23print_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
32void 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
61void
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
71void
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}
vrpn_float64 channel[vrpn_CHANNEL_MAX]
Definition: vrpn_Analog.h:38
void print(void)
Definition: vrpn_Analog.C:42
virtual vrpn_int32 encode_to(char *buf)
Definition: vrpn_Analog.C:54
struct timeval timestamp
Definition: vrpn_Analog.h:41
vrpn_int32 num_channel
Definition: vrpn_Analog.h:40
vrpn_int32 channel_m_id
Definition: vrpn_Analog.h:42
vrpn_Connection * d_connection
Connection that this object talks to.
vrpn_int32 d_sender_id
Sender ID registered with the connection.
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
All button servers should derive from this class, which provides the ability to turn any of the butto...
Definition: vrpn_Button.h:66
vrpn_int32 num_buttons
Definition: vrpn_Button.h:48
virtual void report_changes(void)
Definition: vrpn_Button.C:423
unsigned char buttons[vrpn_BUTTON_MAX_BUTTONS]
Definition: vrpn_Button.h:45
Generic connection class not specific to the transport mechanism.
unsigned char buffer[1024]
Definition: vrpn_Analog.h:75
void report_new_button_info()
Definition: vrpn_Wanda.C:62
vrpn_Wanda(char *name, vrpn_Connection *c, char *portname, int baud, double)
Definition: vrpn_Wanda.C:37
void mainloop(void)
Called once through each main loop iteration to handle updates. Remote object mainloop() should call ...
Definition: vrpn_Wanda.C:96
void report_new_valuator_info()
Definition: vrpn_Wanda.C:72
#define MAX_TIME_INTERVAL
const int vrpn_ANALOG_RESETTING
Definition: vrpn_Analog.h:22
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:515
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
void print_bits(char *buf, int num_bytes)
Definition: vrpn_Wanda.C:23
double the_time()
Definition: vrpn_Wanda.C:15