utils/GPSsmooth/gps.cxx

   1 #include <iostream>
   2
   3 #include <simgear/constants.h>
   4 #include <simgear/misc/sgstream.hxx>
   5 #include <simgear/misc/strutils.hxx>
   6
   7 #include "gps.hxx"
   8
   9 SG_USING_STD(cout);
  10 SG_USING_STD(endl);
  11
  12
  13 GPSTrack::GPSTrack() {};
  14 GPSTrack::~GPSTrack() {};
  15
  16
  17 // load the specified file, return the number of records loaded
  18 int GPSTrack::load( const string &file ) {
  19     int count = 0;
  20
  21     data.clear();
  22
  23     // openg the file
  24     sg_gzifstream in( file );
  25     if ( !in.is_open() ) {
  26         cout << "Cannot open file: " << file << endl;
  27         return 0;
  28     }
  29
  30     vector <string> tokens;
  31     GPSPoint p;
  32
  33     while ( ! in.eof() ) {
  34         char tmp[2049];
  35
  36         in.getline(tmp, 2048);
  37
  38         tokens.clear();
  39         tokens = simgear::strutils::split(tmp, ",");
  40
  41         int dd;
  42         double raw, min;
  43
  44         if ( tokens[0] == "$GPRMC" && tokens.size() == 13 ) {
  45             double raw_time = atof(tokens[1].c_str());
  46             GPSTime gps_time = GPSTime( raw_time );
  47             if ( (gps_time.get_time() > p.gps_time.get_time()) &&
  48                  (p.gps_time.get_time() > 1.0) )
  49             {
  50                 // new data cycle store last data before continuing
  51                 data.push_back( p );
  52                 count++;
  53             }
  54
  55             p.gps_time = gps_time;
  56
  57             raw = atof( tokens[3].c_str() );
  58             dd = (int)(raw / 100.00);
  59             min = raw - dd * 100.0;
  60             p.lat_deg = dd + min / 60.0;
  61             if ( tokens[4] == "S" ) {
  62                 p.lat_deg = -p.lat_deg;
  63             }
  64             raw = atof( tokens[5].c_str() );
  65             dd = (int)(raw / 100.00);
  66             min = raw - dd * 100.0;
  67             p.lon_deg = dd + min / 60.0;
  68             if ( tokens[6] == "W" ) {
  69                 p.lon_deg = -p.lon_deg;
  70             }
  71
  72             static double max_speed = 0.0;
  73             p.speed_kts = atof( tokens[7].c_str() );
  74             if ( p.speed_kts > max_speed ) {
  75                 max_speed = p.speed_kts;
  76                 cout << "max speed = " << max_speed << endl;
  77             }
  78             p.course_true = atof( tokens[8].c_str() ) * SGD_DEGREES_TO_RADIANS;
  79
  80         } else if ( tokens[0] == "$GPGGA" && tokens.size() == 15 ) {
  81             double raw_time = atof(tokens[1].c_str());
  82             GPSTime gps_time = GPSTime( raw_time );
  83             if ( fabs(gps_time.get_time() - p.gps_time.get_time()) < 0.0001 &&
  84                  (p.gps_time.get_time() > 1.0) ) {
  85                 // new data cycle store last data before continuing
  86                 data.push_back( p );
  87                 count++;
  88             }
  89
  90             p.gps_time = gps_time;
  91
  92             raw = atof( tokens[2].c_str() );
  93             dd = (int)(raw / 100.00);
  94             min = raw - dd * 100.0;
  95             p.lat_deg = dd + min / 60.0;
  96             if ( tokens[3] == "S" ) {
  97                 p.lat_deg = -p.lat_deg;
  98             }
  99             raw = atof( tokens[4].c_str() );
 100             dd = (int)(raw / 100.00);
 101             min = raw - dd * 100.0;
 102             p.lon_deg = dd + min / 60.0;
 103             if ( tokens[5] == "W" ) {
 104                 p.lon_deg = -p.lon_deg;
 105             }
 106
 107             p.fix_quality = atoi( tokens[6].c_str() );
 108             p.num_satellites = atoi( tokens[7].c_str() );
 109             p.hdop = atof( tokens[8].c_str() );
 110
 111             static double max_alt = 0.0;
 112             double alt = atof( tokens[9].c_str() );
 113             if ( alt > max_alt ) {
 114                 max_alt = alt;
 115                 cout << "max alt = " << max_alt << endl;
 116             }
 117
 118             if ( tokens[10] == "F" || tokens[10] == "f" ) {
 119                 alt *= SG_FEET_TO_METER;
 120             }
 121             p.altitude_msl = alt;
 122         }
 123     }
 124
 125     return count;
 126 }
 127
 128
 129 static double interp( double a, double b, double p, bool rotational = false ) {
 130     double diff = b - a;
 131     if ( rotational ) {
 132         // special handling of rotational data
 133         if ( diff > SGD_PI ) {
 134             diff -= SGD_2PI;
 135         } else if ( diff < -SGD_PI ) {
 136             diff += SGD_2PI;
 137         }
 138     }
 139     return a + diff * p;
 140 }
 141
 142
 143 GPSPoint GPSInterpolate( const GPSPoint A, const GPSPoint B,
 144                          const double percent ) {
 145     GPSPoint p;
 146     p.gps_time = GPSTime((int)interp(A.gps_time.get_time(),
 147                                      B.gps_time.get_time(),
 148                                      percent));
 149     p.lat_deg = interp(A.lat_deg, B.lat_deg, percent);
 150     p.lon_deg = interp(A.lon_deg, B.lon_deg, percent);
 151     p.fix_quality = (int)interp(A.fix_quality, B.fix_quality, percent);
 152     p.num_satellites = (int)interp(A.num_satellites, B.num_satellites, percent);
 153     p.hdop = interp(A.hdop, B.hdop, percent);
 154     p.altitude_msl = interp(A.altitude_msl, B.altitude_msl, percent);
 155     p.speed_kts = interp(A.speed_kts, B.speed_kts, percent);
 156     p.course_true = interp(A.course_true, B.course_true, percent, true);
 157
 158     return p;
 159 }