utils/GPSsmooth/gps.cxx

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