vrpn  07.33
Virtual Reality Peripheral Network
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
47  vrpn_Connection *c,
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  delete rgSlaves[i];
94  }
95 
96  // now regular flock destructor will be called (just as we need)
97 }
98 
99 // reset is a little different because we don't want it to go into
100 // stream mode.
102 {
103  // call the base class reset (won't stream -- this is a polling
104  // vrpn_Tracker_Flock)
106 
107  // Now, call slave reset(s)
108  for (int i=0;i<cSensors;i++) {
109  rgSlaves[i]->reset();
110  }
111 }
112 
114 {
115  // this could either do nothing or call slave get_report(s)
116  return 0;
117 }
118 
120 {
121  int i;
122 
123  // Call the generic server code, since we are a server.
124  server_mainloop();
125 
126  // call slave mainloops
127  for (i=0;i<cSensors;i++) {
128  rgSlaves[i]->mainloop();
129  }
130 
131  // check slave status (master fails if any slave does
132  // and the master resets the slaves)
133  for (i=0;i<cSensors;i++) {
134  // first check for failure
135  if (rgSlaves[i]->status==vrpn_TRACKER_FAIL) {
137  break;
138  }
139  // now check for reset being needed (cont to check for failures)
142  continue;
143  }
144  }
145 
146  // the master never has reports ready -- it is always either in
147  // fail, reset, or sync mode
148  // The slaves send messages on the master's connection
149  switch (status) {
151  // everything is a-ok
152  break;
153 
155  reset();
156  break;
157 
158  case vrpn_TRACKER_FAIL:
159  checkError();
160  if (cResets==4) {
161  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");
162  vrpn_SleepMsecs(1000.0*15);
163  cResets=0;
164  }
165  fprintf(stderr,
166  "\nvrpn_Tracker_Flock_Parallel: tracker failed, trying to reset ...");
167 
168  // reset master serial i/o
171 
172  // reset slave serial i/o
173  for (i=0;i<cSensors;i++) {
176  rgSlaves[i]->baudrate);
178  }
180  break;
181  }
182 }
183 
184 /*******************************************************************************
185  HERE IS THE VRPN SLAVE CODE
186 *******************************************************************************/
187 
188 
189 // stream mode flock, one sensor, same connection as master
190 // it will send faked messages from the master
193  char *port, long baud,
194  vrpn_int32 masterID,
195  int iSensorID ) :
196  vrpn_Tracker_Flock(name,c,1,port,baud,1)
197 {
198  d_sender_id = masterID; // Spoofing the master
199  d_sensor=iSensorID;
200  fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel_Slave %d: starting up ...",
201  d_sensor);
202  // get_report should operate in non-group mode
203  fGroupMode = 0;
204 }
205 
207 
208  // Some compilers (CenterLine CC on sunos!?) still don't support
209  // automatic aggregate initialization
210 
211  int cLen=0;
212  //unsigned char rgch[2]={'B','G'};
213  unsigned char rgch [2];
214  rgch[cLen++] = 'B';
215 
216  // slaves can't do a sleep
217  // rgch[cLen++] = 'G';
218 
219  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: shutting down ...", d_sensor);
220  // clear output buffer
222 
223  // put the flock to sleep (B to get out of stream mode, G to sleep)
224  if (vrpn_write_characters(serial_fd, (const unsigned char *) rgch, cLen )!=cLen) {
225  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
226  "failed writing sleep cmd to tracker", d_sensor);
228  return;
229  }
230  // make sure the command is sent out
232  fprintf(stderr, " done.\n");
233 }
234 
235 #define poll() { \
236 char chPoint = 'B';\
237 fprintf(stderr,"."); \
238 if (vrpn_write_characters(serial_fd, (const unsigned char *) &chPoint, 1 )!=1) {\
239  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: failed writing set mode cmds to tracker", d_sensor);\
240  status = vrpn_TRACKER_FAIL;\
241  return;\
242 } \
243 vrpn_gettimeofday(&timestamp, NULL);\
244 }
245 
246 
247 // slave resets are ONLY called by the master
249 {
250  // slaves just flush on a reset and then stream (or poll)
251  // master does all the real resetting
252 
253  // set vars for error handling
254  // set them right away so they are set properly in the
255  // event that we fail during the reset.
256  cResets++;
257  cSyncs=0;
259 
260  // Get rid of the characters left over from before the reset
261  // (make sure they are processed)
263 
264  // put back into polled mode (need to stop stream mode
265  // before doing an auto-config)
266  int resetLen=0;
267  unsigned char reset[3];
268  reset[resetLen++]='B';
269 
270  // send the poll mode command (cmd and cmd_size are args)
271  if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
272  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
273  "failed writing poll cmd to tracker", d_sensor);
275  return;
276  }
277 
278  // make sure the command is sent out
280 
281  // wait for tracker to respond and flush buffers
282  vrpn_SleepMsecs(500);
283 
284  // clear the input buffer (it will contain a single point
285  // record from the poll command above and garbage from before reset)
287 
288  // now start it running
289  resetLen = 0;
290 
291  // either stream or let poll take place later
292  if (fStream==1) {
293  // stream mode
294  reset[resetLen++] = '@';
295 
296  if (vrpn_write_characters(serial_fd, (const unsigned char *) reset, resetLen )!=resetLen) {
297  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
298  "failed writing set mode cmds to tracker", d_sensor);
300  return;
301  }
302 
303  // make sure the commands are sent out
305  } else {
306  poll();
307  }
308 
309  fprintf(stderr,"\nvrpn_Tracker_Flock_Parallel_Slave %d: "
310  "done with reset ... running.\n", d_sensor);
311 
312  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
313  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
314 }
315 
316 // max time between start of a report and the finish (or time to
317 // wait for first report)
318 
319 // Allow enough time for startup of many sensors -- 1 second per sensor
320 #define MAX_TIME_INTERVAL (VRPN_FLOCK_MAX_SENSORS*1000000)
321 
323 {
324  // We don't call the generic server mainloop code, because the master unit
325  // will have done that for us.
326 
327  switch (status) {
330  {
331  // It turns out to be important to get the report before checking
332  // to see if it has been too long since the last report. This is
333  // because there is the possibility that some other device running
334  // in the same server may have taken a long time on its last pass
335  // through mainloop(). Trackers that are resetting do this. When
336  // this happens, you can get an infinite loop -- where one tracker
337  // resets and causes the other to timeout, and then it returns the
338  // favor. By checking for the report here, we reset the timestamp
339  // if there is a report ready (ie, if THIS device is still operating).
340  while (get_report()) {
341  send_report();
342  }
343  struct timeval current_time;
344  vrpn_gettimeofday(&current_time, NULL);
345  if ( vrpn_TimevalDuration(current_time,timestamp) > MAX_TIME_INTERVAL) {
346  fprintf(stderr,"Tracker failed to read... current_time=%ld:%ld, timestamp=%ld:%ld\n",
347  current_time.tv_sec, static_cast<long>(current_time.tv_usec),
348  timestamp.tv_sec, static_cast<long>(timestamp.tv_usec));
349  send_text_message("Too long since last report, resetting", current_time, vrpn_TEXT_ERROR);
351  }
352  }
353  break;
354  // master resets us
356  break;
357  case vrpn_TRACKER_FAIL:
358  // here we just fail and let the master figure out that we have
359  // failed and need to be reset
360  fprintf(stderr, "\nvrpn_Tracker_Flock_Parallel_Slave %d: tracker "
361  "failed, trying to reset ...", d_sensor);
362  break;
363  }
364 }
vrpn_Tracker_Serial::portname
char portname[VRPN_TRACKER_BUF_SIZE]
Definition: vrpn_Tracker.h:151
vrpn_Tracker.h
vrpn_Tracker_Flock::cSensors
int cSensors
Definition: vrpn_Flock.h:57
vrpn_BaseClass.h
vrpn_Tracker_Flock_Parallel::rgSlaves
vrpn_Tracker_Flock_Parallel_Slave * rgSlaves[VRPN_FLOCK_MAX_SENSORS]
Definition: vrpn_Flock_Parallel.h:67
vrpn_TimevalDuration
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
vrpn_Tracker_Flock_Parallel_Slave::vrpn_Tracker_Flock_Parallel_Slave
vrpn_Tracker_Flock_Parallel_Slave(char *name, vrpn_Connection *c, char *port, long baud, vrpn_int32 vrpnMasterID, int iSensorID)
Definition: vrpn_Flock_Parallel.C:192
vrpn_Tracker::d_sensor
vrpn_int32 d_sensor
Definition: vrpn_Tracker.h:94
vrpn_Tracker_Flock::fGroupMode
int fGroupMode
Definition: vrpn_Flock.h:59
vrpn_Tracker_Flock_Parallel::get_report
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
Definition: vrpn_Flock_Parallel.C:113
vrpn_drain_output_buffer
int vrpn_drain_output_buffer(int comm)
Wait until all of the characters in the output buffer are sent, then return.
Definition: vrpn_Serial.C:485
vrpn_Tracker_Serial::serial_fd
int serial_fd
Definition: vrpn_Tracker.h:153
MAX_TIME_INTERVAL
#define MAX_TIME_INTERVAL
Definition: vrpn_Flock_Parallel.C:320
vrpn_Tracker_Flock_Parallel::vrpn_Tracker_Flock_Parallel
vrpn_Tracker_Flock_Parallel(char *name, vrpn_Connection *c, int cSensors, char *port, long baud, char *slavePortArray[], bool invertQuaternion=false)
Definition: vrpn_Flock_Parallel.C:46
vrpn_Tracker_Flock_Parallel::~vrpn_Tracker_Flock_Parallel
virtual ~vrpn_Tracker_Flock_Parallel()
Definition: vrpn_Flock_Parallel.C:88
vrpn_Tracker::timestamp
struct timeval timestamp
Definition: vrpn_Tracker.h:100
vrpn_Serial.h
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_Tracker_Flock_Parallel_Slave::~vrpn_Tracker_Flock_Parallel_Slave
virtual ~vrpn_Tracker_Flock_Parallel_Slave()
Definition: vrpn_Flock_Parallel.C:206
vrpn_Tracker_Flock_Parallel_Slave
class VRPN_API vrpn_Tracker_Flock_Parallel_Slave
Definition: vrpn_Flock_Parallel.h:42
vrpn_BaseClassUnique::d_connection
vrpn_Connection * d_connection
Connection that this object talks to.
Definition: vrpn_BaseClass.h:224
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_SleepMsecs
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
vrpn_Shared.h
vrpn_TRACKER_PARTIAL
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Tracker_Flock::fFirstStatusReport
int fFirstStatusReport
Definition: vrpn_Flock.h:69
vrpn_BaseClassUnique::d_sender_id
vrpn_int32 d_sender_id
Sender ID registered with the connection.
Definition: vrpn_BaseClass.h:228
vrpn_Tracker_Flock::get_report
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
Definition: vrpn_Flock.C:525
vrpn_Tracker_Flock_Parallel_Slave::reset
virtual void reset()
Reset the tracker.
Definition: vrpn_Flock_Parallel.C:248
vrpn_Connection
Generic connection class not specific to the transport mechanism.
Definition: vrpn_Connection.h:510
vrpn_TRACKER_FAIL
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
vrpn_Tracker_Flock::cSyncs
unsigned cSyncs
Definition: vrpn_Flock.h:66
vrpn_TRACKER_RESETTING
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
poll
#define poll()
Definition: vrpn_Flock_Parallel.C:235
vrpn_gettimeofday
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
vrpn_Tracker::status
int status
Definition: vrpn_Tracker.h:129
vrpn_Tracker_Flock::fStream
int fStream
Definition: vrpn_Flock.h:58
vrpn_close_commport
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:345
vrpn_Tracker_Flock::cResets
unsigned cResets
Definition: vrpn_Flock.h:65
vrpn_TRACKER_SYNCING
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_Tracker_Serial::baudrate
long baudrate
Definition: vrpn_Tracker.h:152
vrpn_Tracker_Flock::checkError
int checkError()
Definition: vrpn_Flock.C:156
vrpn_Tracker_Flock::reset
virtual void reset()
Reset the tracker.
Definition: vrpn_Flock.C:296
vrpn_write_characters
int vrpn_write_characters(int comm, const unsigned char *buffer, size_t bytes)
Write the buffer to the serial port.
Definition: vrpn_Serial.C:643
vrpn_open_commport
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_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_Tracker_Flock_Parallel_Slave::mainloop
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
Definition: vrpn_Flock_Parallel.C:322
vrpn_Tracker_Flock_Parallel::mainloop
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
Definition: vrpn_Flock_Parallel.C:119
vrpn_Tracker_Flock_Parallel::reset
virtual void reset()
Reset the tracker.
Definition: vrpn_Flock_Parallel.C:101
vrpn_Tracker_Flock
Definition: vrpn_Flock.h:38
vrpn_flush_output_buffer
int vrpn_flush_output_buffer(int comm)
Throw out any characters (do not send) within the output buffer.
Definition: vrpn_Serial.C:462
VRPN_API
#define VRPN_API
Definition: vrpn_Configure.h:646
vrpn_Flock_Parallel.h
vrpn_Tracker_Flock::send_report
virtual void send_report(void)
Definition: vrpn_Flock.C:692
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