vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Tracker_GPS.C
Go to the documentation of this file.
1 /*
2  *
3  * vrpn_tracker_GPS added to allow GPS tracking in outdoor AR experiences
4  *
5  */
6 
7 #include <stdio.h> // for printf, NULL, fread, etc
8 #include <string.h> // for strlen
9 
10 #ifndef _WIN32
11 #include <unistd.h> // for usleep, sleep
12 #endif
13 
14 #include "vrpn_BaseClass.h" // for ::vrpn_TEXT_WARNING, etc
15 #include "vrpn_Connection.h" // for vrpn_Connection
16 #include "vrpn_Serial.h"
17 #include "vrpn_Shared.h" // for timeval, vrpn_gettimeofday
18 #include "vrpn_Tracker.h" // for vrpn_TRACKER_SYNCING, etc
19 #include "vrpn_Tracker_GPS.h"
20 #include "vrpn_Types.h" // for vrpn_float64
21 #include "vrpn_MessageMacros.h" // for VRPN_MSG_INFO, VRPN_MSG_WARNING, VRPN_MSG_ERROR
22 
23 #define MAX_TIME_INTERVAL (5000000) // max time between reports (usec)
24 #define INCHES_TO_METERS (2.54/100.0)
25 #define PI (3.14159265358979323846)
26 
27 
28 //--------------------------------------------------
29 
30 
31 
32 
34  vrpn_Connection *c,
35  const char *port,
36  long baud,
37  int utmFlag,
38  int testFileFlag,
39  const char* startSentence) :
40 vrpn_Tracker_Serial(name,c,port,baud)
41 {
42  //this sets the name of the testfile
43  sprintf(testfilename, "GPS-data.txt");
44 
45  // This allow people to set an external flag whether they want to use real GPS or not
46  //Assuming this is 0 by default
47  if (testFileFlag == 1) {
48  //MessageBox(NULL, "CFG file flag set to use sample GPS data","FYI",0);
49  testfile = fopen(testfilename,"r"); //comment this line out to get real data
50  } else {
51  //MessageBox(NULL, temp,"Live GPS data",0);
52  testfile = NULL;
53  }
54  useUTM = utmFlag;
55  if (strlen(startSentence) > 0) {
56  nmeaParser.setStartSentence((char*)startSentence);
57  }
58 
59  // Set the hardware flow-control on the serial line in case
60  // the GPS unit requires it (some do).
61  if (serial_fd >= 0) {
63  VRPN_MSG_WARNING("Set RTS.\n");
64  }
65 
67 
68 }
69 
70 //------------------------------------
72 {
73  //Cleanup
74  if (testfile != NULL) fclose(testfile);
75 
76  if (serial_fd >=0) {
78  serial_fd = -1;
79  }
80 }
81 
82 //---------------------------------
84 {
85 
86  if (serial_fd >= 0) {
88  VRPN_MSG_WARNING("Set RTS during reset.\n");
89  }
90  //printf("serial fd: %d\n",serial_fd);
91  if (serial_fd >= 0)
92  {
93  VRPN_MSG_WARNING("Reset Completed (this is good).\n");
94  //we need to give the port a chance to read new bits
95 #ifdef _WIN32
96  Sleep(100);
97 #else
98  usleep(100000);
99 #endif
100  status = vrpn_TRACKER_SYNCING; // We're trying for a new reading
101 
102  if (testfile != NULL) fseek(testfile,0L,SEEK_SET);
103  } else {
104  printf("Serial not detected on reset. Try power cycle.\n"); //doesn't really ever seem to fail here unless com port gets jammed
105 #ifdef _WIN32
106  Sleep(1000);
107 #else
108  sleep(1);
109 #endif //prevent the console from spamming us
111  }
112 
113  nmeaParser.reset();
114 }
115 
116 //-----------------------------------------------------------------
117 // This function will read characters until it has a full report, then
118 // put that report into the time, sensor, pos and quat fields so that it can
119 // be sent the next time through the loop.
121 {
122  //printf("getting report\n");
123  char errmsg[512]; // Error message to send to VRPN
124  //int ret; // Return value from function call to be checked
125  int ret;
126 
127  int i = 0;
128  // unsigned char *bufptr; // Points into buffer at the current value to read
129  int done=0;
130 
131  //char speed[256];
132  //char course[256];
133  // int buflen = 0;
134  unsigned int numparameter=0;
135  unsigned int index=0;
136  int charRead = 0;
137  // char temp [256];
138 
139  //--------------------------------------------------------------------
140  // Each report starts with an ASCII '$' character. If we're synching,
141  // read a byte at a time until we find a '$' character.
142  //--------------------------------------------------------------------
143 
144  //printf("STATUS =%d\n", status);
146  {
147 
148  // If testfile is live; read data from that
149  if (testfile != NULL) {
150 
151  //Read one char to see if test file is active
152  if(fread(buffer,sizeof(char),1,testfile) != 1)
153  {
154  return 0;
155  }
156 
157  // Else read from the serial port
158  } else {
159 
161 
162 
163  if( charRead == -1)
164  {
165  printf("fail to read first bit\n");
166  return 0;
167  } else if (charRead == 0) {
168  //case of successful read but no new bits, take a quick nap, wait for new info.
169 
170 #ifdef _WIN32
171  Sleep(10);
172 #else
173  usleep(10000);
174 #endif
175  return 0;
176  }
177  }
178  //if it returns 0 (very likely - happens on interrupt or no new bits) then we have to prevent it from reading the same buffer over and over.
179 
180  // If it's not = to $, keep going until the beginning of a packet starts
181  if( buffer[0] != '$')
182  {
183  sprintf(errmsg,"While syncing (looking for '$', got '%c')", buffer[0]);
184 
185 
186  VRPN_MSG_INFO(errmsg);
188 
189  return 0;
190  }
191 
192  //printf("We have a $, setting to partial and moving on.\n");
193  bufcount = 1; // external GPS parser lib expects "$" at start
194 
196 
198  }//if syncing
199 
200  while (!done)
201  {
202  if (testfile != NULL) {
203  ret = static_cast<int>(fread(&buffer[bufcount],sizeof(char),1,testfile));
204  } else {
206  }
207 
208  if (ret == -1) {
209  VRPN_MSG_ERROR("Error reading report");
211  return 0;
212  } else if (ret == 0) {
213  return 0;
214  }
215 
216  bufcount += ret;
218  {
220  return 0;
221  }
222  if(buffer[bufcount-1] == '\n')
223  {
224  buffer[bufcount-1] = '\0';
225  done = 1;
226  }
227  }
228  if (nmeaParser.parseSentence((char*)buffer) == SENTENCE_VALID)
229  {
230  nmeaData = nmeaParser.getData();
231  //printf("Raw lastfixQuality: %d anyValid: %d LAT: %f LONG: %f ALT: %f\n", nmeaData.lastFixQuality, nmeaData.hasCoordEverBeenValid, nmeaData.lat, nmeaData.lon, nmeaData.altitude);
232  //required RMC doesn't do altitude, so this will be needlessly false half of the time.
233  //if (nmeaData.isValidLat && nmeaData.isValidLon && nmeaData.isValidAltitude)
234  if (nmeaData.isValidLat && nmeaData.isValidLon)
235  {
236  if(nmeaData.isValidAltitude){
237 
238  if (useUTM)
239  {
240  utmCoord.setLatLonCoord (nmeaData.lat, nmeaData.lon);
241  if (!utmCoord.isOutsideUTMGrid ())
242  {
243  double x, y;
244  utmCoord.getXYCoord (x,y);
245  // Christopher 07/25/04: We flip to be x = East <-> West and y = North <-> South
246  //Should this be changed to match rtk precision vrpn_float64?
247  pos[0] = (float)(y);
248  pos[1] = (float)(x);
249  pos[2] = (float)(nmeaData.altitude);
250  }
251  } else {
252 
253  pos[0] = (vrpn_float64)(nmeaData.lat);
254  pos[1] = (vrpn_float64)(nmeaData.lon);
255  pos[2] = (vrpn_float64)(nmeaData.altitude);
256  }//use utm d
257  /*
258  vel[0] = vel_data[0];
259  vel[1] = vel_data[1];
260  vel[2] = vel_data[2];
261  */
262 //#ifdef VERBOSE
263  printf("GPS pos: %f, %f, %f\n",pos[0],pos[1],pos[2]);
264 //#endif
265  nmeaParser.reset();
266 
267  //send report -eb
268  //-----------------------------
269  //printf("tracker report ready\n",status);
270 
271  //vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
272 
273  /*
274  // Send the message on the connection
275  if (NULL != vrpn_Tracker::d_connection)
276  {
277  char msgbuf[1000];
278 
279 
280  fprintf(stderr, "position id = %d, sender id = %d", position_m_id, d_sender_id);
281  //MessageBox(NULL, temp,"GPS Testing",0);
282 
283  // Pack position report
284  int len = encode_to(msgbuf);
285  if (d_connection->pack_message(len, timestamp,
286  position_m_id, d_sender_id, msgbuf,
287  vrpn_CONNECTION_LOW_LATENCY))
288  {
289 
290  fprintf(stderr,"GPS: cannot write message: tossing\n");
291  }
292  else
293  {
294  fprintf(stderr,"packed a message\n\n");
295  }
296 
297 
298  }
299  else
300  {
301  fprintf(stderr,"Tracker Fastrak: No valid connection\n");
302  }
303 
304  //-----------------------------*/
305 
306  //printf("%s\n", buffer);
307  //printf("before first sync status is %d\n",status);
309  //printf("after first set sync status is %d\n",status);
310 
311  return 1;
312  }
313  //no valid alt probably RMC, safe to ignore if we care about alt
314  } else {//valid lat lon alt fail
315  printf("GPS cannot determine position (empty fields). Wait for satellites or reposition GPS.\n");
316  //printf("GPS cannot determine position (empty fields).");
318  return 0;
319  }
320  } else {//valid sentence fail
321  //status = vrpn_TRACKER_FAIL;
323  //is this a good place to put it? If it's not a valid sentence? maybe it should be in reset.
324  printf("Sentence Invalid. Resetting tracker and resyncing.\n");
325  return 0;
326  }
327  // failed valid sentence
329  return 0;
330 
331 //#ifdef VERBOSE2
332  // print_latest_report();
333 //#endif
334  }
335 
336 
337  // This function should be called each time through the main loop
338  // of the server code. It polls for a report from the tracker and
339  // sends it if there is one. It will reset the tracker if there is
340  // no data from it for a few seconds.
341 #if 0
343  {
344  //char temp[256];
345  fprintf(stderr,"calling server main\n");
346  // Call the generic server mainloop, since we are a server
347  server_mainloop();
348 
349  //-eb adding get report and removing switch statement
350  //get_report();
351 
352  fprintf(stderr,"status in mainloop is %d\n\n",status);
353 
354  switch (status) {
356  {
357  printf("tracker report ready\n",status);
358 
359  vrpn_gettimeofday(&timestamp, NULL); // Set watchdog now
360 
361  // Send the message on the connection
362  if (d_connection) {
363  char msgbuf[1000];
364 
365 
366  //sprintf(temp, "position id = %d, sender id = %d", position_m_id, d_sender_id);
367  //MessageBox(NULL, temp,"GPS Testing",0);
368 
369  // Pack position report
370  int len = encode_to(msgbuf);
372  position_m_id, d_sender_id, msgbuf,
374 
375  fprintf(stderr,"Fastrak: cannot write message: tossing\n");
376  }
377 
378  // Pack velocity report
379 
380  // len = encode_vel_to(msgbuf);
381  // if (d_connection->pack_message(len, timestamp,
382  // velocity_m_id, d_sender_id, msgbuf,
383  // vrpn_CONNECTION_LOW_LATENCY)){
384  // fprintf(stderr,"Fastrak: cannot write message: tossing\n");
385  // }
386 
387 
388  } else {
389  fprintf(stderr,"Tracker Fastrak: No valid connection\n");
390  }
391 
392  // Ready for another report
394 
395  }
396  break;
397 
401  {
402  // It turns out to be important to get the report before checking
403  // to see if it has been too long since the last report. This is
404  // because there is the possibility that some other device running
405  // in the same server may have taken a long time on its last pass
406  // through mainloop(). Trackers that are resetting do this. When
407  // this happens, you can get an infinite loop -- where one tracker
408  // resets and causes the other to timeout, and then it returns the
409  // favor. By checking for the report here, we reset the timestamp
410  // if there is a report ready (ie, if THIS device is still operating).
411 
412 
413  get_report();
414 
415  }
416  break;
417 
419  reset();
420  break;
421 
422  case vrpn_TRACKER_FAIL:
423  VRPN_MSG_WARNING("Tracking failed, trying to reset (try power cycle if more than 4 attempts made)");
424  //vrpn_close_commport(serial_fd);
425  //serial_fd = vrpn_open_commport(portname, baudrate);
427  break;
428  }//switch
429 
430  }
431 #endif
const vrpn_uint32 vrpn_CONNECTION_LOW_LATENCY
virtual void mainloop()
Uses the get_report, send_report, and reset routines to implement a server.
Definition: vrpn_Tracker.C:907
void server_mainloop(void)
Handles functions that all servers should provide in their mainloop() (ping/pong, for example) Should...
vrpn_Tracker_GPS(const char *name, vrpn_Connection *c, const char *port="/dev/ttyS1", long baud=4800, int utmFlag=1, int testFileFlag=0, const char *startStr="RMC")
int vrpn_close_commport(int comm)
Definition: vrpn_Serial.C:345
const int vrpn_TRACKER_REPORT_READY
Definition: vrpn_Tracker.h:37
int vrpn_flush_input_buffer(int comm)
Throw out any characters within the input buffer.
Definition: vrpn_Serial.C:435
int register_server_handlers(void)
Definition: vrpn_Tracker.C:318
Header containing macros formerly duplicated in a lot of implementation files.
vrpn_Serial: Pulls all the serial port routines into one file to make porting to new operating system...
vrpn_float64 pos[3]
Definition: vrpn_Tracker.h:95
NMEAParser nmeaParser
virtual void reset()
Reset the tracker.
Generic connection class not specific to the transport mechanism.
#define VRPN_MSG_WARNING(msg)
const int vrpn_TRACKER_FAIL
Definition: vrpn_Tracker.h:40
int vrpn_set_rts(int comm)
Definition: vrpn_Serial.C:365
All types of client/server/peer objects in VRPN should be derived from the vrpn_BaseClass type descri...
const int vrpn_TRACKER_RESETTING
Definition: vrpn_Tracker.h:39
int vrpn_read_available_characters(int comm, unsigned char *buffer, size_t bytes)
Definition: vrpn_Serial.C:512
const int vrpn_TRACKER_PARTIAL
Definition: vrpn_Tracker.h:38
vrpn_Connection * d_connection
Connection that this object talks to.
virtual int pack_message(vrpn_uint32 len, struct timeval time, vrpn_int32 type, vrpn_int32 sender, const char *buffer, vrpn_uint32 class_of_service)
Pack a message that will be sent the next time mainloop() is called. Turn off the RELIABLE flag if yo...
const int vrpn_TRACKER_SYNCING
Definition: vrpn_Tracker.h:35
vrpn_uint32 bufcount
Definition: vrpn_Tracker.h:157
#define VRPN_MSG_INFO(msg)
virtual int encode_to(char *buf)
Definition: vrpn_Tracker.C:533
unsigned char buffer[VRPN_TRACKER_BUF_SIZE *10]
This function should be called each time through the main loop of the server code....
const int vrpn_TRACKER_AWAITING_STATION
Definition: vrpn_Tracker.h:36
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
virtual int get_report(void)
Gets a report if one is available, returns 0 if not, 1 if complete report.
char testfilename[256]
vrpn_int32 d_sender_id
Sender ID registered with the connection.
struct timeval timestamp
Definition: vrpn_Tracker.h:100
#define VRPN_TRACKER_BUF_SIZE
Definition: vrpn_Tracker.h:142
#define VRPN_MSG_ERROR(msg)
vrpn_int32 position_m_id
Definition: vrpn_Tracker.h:80