vrpn 07.35
Virtual Reality Peripheral Network
 
Loading...
Searching...
No Matches
vrpn_Flock_Parallel.C
Go to the documentation of this file.
1/*****************************************************************************\
2 vrpn_Flock_Parallel.C
3 --
4 Description : A master and slave class are defined in this file.
5 The master class connects to a flock controller and
6 manages the slave receivers.
7
8 ----------------------------------------------------------------------------
9 Author: weberh
10 Created: Thu Mar 5 19:38:55 1998
11 Revised: Fri Mar 19 15:05:56 1999 by weberh
12\*****************************************************************************/
13
14// The structure of this code came from vrpn_3Space.[Ch]
15// Most of the flock specific code comes from snippets in:
16// ~hmd/src/libs/tracker/apps/ascension/FLOCK232/C
17// That directory includes a program called cbird which allows you
18// to send almost any command to the flock.
19// If you are having trouble with the flock, compile and run that program.
20// Things to remember:
21// Drain or flush i/o buffers as appropriate
22// If the flock is in stream mode ('@'), put it into point mode ('B')
23// before trying to send other commands
24// Even if you are running in group mode, you need to preface data
25// specification commands with the RS232TOFBB command.
26// (weberh 1/11/98)
27
28// If you want to try polling instead of stream mode, just set define POLL
29// #define POLL
30
31#include <stdio.h> // for fprintf, stderr, NULL, etc
32
33#include "vrpn_BaseClass.h" // for ::vrpn_TEXT_ERROR
34#include "vrpn_Flock_Parallel.h"
35#include "vrpn_Serial.h" // for vrpn_drain_output_buffer, etc
36#include "vrpn_Shared.h" // for timeval, vrpn_SleepMsecs, etc
37#include "vrpn_Tracker.h" // for vrpn_TRACKER_FAIL, etc
38
40
41// output a status msg every status_msg_secs
42#define STATUS_MSG
43#define STATUS_MSG_SECS 600
44
45// we create a vrpn_Tracker_Flock in polling mode as the master
48 int cSensors,
49 char *port,
50 long baud,
51 char *slavePortArray[],
52 bool invertQuaternion) :
53 vrpn_Tracker_Flock(name,c,cSensors,port,baud,0, 1, invertQuaternion)
54{
55 if (cSensors<=0) {
56 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: must ask for pos num "
57 "of sensors");
58 cSensors = 0;
59 return;
60 }
61 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: starting up ...");
62
63 if (!slavePortArray) {
64 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: null slave port array.");
65 return;
66 }
67
68 for (int i=0;i<cSensors;i++) {
69 char rgch[15];
70 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: initing slave %d ...", i);
71 if (!slavePortArray[i]) {
72 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel:slave %d: null port array"
73 " entry.", i);
74 return;
75 } else {
76 // create a traker name
77 sprintf(rgch, "flockSlave%d", i);
80 slavePortArray[i],
81 baud,
83 i );
84 }
85 }
86}
87
89
90 // have all slaves shut down (they just send a 'B' -- slaves
91 // can't run or sleep
92 for (int i=0;i<cSensors;i++) {
93 try {
94 delete rgSlaves[i];
95 } catch (...) {
96 fprintf(stderr, "vrpn_Tracker_Flock_Parallel::~vrpn_Tracker_Flock_Parallel(): delete failed\n");
97 return;
98 }
99 }
100
101 // now regular flock destructor will be called (just as we need)
102}
103
104// reset is a little different because we don't want it to go into
105// stream mode.
107{
108 // call the base class reset (won't stream -- this is a polling
109 // vrpn_Tracker_Flock)
111
112 // Now, call slave reset(s)
113 for (int i=0;i<cSensors;i++) {
114 rgSlaves[i]->reset();
115 }
116}
117
119{
120 // this could either do nothing or call slave get_report(s)
121 return 0;
122}
123
125{
126 int i;
127
128 // Call the generic server code, since we are a server.
130
131 // call slave mainloops
132 for (i=0;i<cSensors;i++) {
133 rgSlaves[i]->mainloop();
134 }
135
136 // check slave status (master fails if any slave does
137 // and the master resets the slaves)
138 for (i=0;i<cSensors;i++) {
139 // first check for failure
142 break;
143 }
144 // now check for reset being needed (cont to check for failures)
147 continue;
148 }
149 }
150
151 // the master never has reports ready -- it is always either in
152 // fail, reset, or sync mode
153 // The slaves send messages on the master's connection
154 switch (status) {
156 // everything is a-ok
157 break;
158
160 reset();
161 break;
162
164 checkError();
165 if (cResets==4) {
166 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel: problems resetting ... check that: a) all cables are attached, b) all units have FLY/STANDBY switches in FLY mode, and c) no receiver is laying too close to the transmitter. When done checking, power cycle the flock.\nWill attempt to reset in 15 seconds.\n");
167 vrpn_SleepMsecs(1000.0*15);
168 cResets=0;
169 }
170 fprintf(stderr,
171 "\nvrpn_Tracker_Flock_Parallel: tracker failed, trying to reset ...");
172
173 // reset master serial i/o
176
177 // reset slave serial i/o
178 for (i=0;i<cSensors;i++) {
180 rgSlaves[i]->serial_fd = vrpn_open_commport(rgSlaves[i]->portname,
181 rgSlaves[i]->baudrate);
182 rgSlaves[i]->status = vrpn_TRACKER_RESETTING;
183 }
185 break;
186 }
187}
188
189/*******************************************************************************
190 HERE IS THE VRPN SLAVE CODE
191*******************************************************************************/
192
193
194// stream mode flock, one sensor, same connection as master
195// it will send faked messages from the master
198 char *port, long baud,
199 vrpn_int32 masterID,
200 int iSensorID ) :
201 vrpn_Tracker_Flock(name,c,1,port,baud,1)
202{
203 d_sender_id = masterID; // Spoofing the master
204 d_sensor=iSensorID;
205 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel_Slave %d: starting up ...",
206 d_sensor);
207 // get_report should operate in non-group mode
208 fGroupMode = 0;
209}
210
212
213 // Some compilers (CenterLine CC on sunos!?) still don't support
214 // automatic aggregate initialization
215
216 int cLen=0;
217 //unsigned char rgch[2]={'B','G'};
218 unsigned char rgch [2];
219 rgch[cLen++] = 'B';
220
221 // slaves can't do a sleep
222 // rgch[cLen++] = 'G';
223
224 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: shutting down ...", d_sensor);
225 // clear output buffer
227
228 // put the flock to sleep (B to get out of stream mode, G to sleep)
229 if (vrpn_write_characters(serial_fd, (const unsigned char *) rgch, cLen )!=cLen) {
230 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
231 "failed writing sleep cmd to tracker", d_sensor);
233 return;
234 }
235 // make sure the command is sent out
237 fprintf(stderr, " done.\n");
238}
239
240#define poll() { \
241char chPoint = 'B';\
242fprintf(stderr,"."); \
243if (vrpn_write_characters(serial_fd, (const unsigned char *) &chPoint, 1 )!=1) {\
244 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: failed writing set mode cmds to tracker", d_sensor);\
245 status = vrpn_TRACKER_FAIL;\
246 return;\
247} \
248vrpn_gettimeofday(&timestamp, NULL);\
249}
250
251
252// slave resets are ONLY called by the master
254{
255 // slaves just flush on a reset and then stream (or poll)
256 // master does all the real resetting
257
258 // set vars for error handling
259 // set them right away so they are set properly in the
260 // event that we fail during the reset.
261 cResets++;
262 cSyncs=0;
264
265 // Get rid of the characters left over from before the reset
266 // (make sure they are processed)
268
269 // put back into polled mode (need to stop stream mode
270 // before doing an auto-config)
271 int resetLen=0;
272 unsigned char reset[3];
273 reset[resetLen++]='B';
274
275 // send the poll mode command (cmd and cmd_size are args)
276 if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
277 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
278 "failed writing poll cmd to tracker", d_sensor);
280 return;
281 }
282
283 // make sure the command is sent out
285
286 // wait for tracker to respond and flush buffers
287 vrpn_SleepMsecs(500);
288
289 // clear the input buffer (it will contain a single point
290 // record from the poll command above and garbage from before reset)
292
293 // now start it running
294 resetLen = 0;
295
296 // either stream or let poll take place later
297 if (fStream==1) {
298 // stream mode
299 reset[resetLen++] = '@';
300
301 if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
302 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
303 "failed writing set mode cmds to tracker", d_sensor);
305 return;
306 }
307
308 // make sure the commands are sent out
310 } else {
311 poll();
312 }
313
314 fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
315 "done with reset ... running.\n", d_sensor);
316
317 vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
318 status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
319}
320
321// max time between start of a report and the finish (or time to
322// wait for first report)
323
324// Allow enough time for startup of many sensors -- 1 second per sensor
325#define MAX_TIME_INTERVAL (VRPN_FLOCK_MAX_SENSORS*1000000)
326
328{
329 // We don't call the generic server mainloop code, because the master unit
330 // will have done that for us.
331
332 switch (status) {
335 {
336 // It turns out to be important to get the report before checking
337 // to see if it has been too long since the last report. This is
338 // because there is the possibility that some other device running
339 // in the same server may have taken a long time on its last pass
340 // through mainloop(). Trackers that are resetting do this. When
341 // this happens, you can get an infinite loop -- where one tracker
342 // resets and causes the other to timeout, and then it returns the
343 // favor. By checking for the report here, we reset the timestamp
344 // if there is a report ready (ie, if THIS device is still operating).
345 while (get_report()) {
346 send_report();
347 }
348 struct timeval current_time;
349 vrpn_gettimeofday(&current_time, NULL);
350 if ( vrpn_TimevalDuration(current_time,timestamp) > MAX_TIME_INTERVAL) {
351 fprintf(stderr,"Tracker failed to read... current_time=%ld:%ld, timestamp=%ld:%ld\n",
352 current_time.tv_sec, static_cast<long>(current_time.tv_usec),
353 timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
354 send_text_message("Too long since last report, resetting", current_time, vrpn_TEXT_ERROR);
356 }
357 }
358 break;
359 // master resets us
361 break;
363 // here we just fail and let the master figure out that we have
364 // failed and need to be reset
365 fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel_Slave %d: tracker "
366 "failed, trying to reset ...", d_sensor);
367 break;
368 }
369}
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...
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.
Generic connection class not specific to the transport mechanism.
virtual void reset()
Reset the tracker.
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
vrpn_Tracker_Flock_Parallel_Slave(char *name, vrpn_Connection *c, char *port, long baud, vrpn_int32 vrpnMasterID, int iSensorID)
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
virtual void reset()
Reset the tracker.
vrpn_Tracker_Flock_Parallel(char *name, vrpn_Connection *c, int cSensors, char *port, long baud, char *slavePortArray[], bool invertQuaternion=false)
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
vrpn_Tracker_Flock_Parallel_Slave * rgSlaves[VRPN_FLOCK_MAX_SENSORS]
virtual void reset()
Reset the tracker.
Definition vrpn_Flock.C:303
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
Definition vrpn_Flock.C:532
vrpn_Tracker_Flock(char *name, vrpn_Connection *c, int cSensors=1, const char *port="/dev/ttyd3", long baud=38400, int fStreamMode=1, int useERT=1, bool invertQuaternion=false, int active_hemisphere=HEMI_PLUSZ)
Definition vrpn_Flock.C:216
virtual void send_report(void)
Definition vrpn_Flock.C:711
char portname[VRPN_TRACKER_BUF_SIZE]
vrpn_int32 d_sensor
struct timeval timestamp
#define MAX_TIME_INTERVAL
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
@ vrpn_TEXT_ERROR
#define VRPN_API
#define poll()
Definition vrpn_Flock.C:688
class VRPN_API vrpn_Tracker_Flock_Parallel_Slave
class VRPN_API vrpn_Connection
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
int vrpn_close_commport(int comm)
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
int vrpn_open_commport(const char *portname, long baud, int charsize, vrpn_SER_PARITY parity, bool rts_flow)
Open a serial port, given its name and baud rate.
Definition vrpn_Serial.C:54
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
void vrpn_SleepMsecs(double dMilliSecs)
#define vrpn_gettimeofday
Definition vrpn_Shared.h:99
const int vrpn_TRACKER_FAIL
const int vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_PARTIAL