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.
12 #include <simgear/compiler.h>
13 #include <Aircraft/aircraft.hxx>
14 #include <Main/fg_props.hxx>
15 #include <Main/util.hxx> // for fgLowPass
16 #include <Navaids/positioned.hxx>
18 #include <simgear/math/sg_random.h>
19 #include <simgear/sg_inlines.h>
20 #include <simgear/math/sg_geodesy.hxx>
25 void SGGeodProperty::init(SGPropertyNode* base, const char* lonStr, const char* latStr, const char* altStr)
27 _lon = base->getChild(lonStr, 0, true);
28 _lat = base->getChild(latStr, 0, true);
30 _alt = base->getChild(altStr, 0, true);
34 void SGGeodProperty::init(const char* lonStr, const char* latStr, const char* altStr)
36 _lon = fgGetNode(lonStr, true);
37 _lat = fgGetNode(latStr, true);
39 _alt = fgGetNode(altStr, true);
43 void SGGeodProperty::clear()
45 _lon = _lat = _alt = NULL;
48 void SGGeodProperty::operator=(const SGGeod& geod)
50 _lon->setDoubleValue(geod.getLongitudeDeg());
51 _lat->setDoubleValue(geod.getLatitudeDeg());
53 _alt->setDoubleValue(geod.getElevationFt());
57 SGGeod SGGeodProperty::get() const
59 double lon = _lon->getDoubleValue(),
60 lat = _lat->getDoubleValue();
62 return SGGeod::fromDegFt(lon, lat, _alt->getDoubleValue());
64 return SGGeod::fromDeg(lon,lat);
69 GPS::GPS ( SGPropertyNode *node)
74 _name(node->getStringValue("name", "gps")),
75 _num(node->getIntValue("number", 0))
87 branch = "/instrumentation/" + _name;
89 SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
90 _position.init("/position/longitude-deg", "/position/latitude-deg", "/position/altitude-ft");
91 _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
92 _serviceable_node = node->getChild("serviceable", 0, true);
93 _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
95 SGPropertyNode *wp_node = node->getChild("wp", 0, true);
96 SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
97 SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
99 _wp0_position.init(wp0_node, "longitude-deg", "latitude-deg", "altitude-ft");
100 _wp0_ID_node = wp0_node->getChild("ID", 0, true);
101 _wp0_name_node = wp0_node->getChild("name", 0, true);
102 _wp0_course_node = wp0_node->getChild("desired-course-deg", 0, true);
103 _wp0_distance_node = wp0_node->getChild("distance-nm", 0, true);
104 _wp0_ttw_node = wp0_node->getChild("TTW", 0, true);
105 _wp0_bearing_node = wp0_node->getChild("bearing-true-deg", 0, true);
106 _wp0_mag_bearing_node = wp0_node->getChild("bearing-mag-deg", 0, true);
107 _wp0_course_deviation_node =
108 wp0_node->getChild("course-deviation-deg", 0, true);
109 _wp0_course_error_nm_node = wp0_node->getChild("course-error-nm", 0, true);
110 _wp0_to_flag_node = wp0_node->getChild("to-flag", 0, true);
111 _true_wp0_bearing_error_node =
112 wp0_node->getChild("true-bearing-error-deg", 0, true);
113 _magnetic_wp0_bearing_error_node =
114 wp0_node->getChild("magnetic-bearing-error-deg", 0, true);
116 _wp1_position.init(wp1_node, "longitude-deg", "latitude-deg", "altitude-ft");
117 _wp1_ID_node = wp1_node->getChild("ID", 0, true);
118 _wp1_name_node = wp1_node->getChild("name", 0, true);
119 _wp1_course_node = wp1_node->getChild("desired-course-deg", 0, true);
120 _wp1_distance_node = wp1_node->getChild("distance-nm", 0, true);
121 _wp1_ttw_node = wp1_node->getChild("TTW", 0, true);
122 _wp1_bearing_node = wp1_node->getChild("bearing-true-deg", 0, true);
123 _wp1_mag_bearing_node = wp1_node->getChild("bearing-mag-deg", 0, true);
124 _wp1_course_deviation_node =
125 wp1_node->getChild("course-deviation-deg", 0, true);
126 _wp1_course_error_nm_node = wp1_node->getChild("course-error-nm", 0, true);
127 _wp1_to_flag_node = wp1_node->getChild("to-flag", 0, true);
128 _true_wp1_bearing_error_node =
129 wp1_node->getChild("true-bearing-error-deg", 0, true);
130 _magnetic_wp1_bearing_error_node =
131 wp1_node->getChild("magnetic-bearing-error-deg", 0, true);
132 _get_nearest_airport_node =
133 wp1_node->getChild("get-nearest-airport", 0, true);
135 _tracking_bug_node = node->getChild("tracking-bug", 0, true);
136 _raim_node = node->getChild("raim", 0, true);
138 _indicated_pos.init(node, "indicated-longitude-deg",
139 "indicated-latitude-deg", "indicated-altitude-ft");
141 _indicated_vertical_speed_node =
142 node->getChild("indicated-vertical-speed", 0, true);
144 node->getChild("indicated-track-true-deg", 0, true);
145 _magnetic_track_node =
146 node->getChild("indicated-track-magnetic-deg", 0, true);
148 node->getChild("indicated-ground-speed-kt", 0, true);
150 node->getChild("odometer", 0, true);
151 _trip_odometer_node =
152 node->getChild("trip-odometer", 0, true);
153 _true_bug_error_node =
154 node->getChild("true-bug-error-deg", 0, true);
155 _magnetic_bug_error_node =
156 node->getChild("magnetic-bug-error-deg", 0, true);
159 wp_node->getChild("leg-distance-nm", 0, true);
161 wp_node->getChild("leg-true-course-deg", 0, true);
162 _leg_magnetic_course_node =
163 wp_node->getChild("leg-mag-course-deg", 0, true);
164 _alt_dist_ratio_node =
165 wp_node->getChild("alt-dist-ratio", 0, true);
166 _leg_course_deviation_node =
167 wp_node->getChild("leg-course-deviation-deg", 0, true);
168 _leg_course_error_nm_node =
169 wp_node->getChild("leg-course-error-nm", 0, true);
171 wp_node->getChild("leg-to-flag", 0, true);
172 _alt_deviation_node =
173 wp_node->getChild("alt-deviation-ft", 0, true);
175 _serviceable_node->setBoolValue(true);
183 _last_pos = SGGeod();
184 _raim_node->setDoubleValue(false);
185 _indicated_pos = SGGeod();
186 _indicated_vertical_speed_node->setDoubleValue(0);
187 _true_track_node->setDoubleValue(0);
188 _magnetic_track_node->setDoubleValue(0);
189 _speed_node->setDoubleValue(0);
190 _wp1_distance_node->setDoubleValue(0);
191 _wp1_bearing_node->setDoubleValue(0);
192 _wp1_position = SGGeod();
193 _wp1_course_node->setDoubleValue(0);
194 _odometer_node->setDoubleValue(0);
195 _trip_odometer_node->setDoubleValue(0);
196 _tracking_bug_node->setDoubleValue(0);
197 _true_bug_error_node->setDoubleValue(0);
198 _magnetic_bug_error_node->setDoubleValue(0);
199 _true_wp1_bearing_error_node->setDoubleValue(0);
200 _magnetic_wp1_bearing_error_node->setDoubleValue(0);
204 GPS::update (double delta_time_sec)
206 // If it's off, don't bother.
207 if (!_serviceable_node->getBoolValue() || !_electrical_node->getBoolValue()) {
213 ctx.dt = delta_time_sec;
214 ctx.waypoint_changed = false;
215 ctx.pos = _position.get();
217 // TODO: Add noise and other errors.
220 // Bias and random error
221 double random_factor = sg_random();
222 double random_error = 1.4;
223 double error_radius = 5.1;
224 double bias_max_radius = 5.1;
225 double random_max_radius = 1.4;
227 bias_length += (random_factor-0.5) * 1.0e-3;
228 if (bias_length <= 0.0) bias_length = 0.0;
229 else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
230 bias_angle += (random_factor-0.5) * 1.0e-3;
231 if (bias_angle <= 0.0) bias_angle = 0.0;
232 else if (bias_angle >= 360.0) bias_angle = 360.0;
234 double random_length = random_factor * random_max_radius;
235 double random_angle = random_factor * 360.0;
237 double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
238 double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
239 double random_x = random_length * cos(random_angle * SG_PI / 180.0);
240 double random_y = random_length * sin(random_angle * SG_PI / 180.0);
241 double error_x = bias_x + random_x;
242 double error_y = bias_y + random_y;
243 double error_length = sqrt(error_x*error_x + error_y*error_y);
244 double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
249 geo_direct_wgs_84 ( altitude_m, latitude_deg,
250 longitude_deg, error_angle,
251 error_length, &lat2, &lon2,
253 //cout << lat2 << " " << lon2 << endl;
254 printf("%f %f \n", bias_length, bias_angle);
255 printf("%3.7f %3.7f \n", lat2, lon2);
256 printf("%f %f \n", error_length, error_angle);
259 _raim_node->setBoolValue(true);
260 _indicated_pos = ctx.pos;
263 updateWithValid(ctx);
265 _true_track_node->setDoubleValue(0.0);
266 _magnetic_track_node->setDoubleValue(0.0);
267 _speed_node->setDoubleValue(0.0);
275 GPS::updateNearestAirport(UpdateContext& ctx)
277 if (!_get_nearest_airport_node->getBoolValue()) {
281 // If the get-nearest-airport-node is true.
282 // Get the nearest airport, and set it as waypoint 1.
284 FGPositioned::TypeFilter aptFilter(FGPositioned::AIRPORT);
285 FGPositionedRef a = FGPositioned::findClosest(ctx.pos, 360.0, &aptFilter);
290 _wp1_position = a->geod();
291 _wp1_ID_node->setStringValue(a->ident().c_str());
292 _wp1_name_node->setStringValue(a->name().c_str());
293 _get_nearest_airport_node->setBoolValue(false);
294 _last_wp1_ID = a->ident(); // don't trigger updateWaypoint1();
295 ctx.waypoint_changed = true;
299 GPS::updateWithValid(UpdateContext& ctx)
303 SGGeodesy::inverse(_last_pos, ctx.pos, ctx.track1_deg, ctx.track2_deg, distance_m );
305 ctx.speed_kt = ((distance_m * SG_METER_TO_NM) * ((1 / ctx.dt) * 3600.0));
307 double vertical_speed_mpm = ((ctx.pos.getElevationM() - _last_pos.getElevationM()) * 60 /
309 _indicated_vertical_speed_node->setDoubleValue(vertical_speed_mpm * SG_METER_TO_FEET);
310 _true_track_node->setDoubleValue(ctx.track1_deg);
312 ctx.magvar_deg = _magvar_node->getDoubleValue();
313 double mag_track_bearing = ctx.track1_deg - ctx.magvar_deg;
314 SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
315 _magnetic_track_node->setDoubleValue(mag_track_bearing);
316 ctx.speed_kt = fgGetLowPass(_last_speed_kts, ctx.speed_kt, ctx.dt/20.0);
317 _last_speed_kts = ctx.speed_kt;
318 _speed_node->setDoubleValue(ctx.speed_kt);
320 double odometer = _odometer_node->getDoubleValue();
321 _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
322 odometer = _trip_odometer_node->getDoubleValue();
323 _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
325 updateNearestAirport(ctx);
326 updateWaypoint0(ctx);
327 updateWaypoint1(ctx);
329 ctx.wp0_pos = _wp0_position.get();
330 ctx.wp1_pos = _wp1_position.get();
331 // if this flag is set, we need to recompute leg data, because either
332 // WP0 or WP1 has been updated
333 if (ctx.waypoint_changed) {
334 waypointChanged(ctx);
337 ctx.wp0_course_deg = _wp0_course_node->getDoubleValue();
338 ctx.wp1_course_deg = _wp1_course_node->getDoubleValue();
340 updateWaypoint0Course(ctx);
341 updateWaypoint1Course(ctx);
342 updateLegCourse(ctx);
344 // Altitude deviation
345 //double desired_altitude_m = wp1_altitude_m
346 // + wp1_distance * _alt_dist_ratio;
347 //double altitude_deviation_m = altitude_m - desired_altitude_m;
348 // _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
350 updateTrackingBug(ctx);
354 GPS::updateLegCourse(UpdateContext& ctx)
356 // Leg course deviation is the diffenrence between the bearing
358 double course_deviation_deg = ctx.wp1_bearing_deg - _course_deg;
359 SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
361 // If the course deviation is less than 90 degrees to either side,
362 // our desired course is towards the waypoint.
363 // It does not matter if we are actually moving
364 // towards or from the waypoint.
365 if (fabs(course_deviation_deg) < 90.0) {
366 _leg_to_flag_node->setBoolValue(true);
368 // If it's more than 90 degrees the desired
369 // course is from the waypoint.
370 else if (fabs(course_deviation_deg) > 90.0) {
371 _leg_to_flag_node->setBoolValue(false);
372 // When the course is away from the waypoint,
373 // it makes sense to change the sign of the deviation.
374 course_deviation_deg *= -1.0;
375 SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
378 _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
380 // Cross track error.
381 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
383 _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
388 GPS::updateTrackingBug(UpdateContext& ctx)
390 double tracking_bug = _tracking_bug_node->getDoubleValue();
391 double true_bug_error = tracking_bug - ctx.track1_deg;
392 double magnetic_bug_error = tracking_bug - _magnetic_track_node->getDoubleValue();
394 // Get the errors into the (-180,180) range.
395 SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
396 SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
398 _true_bug_error_node->setDoubleValue(true_bug_error);
399 _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
403 GPS::waypointChanged(UpdateContext& ctx)
405 // If any of the two waypoints have changed
406 // we need to calculate a new course between them,
407 // and values for vertical navigation.
408 assert(ctx.waypoint_changed);
411 SGGeodesy::inverse(ctx.wp0_pos, ctx.wp1_pos, _course_deg, track2, _distance_m);
413 double leg_mag_course = _course_deg - _magvar_node->getDoubleValue();
414 SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
416 // Get the altitude / distance ratio
417 if ( _distance_m > 0.0 ) {
418 double alt_difference_m = ctx.wp0_pos.getElevationM() - ctx.wp1_pos.getElevationM();
419 _alt_dist_ratio = alt_difference_m / _distance_m;
422 _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
423 _leg_course_node->setDoubleValue(_course_deg);
424 _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
425 _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
429 GPS::updateWaypoint0(UpdateContext& ctx)
431 string id(_wp0_ID_node->getStringValue());
432 if (_last_wp0_ID == id) {
433 return; // easy, nothing to do
436 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
443 _wp0_position = result->geod();
444 _wp0_name_node->setStringValue(result->name().c_str());
446 ctx.waypoint_changed = true;
450 GPS::updateWaypoint1(UpdateContext& ctx)
452 string id(_wp1_ID_node->getStringValue());
453 if (_last_wp1_ID == id) {
454 return; // easy, nothing to do
457 FGPositionedRef result = FGPositioned::findClosestWithIdent(id, ctx.pos);
464 _wp1_position = result->geod();
465 _wp1_name_node->setStringValue(result->name().c_str());
467 ctx.waypoint_changed = true;
471 GPS::updateTTWNode(UpdateContext& ctx, double distance_m, SGPropertyNode_ptr node)
473 // Estimate time to waypoint.
474 // The estimation does not take track into consideration,
475 // so if you are going away from the waypoint the TTW will
476 // increase. Makes most sense when travelling directly towards
479 if (ctx.speed_kt > 0.0 && distance_m > 0.0) {
480 TTW = (distance_m * SG_METER_TO_NM) / (ctx.speed_kt / 3600);
482 if (TTW < 356400.5) { // That's 99 hours
483 unsigned int TTW_seconds = (int) (TTW + 0.5);
484 unsigned int TTW_minutes = 0;
485 unsigned int TTW_hours = 0;
487 TTW_hours = TTW_seconds / 3600;
488 TTW_minutes = (TTW_seconds / 60) % 60;
489 TTW_seconds = TTW_seconds % 60;
490 snprintf(TTW_str, 9, "%02d:%02d:%02d",
491 TTW_hours, TTW_minutes, TTW_seconds);
492 node->setStringValue(TTW_str);
494 node->setStringValue("--:--:--");
499 GPS::updateWaypoint0Course(UpdateContext& ctx)
501 // Find the bearing and distance to waypoint 0.
503 SGGeodesy::inverse(ctx.pos, ctx.wp0_pos, ctx.wp0_bearing_deg, az2,ctx.wp0_distance);
504 _wp0_distance_node->setDoubleValue(ctx.wp0_distance * SG_METER_TO_NM);
505 _wp0_bearing_node->setDoubleValue(ctx.wp0_bearing_deg);
507 double mag_bearing_deg = ctx.wp0_bearing_deg - ctx.magvar_deg;
508 SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
509 _wp0_mag_bearing_node->setDoubleValue(mag_bearing_deg);
510 double bearing_error_deg = ctx.track1_deg - ctx.wp0_bearing_deg;
511 SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
512 _true_wp0_bearing_error_node->setDoubleValue(bearing_error_deg);
514 updateTTWNode(ctx, ctx.wp0_distance, _wp0_ttw_node);
516 // Course deviation is the diffenrence between the bearing
518 double course_deviation_deg = ctx.wp0_bearing_deg -
520 SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
522 // If the course deviation is less than 90 degrees to either side,
523 // our desired course is towards the waypoint.
524 // It does not matter if we are actually moving
525 // towards or from the waypoint.
526 if (fabs(course_deviation_deg) < 90.0) {
527 _wp0_to_flag_node->setBoolValue(true);
529 // If it's more than 90 degrees the desired
530 // course is from the waypoint.
531 else if (fabs(course_deviation_deg) > 90.0) {
532 _wp0_to_flag_node->setBoolValue(false);
533 // When the course is away from the waypoint,
534 // it makes sense to change the sign of the deviation.
535 course_deviation_deg *= -1.0;
536 SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
539 _wp0_course_deviation_node->setDoubleValue(course_deviation_deg);
541 // Cross track error.
542 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
543 * (ctx.wp0_distance);
544 _wp0_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
548 GPS::updateWaypoint1Course(UpdateContext& ctx)
550 // Find the bearing and distance to waypoint 0.
552 SGGeodesy::inverse(ctx.pos, ctx.wp1_pos, ctx.wp1_bearing_deg, az2,ctx.wp1_distance);
553 _wp1_distance_node->setDoubleValue(ctx.wp1_distance * SG_METER_TO_NM);
554 _wp1_bearing_node->setDoubleValue(ctx.wp1_bearing_deg);
556 double mag_bearing_deg = ctx.wp1_bearing_deg - ctx.magvar_deg;
557 SG_NORMALIZE_RANGE(mag_bearing_deg, 0.0, 360.0);
558 _wp1_mag_bearing_node->setDoubleValue(mag_bearing_deg);
559 double bearing_error_deg = ctx.track1_deg - ctx.wp1_bearing_deg;
560 SG_NORMALIZE_RANGE(bearing_error_deg, -180.0, 180.0);
561 _true_wp1_bearing_error_node->setDoubleValue(bearing_error_deg);
563 updateTTWNode(ctx, ctx.wp1_distance, _wp1_ttw_node);
565 // Course deviation is the diffenrence between the bearing
567 double course_deviation_deg = ctx.wp1_bearing_deg -
569 SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
571 // If the course deviation is less than 90 degrees to either side,
572 // our desired course is towards the waypoint.
573 // It does not matter if we are actually moving
574 // towards or from the waypoint.
575 if (fabs(course_deviation_deg) < 90.0) {
576 _wp1_to_flag_node->setBoolValue(true);
578 // If it's more than 90 degrees the desired
579 // course is from the waypoint.
580 else if (fabs(course_deviation_deg) > 90.0) {
581 _wp1_to_flag_node->setBoolValue(false);
582 // When the course is away from the waypoint,
583 // it makes sense to change the sign of the deviation.
584 course_deviation_deg *= -1.0;
585 SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
588 _wp1_course_deviation_node->setDoubleValue(course_deviation_deg);
590 // Cross track error.
591 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
592 * (ctx.wp1_distance);
593 _wp1_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);