7 #include <simgear/constants.h>
8 #include <simgear/misc/sgstream.hxx>
9 #include <simgear/misc/strutils.hxx>
17 GPSTrack::GPSTrack() {};
18 GPSTrack::~GPSTrack() {};
21 // load the specified file, return the number of records loaded
22 int GPSTrack::load( const string &file ) {
28 sg_gzifstream in( file );
29 if ( !in.is_open() ) {
30 cout << "Cannot open file: " << file << endl;
34 vector <string> tokens;
37 while ( ! in.eof() ) {
40 in.getline(tmp, 2048);
43 tokens = simgear::strutils::split(tmp, ",");
48 if ( tokens[0] == "$GPRMC" && tokens.size() == 13 ) {
49 double raw_time = atof(tokens[1].c_str());
50 GPSTime gps_time = GPSTime( raw_time );
51 if ( (gps_time.get_time() > p.gps_time.get_time()) &&
52 (p.gps_time.get_time() > 1.0) )
54 // new data cycle store last data before continuing
59 p.gps_time = gps_time;
61 raw = atof( tokens[3].c_str() );
62 dd = (int)(raw / 100.00);
63 min = raw - dd * 100.0;
64 p.lat_deg = dd + min / 60.0;
65 if ( tokens[4] == "S" ) {
66 p.lat_deg = -p.lat_deg;
68 raw = atof( tokens[5].c_str() );
69 dd = (int)(raw / 100.00);
70 min = raw - dd * 100.0;
71 p.lon_deg = dd + min / 60.0;
72 if ( tokens[6] == "W" ) {
73 p.lon_deg = -p.lon_deg;
76 static double max_speed = 0.0;
77 p.speed_kts = atof( tokens[7].c_str() );
78 if ( p.speed_kts > max_speed ) {
79 max_speed = p.speed_kts;
80 cout << "max speed = " << max_speed << endl;
82 p.course_true = atof( tokens[8].c_str() ) * SGD_DEGREES_TO_RADIANS;
84 } else if ( tokens[0] == "$GPGGA" && tokens.size() == 15 ) {
85 double raw_time = atof(tokens[1].c_str());
86 GPSTime gps_time = GPSTime( raw_time );
87 if ( fabs(gps_time.get_time() - p.gps_time.get_time()) < 0.0001 &&
88 (p.gps_time.get_time() > 1.0) ) {
89 // new data cycle store last data before continuing
94 p.gps_time = gps_time;
96 raw = atof( tokens[2].c_str() );
97 dd = (int)(raw / 100.00);
98 min = raw - dd * 100.0;
99 p.lat_deg = dd + min / 60.0;
100 if ( tokens[3] == "S" ) {
101 p.lat_deg = -p.lat_deg;
103 raw = atof( tokens[4].c_str() );
104 dd = (int)(raw / 100.00);
105 min = raw - dd * 100.0;
106 p.lon_deg = dd + min / 60.0;
107 if ( tokens[5] == "W" ) {
108 p.lon_deg = -p.lon_deg;
111 p.fix_quality = atoi( tokens[6].c_str() );
112 p.num_satellites = atoi( tokens[7].c_str() );
113 p.hdop = atof( tokens[8].c_str() );
115 static double max_alt = 0.0;
116 double alt = atof( tokens[9].c_str() );
117 if ( alt > max_alt ) {
119 cout << "max alt = " << max_alt << endl;
122 if ( tokens[10] == "F" || tokens[10] == "f" ) {
123 alt *= SG_FEET_TO_METER;
125 p.altitude_msl = alt;
133 static double interp( double a, double b, double p, bool rotational = false ) {
136 // special handling of rotational data
137 if ( diff > SGD_PI ) {
139 } else if ( diff < -SGD_PI ) {
147 GPSPoint GPSInterpolate( const GPSPoint A, const GPSPoint B,
148 const double percent ) {
150 p.gps_time = GPSTime((int)interp(A.gps_time.get_time(),
151 B.gps_time.get_time(),
153 p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
154 p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
155 p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
156 p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
157 p.hdop = interp(A.hdop, B.hdop, percent);
158 p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
159 p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
160 p.course_true = interp(A.course_true, B.course_true, percent, true);