1 // gps.cxx - distance-measuring equipment.
2 // Written by David Megginson, started 2003.
4 // This file is in the Public Domain and comes with no warranty.
10 #include <simgear/compiler.h>
12 #include <Aircraft/aircraft.hxx>
13 #include <simgear/route/route.hxx>
15 #include <Airports/simple.hxx>
16 #include <Main/fg_init.hxx>
17 #include <Main/globals.hxx>
18 #include <Main/fg_props.hxx>
19 #include <Main/util.hxx>
20 #include <Navaids/fixlist.hxx>
21 #include <Navaids/navlist.hxx>
30 _last_longitude_deg(0),
31 _last_latitude_deg(0),
44 _longitude_node = fgGetNode("/position/longitude-deg", true);
45 _latitude_node = fgGetNode("/position/latitude-deg", true);
46 _altitude_node = fgGetNode("/position/altitude-ft", true);
47 _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
48 _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
49 _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
51 fgGetNode("/instrumentation/gps/wp-longitude-deg", true);
53 fgGetNode("/instrumentation/gps/wp-latitude-deg", true);
55 fgGetNode("/instrumentation/gps/wp-ID", true);
57 fgGetNode("/instrumentation/gps/wp-name", true);
59 fgGetNode("/instrumentation/gps/desired-course-deg", true);
60 _get_nearest_airport_node =
61 fgGetNode("/instrumentation/gps/get-nearest-airport", true);
63 fgGetNode("/instrumentation/gps/waypoint-type", true);
65 fgGetNode("/instrumentation/gps/tracking-bug", true);
67 _raim_node = fgGetNode("/instrumentation/gps/raim", true);
68 _indicated_longitude_node =
69 fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
70 _indicated_latitude_node =
71 fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
72 _indicated_altitude_node =
73 fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
75 fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
76 _magnetic_track_node =
77 fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
79 fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
81 fgGetNode("/instrumentation/gps/wp-distance-nm", true);
83 fgGetNode("/instrumentation/gps/TTW",true);
85 fgGetNode("/instrumentation/gps/wp-bearing-deg", true);
86 _wp_mag_bearing_node =
87 fgGetNode("/instrumentation/gps/wp-bearing-mag-deg", true);
88 _wp_course_deviation_node =
89 fgGetNode("/instrumentation/gps/course-deviation-deg", true);
90 _wp_course_error_nm_node =
91 fgGetNode("/instrumentation/gps/course-error-nm", true);
93 fgGetNode("/instrumentation/gps/to-flag", true);
95 fgGetNode("/instrumentation/gps/odometer", true);
97 fgGetNode("/instrumentation/gps/trip-odometer", true);
98 _true_bug_error_node =
99 fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
100 _magnetic_bug_error_node =
101 fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
106 GPS::update (double delta_time_sec)
108 // If it's off, don't bother.
109 if (!_serviceable_node->getBoolValue() ||
110 !_electrical_node->getBoolValue()) {
112 _last_longitude_deg = 0;
113 _last_latitude_deg = 0;
114 _last_altitude_m = 0;
116 _raim_node->setDoubleValue(false);
117 _indicated_longitude_node->setDoubleValue(0);
118 _indicated_latitude_node->setDoubleValue(0);
119 _indicated_altitude_node->setDoubleValue(0);
120 _true_track_node->setDoubleValue(0);
121 _magnetic_track_node->setDoubleValue(0);
122 _speed_node->setDoubleValue(0);
123 _wp_distance_node->setDoubleValue(0);
124 _wp_bearing_node->setDoubleValue(0);
125 _wp_longitude_node->setDoubleValue(0);
126 _wp_latitude_node->setDoubleValue(0);
127 _wp_course_node->setDoubleValue(0);
128 _odometer_node->setDoubleValue(0);
129 _trip_odometer_node->setDoubleValue(0);
130 _tracking_bug_node->setDoubleValue(0);
131 _true_bug_error_node->setDoubleValue(0);
132 _magnetic_bug_error_node->setDoubleValue(0);
136 // Get the aircraft position
137 // TODO: Add noise and other errors.
138 double longitude_deg = _longitude_node->getDoubleValue();
139 double latitude_deg = _latitude_node->getDoubleValue();
140 double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
141 double magvar_deg = _magvar_node->getDoubleValue();
145 _raim_node->setBoolValue(true);
146 _indicated_longitude_node->setDoubleValue(longitude_deg);
147 _indicated_latitude_node->setDoubleValue(latitude_deg);
148 _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
151 double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
152 geo_inverse_wgs_84(altitude_m,
153 _last_latitude_deg, _last_longitude_deg,
154 latitude_deg, longitude_deg,
155 &track1_deg, &track2_deg, &distance_m);
156 speed_kt = ((distance_m * SG_METER_TO_NM) *
157 ((1 / delta_time_sec) * 3600.0));
158 _true_track_node->setDoubleValue(track1_deg);
159 mag_track_bearing = deg360(track1_deg - magvar_deg);
160 _magnetic_track_node->setDoubleValue(mag_track_bearing);
161 speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
162 _last_speed_kts = speed_kt;
163 _speed_node->setDoubleValue(speed_kt);
165 odometer = _odometer_node->getDoubleValue();
166 _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
167 odometer = _trip_odometer_node->getDoubleValue();
168 _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
170 // Get waypoint position
171 double wp_longitude_deg = _wp_longitude_node->getDoubleValue();
172 double wp_latitude_deg = _wp_latitude_node->getDoubleValue();
173 double wp_course_deg =
174 _wp_course_node->getDoubleValue();
175 double wp_distance, wp_bearing_deg, wp_course_deviation_deg,
176 wp_course_error_m, wp_TTW;
177 string wp_ID = _wp_ID_node->getStringValue();
179 // If the get-nearest-airport-node is true.
180 // Get the nearest airport, and set it as waypoint.
181 if (_get_nearest_airport_node->getBoolValue()) {
183 cout << "Airport found" << endl;
184 a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
185 _wp_ID_node->setStringValue(a.id.c_str());
186 wp_longitude_deg = a.longitude;
187 wp_latitude_deg = a.latitude;
188 _wp_name_node->setStringValue(a.name.c_str());
189 _get_nearest_airport_node->setBoolValue(false);
190 _last_wp_ID = wp_ID = a.id.c_str();
193 // If the waypoint ID has changed, try to find the new ID
194 // in the airport-, fix-, nav-database.
195 if ( !(_last_wp_ID == wp_ID) ) {
196 string waypont_type =
197 _waypoint_type_node->getStringValue();
198 if (waypont_type == "airport") {
200 a = globals->get_airports()->search( wp_ID );
201 if ( a.id == wp_ID ) {
202 cout << "Airport found" << endl;
203 wp_longitude_deg = a.longitude;
204 wp_latitude_deg = a.latitude;
205 _wp_name_node->setStringValue(a.name.c_str());
208 else if (waypont_type == "nav") {
210 if ( (n = current_navlist->findByIdent(wp_ID.c_str(),
212 latitude_deg)) != NULL) {
213 cout << "Nav found" << endl;
214 wp_longitude_deg = n->get_lon();
215 wp_latitude_deg = n->get_lat();
216 _wp_name_node->setStringValue(n->get_name().c_str());
219 else if (waypont_type == "fix") {
221 if ( current_fixlist->query(wp_ID, &f) ) {
222 cout << "Fix found" << endl;
223 wp_longitude_deg = f.get_lon();
224 wp_latitude_deg = f.get_lat();
225 _wp_name_node->setStringValue(wp_ID.c_str());
231 // Find the bearing and distance to the waypoint.
232 SGWayPoint wp(wp_longitude_deg, wp_latitude_deg, altitude_m);
233 wp.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
234 &wp_bearing_deg, &wp_distance);
235 _wp_longitude_node->setDoubleValue(wp_longitude_deg);
236 _wp_latitude_node->setDoubleValue(wp_latitude_deg);
237 _wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM);
238 _wp_bearing_node->setDoubleValue(wp_bearing_deg);
239 _wp_mag_bearing_node->setDoubleValue(deg360(wp_bearing_deg - magvar_deg));
241 // Estimate time to waypoint.
242 // The estimation does not take track into consideration,
243 // so if you are going away from the waypoint the TTW will
244 // increase. Makes most sense when travelling directly towards
246 if (speed_kt > 0.0 && wp_distance > 0.0) {
247 wp_TTW = (wp_distance * SG_METER_TO_NM) / (speed_kt / 3600);
252 unsigned int wp_TTW_seconds = (int) (wp_TTW + 0.5);
253 if (wp_TTW_seconds < 356400) { // That's 99 hours
254 unsigned int wp_TTW_minutes = 0;
255 unsigned int wp_TTW_hours = 0;
257 while (wp_TTW_seconds >= 3600) {
258 wp_TTW_seconds -= 3600;
261 while (wp_TTW_seconds >= 60) {
262 wp_TTW_seconds -= 60;
265 sprintf(wp_TTW_str, "%02d:%02d:%02d",
266 wp_TTW_hours, wp_TTW_minutes, wp_TTW_seconds);
267 _wp_ttw_node->setStringValue(wp_TTW_str);
270 _wp_ttw_node->setStringValue("--:--:--");
272 // Course deviation is the diffenrence between the bearing
274 wp_course_deviation_deg = wp_bearing_deg -
276 while (wp_course_deviation_deg < -180.0) {
277 wp_course_deviation_deg += 360.0; }
278 while (wp_course_deviation_deg > 180.0) {
279 wp_course_deviation_deg -= 360.0; }
281 // If the course deviation is less than 90 degrees to either side,
282 // our desired course is towards the waypoint.
283 // It does not matter if we are actually moving towards or from the waypoint.
284 if (fabs(wp_course_deviation_deg) < 90.0) {
285 _wp_to_flag_node->setBoolValue(true); }
286 // If it's more than 90 degrees the desired course is from the waypoint.
287 else if (fabs(wp_course_deviation_deg) > 90.0) {
288 _wp_to_flag_node->setBoolValue(false);
289 // When the course is away from the waypoint,
290 // it makes sense to change the sign of the deviation.
291 wp_course_deviation_deg *= -1.0;
292 while (wp_course_deviation_deg < -90.0)
293 wp_course_deviation_deg += 180.0;
294 while (wp_course_deviation_deg > 90.0)
295 wp_course_deviation_deg -= 180.0; }
297 _wp_course_deviation_node->setDoubleValue(wp_course_deviation_deg);
299 // Cross track error.
300 wp_course_error_m = sin(wp_course_deviation_deg * SG_PI / 180.0)
302 _wp_course_error_nm_node->setDoubleValue(wp_course_error_m
306 double tracking_bug = _tracking_bug_node->getDoubleValue();
307 double true_bug_error = tracking_bug - track1_deg;
308 double magnetic_bug_error = tracking_bug - mag_track_bearing;
310 // Get the errors into the (-180,180) range.
311 while (true_bug_error < -180.0)
312 true_bug_error += 360.0;
313 while (true_bug_error > 180.0)
314 true_bug_error -= 360.0;
316 while (magnetic_bug_error < -180.0)
317 magnetic_bug_error += 360.0;
318 while (magnetic_bug_error > 180.0)
319 magnetic_bug_error -= 360.0;
321 _true_bug_error_node->setDoubleValue(true_bug_error);
322 _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
325 _true_track_node->setDoubleValue(0.0);
326 _magnetic_track_node->setDoubleValue(0.0);
327 _speed_node->setDoubleValue(0.0);
331 _last_longitude_deg = longitude_deg;
332 _last_latitude_deg = latitude_deg;
333 _last_altitude_m = altitude_m;
336 double GPS::deg360 (double deg)
340 while (deg > 360.0) {