GPS Library  0.1
GPS driver and NMEA parser
 All Classes Namespaces Files Functions Variables Typedefs Enumerator Friends Macros
GPSInfo.cpp
Go to the documentation of this file.
1 #include <GPS/Driver/I_GPSDriver.h>
2 #include <GPS/Data/GPSData.h>
3 #include <GPS/Interpreter/NMEA/NMEAMessages.h>
4 #include <math.h>
5 
6 using namespace GPS;
7 
8 Time & Time::operator-=(const Time & other)
9 {
10  int carry;
11  second -= other.GetSecond();
12  if (second < 0.0) {
13  carry = -1;
14  second += 60.0;
15  } else if (second >= 60.0) {
16  carry = 1;
17  second -= 60.0;
18  } else {
19  carry = 0;
20  }
21  minute -= other.GetMinute() - carry;
22  if (minute < 0) {
23  carry = -1;
24  minute += 60;
25  } else if (minute >= 60) {
26  carry = 1;
27  minute -= 60;
28  } else {
29  carry = 0;
30  }
31  hour -= other.GetHour() - carry;
32  if (hour < 0) {
33  hour += 24;
34  carry = -1;
35  } else if (hour >= 24) {
36  hour -= 24;
37  carry = 1;
38  } else {
39  carry = 0;
40  }
41  day -= carry;
42  return *this;
43 }
44 
45 Time & Time::operator+=(const Time & other)
46 {
47  int carry;
48  second += other.GetSecond();
49  if (second < 0.0) {
50  carry = -1;
51  second += 60.0;
52  } else if (second >= 60.0) {
53  carry = 1;
54  second -= 60.0;
55  } else {
56  carry = 0;
57  }
58  minute += other.GetMinute() + carry;
59  if (minute < 0) {
60  carry = -1;
61  minute += 60;
62  } else if (minute >= 60) {
63  carry = 1;
64  minute -= 60;
65  } else {
66  carry = 0;
67  }
68  hour += other.GetHour() + carry;
69  if (hour < 0) {
70  hour += 24;
71  carry = -1;
72  } else if (hour >= 24) {
73  hour -= 24;
74  carry = 1;
75  } else {
76  carry = 0;
77  }
78  day += carry;
79  return *this;
80 }
81 
82 int Time::ToMillisecond(void) const
83 {
84  int result = GetSecond() * 1000.0 + 0.5;
85  result += (((GetDay() * 24) + GetHour() * 60) + GetMinute()) * 60000;
86  return result;
87 }
88 
89 std::ostream & operator<<(std::ostream & os, const GPS::Time & time)
90 {
91  return os << time.GetHour() << ":" << time.GetMinute() << ":" << time.GetSecond();
92 }
93 
94 std::ostream & operator<<(std::ostream & os, const GPS::Date & date)
95 {
96  return os << date.GetYear() << "/" << date.GetMonth() << "/" << date.GetDay();
97 }
98 
100  previous_time(0, 0, 0, -1),
101  filters(0),
102  analyzer(filters),
103  parentData(parent),
104  gpgga_timer(1)
105 {
106  GetAnalyzer().Next(FilterOrder);
107  filters.Reset();
108 }
109 
110 void GPSInfo::GotNewPosition(const Time & gps_time, I_GPSDriver * driver)
111 {
112  SYS_DEBUG_MEMBER(DM_GPS);
113 
114  int time_to_wait = parentData.GotNewGPSTime(gps_time);
115  if (driver) {
116  driver->Wait(time_to_wait);
117  }
118 
119  if (previous_time.GetDay() == 0) {
120  int elapsed = (gps_time - previous_time).ToMillisecond();
121  SYS_DEBUG(DL_INFO1, "Elapsed time: " << elapsed << " ms, pos: " << position);
122  if (elapsed > 0) {
123  Filter::filter_time_t delta_time = elapsed / 1000.0; // in seconds
124  GetAnalyzer().In(position, delta_time);
125 
126  MyFilter * an = &GetAnalyzer();
127  int i = 0;
128  for (;;) {
129  Pos3Draw coefficient(0);
130  an->Set(coefficient);
131  SYS_DEBUG(DL_INFO1, "coeff #" << i << "=" << coefficient);
132  if (++i > FilterOrder)
133  break;
134  an = &an->Next();
135  }
136  }
137  }
138 
139  previous_time = gps_time;
140 }
141 
142 namespace GPS
143 {
145  template <>
147  {
148  SYS_DEBUG_MEMBER(DM_GPS);
149 
150  SYS_DEBUG(DL_INFO1, "GPGGA: " << msg.time << ", " << msg.latitude << ", " << msg.longitude);
151 
152  gpgga_timer = 5;
153 
154  Lock _l(this);
155 
156  GotNewPosition(msg.time, driver);
157 
158  position.GetLatitude() = msg.latitude;
159  position.GetLongitude() = msg.longitude;
160  position.GetAltitude() = msg.altitude;
161  }
162 
164  template <>
166  {
167  SYS_DEBUG_MEMBER(DM_GPS);
168  }
169 
171  template <>
173  {
174  SYS_DEBUG_MEMBER(DM_GPS);
175 
176  SYS_DEBUG(DL_INFO1, "GPRMC at " << msg.date << "-" << msg.time << ", " << msg.latitude << ", " << msg.longitude);
177 
178  Lock _l(this);
179 
180  // For positions, prefer GPGGA message, because that contains altitude too:
181  if (gpgga_timer > 0) {
182  // Do not use GPRMC yet:
183  --gpgga_timer;
184  } else {
185  // We have no GPGGA, use GPRMC instead:
186  GotNewPosition(msg.time, driver);
187  position.GetLatitude() = msg.latitude;
188  position.GetLongitude() = msg.longitude;
189  }
190 
191  date = msg.date;
192  time = msg.time;
193  }
194 
196  template <>
198  {
199  SYS_DEBUG_MEMBER(DM_GPS);
200  }
201 
202 } // namespace GPS
203 
204 /* * * * * * * * * * * * * End - of - File * * * * * * * * * * * * * * */