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