5 #include <simgear/compiler.h>
9 #include <simgear/constants.h>
10 #include <simgear/misc/sgstream.hxx>
11 #include <simgear/misc/strutils.hxx>
19 GPSTrack::GPSTrack() {};
20 GPSTrack::~GPSTrack() {};
23 // load the specified file, return the number of records loaded
24 int GPSTrack::load( const string &file ) {
30 sg_gzifstream in( file );
31 if ( !in.is_open() ) {
32 cout << "Cannot open file: " << file << endl;
36 vector <string> tokens;
39 while ( ! in.eof() ) {
42 in.getline(tmp, 2048);
45 tokens = simgear::strutils::split(tmp, ",");
50 if ( tokens[0] == "$GPRMC" && tokens.size() == 13 ) {
51 double raw_time = atof(tokens[1].c_str());
52 GPSTime gps_time = GPSTime( raw_time );
53 if ( (gps_time.get_time() > p.gps_time.get_time()) &&
54 (p.gps_time.get_time() > 1.0) )
56 // new data cycle store last data before continuing
61 p.gps_time = gps_time;
63 raw = atof( tokens[3].c_str() );
64 dd = (int)(raw / 100.00);
65 min = raw - dd * 100.0;
66 p.lat_deg = dd + min / 60.0;
67 if ( tokens[4] == "S" ) {
68 p.lat_deg = -p.lat_deg;
70 raw = atof( tokens[5].c_str() );
71 dd = (int)(raw / 100.00);
72 min = raw - dd * 100.0;
73 p.lon_deg = dd + min / 60.0;
74 if ( tokens[6] == "W" ) {
75 p.lon_deg = -p.lon_deg;
78 static double max_speed = 0.0;
79 p.speed_kts = atof( tokens[7].c_str() );
80 if ( p.speed_kts > max_speed ) {
81 max_speed = p.speed_kts;
82 cout << "max speed = " << max_speed << endl;
84 p.course_true = atof( tokens[8].c_str() ) * SGD_DEGREES_TO_RADIANS;
86 } else if ( tokens[0] == "$GPGGA" && tokens.size() == 15 ) {
87 double raw_time = atof(tokens[1].c_str());
88 GPSTime gps_time = GPSTime( raw_time );
89 if ( fabs(gps_time.get_time() - p.gps_time.get_time()) > 0.0001 &&
90 (p.gps_time.get_time() > 1.0) ) {
91 // new data cycle store last data before continuing
96 p.gps_time = gps_time;
98 raw = atof( tokens[2].c_str() );
99 dd = (int)(raw / 100.00);
100 min = raw - dd * 100.0;
101 p.lat_deg = dd + min / 60.0;
102 if ( tokens[3] == "S" ) {
103 p.lat_deg = -p.lat_deg;
105 raw = atof( tokens[4].c_str() );
106 dd = (int)(raw / 100.00);
107 min = raw - dd * 100.0;
108 p.lon_deg = dd + min / 60.0;
109 if ( tokens[5] == "W" ) {
110 p.lon_deg = -p.lon_deg;
113 p.fix_quality = atoi( tokens[6].c_str() );
114 p.num_satellites = atoi( tokens[7].c_str() );
115 p.hdop = atof( tokens[8].c_str() );
117 static double max_alt = 0.0;
118 double alt = atof( tokens[9].c_str() );
119 if ( alt > max_alt ) {
121 cout << "max alt = " << max_alt << endl;
124 if ( tokens[10] == "F" || tokens[10] == "f" ) {
125 alt *= SG_FEET_TO_METER;
127 p.altitude_msl = alt;
135 static double interp( double a, double b, double p, bool rotational = false ) {
138 // special handling of rotational data
139 if ( diff > SGD_PI ) {
141 } else if ( diff < -SGD_PI ) {
149 GPSPoint GPSInterpolate( const GPSPoint A, const GPSPoint B,
150 const double percent ) {
152 p.gps_time = GPSTime((int)interp(A.gps_time.get_time(),
153 B.gps_time.get_time(),
155 p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
156 p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
157 p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
158 p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
159 p.hdop = interp(A.hdop, B.hdop, percent);
160 p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
161 p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
162 p.course_true = interp(A.course_true, B.course_true, percent, true);