utils/GPSsmooth/GPSsmooth.cxx

   1 #ifdef HAVE_CONFIG_H
   2 #  include <config.h>
   3 #endif
   4
   5 #include <simgear/compiler.h>
   6
   7 #include <iostream>
   8
   9 #include <simgear/constants.h>
  10 #include <simgear/misc/sgstream.hxx>
  11 #include <simgear/misc/strutils.hxx>
  12
  13 #include "GPSsmooth.hxx"
  14
  15 using std::cout;
  16 using std::endl;
  17
  18
  19 GPSTrack::GPSTrack() {};
  20 GPSTrack::~GPSTrack() {};
  21
  22
  23 // load the specified file, return the number of records loaded
  24 int GPSTrack::load( const string &file ) {
  25     int count = 0;
  26
  27     data.clear();
  28
  29     // openg the file
  30     sg_gzifstream in( file );
  31     if ( !in.is_open() ) {
  32         cout << "Cannot open file: " << file << endl;
  33         return 0;
  34     }
  35
  36     vector <string> tokens;
  37     GPSPoint p;
  38
  39     while ( ! in.eof() ) {
  40         char tmp[2049];
  41
  42         in.getline(tmp, 2048);
  43
  44         tokens.clear();
  45         tokens = simgear::strutils::split(tmp, ",");
  46
  47         int dd;
  48         double raw, min;
  49
  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) )
  55             {
  56                 // new data cycle store last data before continuing
  57                 data.push_back( p );
  58                 count++;
  59             }
  60
  61             p.gps_time = gps_time;
  62
  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;
  69             }
  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;
  76             }
  77
  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;
  83             }
  84             p.course_true = atof( tokens[8].c_str() ) * SGD_DEGREES_TO_RADIANS;
  85
  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
  92                 data.push_back( p );
  93                 count++;
  94             }
  95
  96             p.gps_time = gps_time;
  97
  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;
 104             }
 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;
 111             }
 112
 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() );
 116
 117             static double max_alt = 0.0;
 118             double alt = atof( tokens[9].c_str() );
 119             if ( alt > max_alt ) {
 120                 max_alt = alt;
 121                 cout << "max alt = " << max_alt << endl;
 122             }
 123
 124             if ( tokens[10] == "F" || tokens[10] == "f" ) {
 125                 alt *= SG_FEET_TO_METER;
 126             }
 127             p.altitude_msl = alt;
 128         }
 129     }
 130
 131     return count;
 132 }
 133
 134
 135 static double interp( double a, double b, double p, bool rotational = false ) {
 136     double diff = b - a;
 137     if ( rotational ) {
 138         // special handling of rotational data
 139         if ( diff > SGD_PI ) {
 140             diff -= SGD_2PI;
 141         } else if ( diff < -SGD_PI ) {
 142             diff += SGD_2PI;
 143         }
 144     }
 145     return a + diff * p;
 146 }
 147
 148
 149 GPSPoint GPSInterpolate( const GPSPoint A, const GPSPoint B,
 150                          const double percent ) {
 151     GPSPoint p;
 152     p.gps_time = GPSTime((int)interp(A.gps_time.get_time(),
 153                                      B.gps_time.get_time(),
 154                                      percent));
 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);
 163
 164     return p;
 165 }