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>
11 #include <Aircraft/aircraft.hxx>
13 #include <simgear/route/route.hxx>
14 #include <simgear/math/sg_random.h>
15 #include <simgear/sg_inlines.h>
16 #include <Airports/simple.hxx>
18 #include <Main/fg_init.hxx>
19 #include <Main/globals.hxx>
20 #include <Main/fg_props.hxx>
21 #include <Main/util.hxx>
22 #include <Navaids/fixlist.hxx>
23 #include <Navaids/navlist.hxx>
30 GPS::GPS ( SGPropertyNode *node)
32 _last_longitude_deg(0),
33 _last_latitude_deg(0),
37 _wp0_longitude_deg(0),
40 _wp1_longitude_deg(0),
47 for ( i = 0; i < node->nChildren(); ++i ) {
48 SGPropertyNode *child = node->getChild(i);
49 string cname = child->getName();
50 string cval = child->getStringValue();
51 if ( cname == "name" ) {
53 } else if ( cname == "number" ) {
54 num = child->getIntValue();
56 SG_LOG( SG_INSTR, SG_WARN, "Error in gps config logic" );
57 if ( name.length() ) {
58 SG_LOG( SG_INSTR, SG_WARN, "Section = " << name );
66 _last_longitude_deg(0),
67 _last_latitude_deg(0),
71 _wp0_longitude_deg(0),
74 _wp1_longitude_deg(0),
93 branch = "/instrumentation/" + name;
95 SGPropertyNode *node = fgGetNode(branch.c_str(), num, true );
97 _longitude_node = fgGetNode("/position/longitude-deg", true);
98 _latitude_node = fgGetNode("/position/latitude-deg", true);
99 _altitude_node = fgGetNode("/position/altitude-ft", true);
100 _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
101 _serviceable_node = node->getChild("serviceable", 0, true);
102 _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
104 SGPropertyNode *wp_node = node->getChild("wp", 0, true);
105 SGPropertyNode *wp0_node = wp_node->getChild("wp", 0, true);
106 SGPropertyNode *wp1_node = wp_node->getChild("wp", 1, true);
107 addWp = wp1_node->getChild("Add-to-route", 0, true);
109 _wp0_longitude_node = wp0_node->getChild("longitude-deg", 0, true);
110 _wp0_latitude_node = wp0_node->getChild("latitude-deg", 0, true);
111 _wp0_altitude_node = wp0_node->getChild("altitude-ft", 0, true);
112 _wp0_ID_node = wp0_node->getChild("ID", 0, true);
113 _wp0_name_node = wp0_node->getChild("name", 0, true);
114 _wp0_course_node = wp0_node->getChild("desired-course-deg", 0, true);
115 _wp0_waypoint_type_node = wp0_node->getChild("waypoint-type", 0, true);
116 _wp0_distance_node = wp0_node->getChild("distance-nm", 0, true);
117 _wp0_ttw_node = wp0_node->getChild("TTW", 0, true);
118 _wp0_bearing_node = wp0_node->getChild("bearing-true-deg", 0, true);
119 _wp0_mag_bearing_node = wp0_node->getChild("bearing-mag-deg", 0, true);
120 _wp0_course_deviation_node =
121 wp0_node->getChild("course-deviation-deg", 0, true);
122 _wp0_course_error_nm_node = wp0_node->getChild("course-error-nm", 0, true);
123 _wp0_to_flag_node = wp0_node->getChild("to-flag", 0, true);
124 _true_wp0_bearing_error_node =
125 wp0_node->getChild("true-bearing-error-deg", 0, true);
126 _magnetic_wp0_bearing_error_node =
127 wp0_node->getChild("magnetic-bearing-error-deg", 0, true);
129 _wp1_longitude_node = wp1_node->getChild("longitude-deg", 0, true);
130 _wp1_latitude_node = wp1_node->getChild("latitude-deg", 0, true);
131 _wp1_altitude_node = wp1_node->getChild("altitude-ft", 0, true);
132 _wp1_ID_node = wp1_node->getChild("ID", 0, true);
133 _wp1_name_node = wp1_node->getChild("name", 0, true);
134 _wp1_course_node = wp1_node->getChild("desired-course-deg", 0, true);
135 _wp1_waypoint_type_node = wp1_node->getChild("waypoint-type", 0, true);
136 _wp1_distance_node = wp1_node->getChild("distance-nm", 0, true);
137 _wp1_ttw_node = wp1_node->getChild("TTW", 0, true);
138 _wp1_bearing_node = wp1_node->getChild("bearing-true-deg", 0, true);
139 _wp1_mag_bearing_node = wp1_node->getChild("bearing-mag-deg", 0, true);
140 _wp1_course_deviation_node =
141 wp1_node->getChild("course-deviation-deg", 0, true);
142 _wp1_course_error_nm_node = wp1_node->getChild("course-error-nm", 0, true);
143 _wp1_to_flag_node = wp1_node->getChild("to-flag", 0, true);
144 _true_wp1_bearing_error_node =
145 wp1_node->getChild("true-bearing-error-deg", 0, true);
146 _magnetic_wp1_bearing_error_node =
147 wp1_node->getChild("magnetic-bearing-error-deg", 0, true);
148 _get_nearest_airport_node =
149 wp1_node->getChild("get-nearest-airport", 0, true);
151 _tracking_bug_node = node->getChild("tracking-bug", 0, true);
152 _raim_node = node->getChild("raim", 0, true);
154 _indicated_longitude_node =
155 node->getChild("indicated-longitude-deg", 0, true);
156 _indicated_latitude_node =
157 node->getChild("indicated-latitude-deg", 0, true);
158 _indicated_altitude_node =
159 node->getChild("indicated-altitude-ft", 0, true);
160 _indicated_vertical_speed_node =
161 node->getChild("indicated-vertical-speed", 0, true);
163 node->getChild("indicated-track-true-deg", 0, true);
164 _magnetic_track_node =
165 node->getChild("indicated-track_magnetic-deg", 0, true);
167 node->getChild("indicated-ground-speed-kt", 0, true);
169 node->getChild("odometer", 0, true);
170 _trip_odometer_node =
171 node->getChild("trip-odometer", 0, true);
172 _true_bug_error_node =
173 node->getChild("true-bug-error-deg", 0, true);
174 _magnetic_bug_error_node =
175 node->getChild("magnetic-bug-error-deg", 0, true);
178 wp_node->getChild("leg-distance-nm", 0, true);
180 wp_node->getChild("leg-true-course-deg", 0, true);
181 _leg_magnetic_course_node =
182 wp_node->getChild("leg-mag-course-deg", 0, true);
183 _alt_dist_ratio_node =
184 wp_node->getChild("alt-dist-ratio", 0, true);
185 _leg_course_deviation_node =
186 wp_node->getChild("leg-course-deviation-deg", 0, true);
187 _leg_course_error_nm_node =
188 wp_node->getChild("leg-course-error-nm", 0, true);
190 wp_node->getChild("leg-to-flag", 0, true);
191 _alt_deviation_node =
192 wp_node->getChild("alt-deviation-ft", 0, true);
194 _route = node->getChild("route", 0, true);
195 popWp = _route->getChild("Pop-WP", 0, true);
197 addWp->setBoolValue(false);
198 popWp->setBoolValue(false);
200 _serviceable_node->setBoolValue(true);
204 GPS::update (double delta_time_sec)
206 // If it's off, don't bother.
207 if (!_serviceable_node->getBoolValue() ||
208 !_electrical_node->getBoolValue()) {
210 _last_longitude_deg = 0;
211 _last_latitude_deg = 0;
212 _last_altitude_m = 0;
214 _raim_node->setDoubleValue(false);
215 _indicated_longitude_node->setDoubleValue(0);
216 _indicated_latitude_node->setDoubleValue(0);
217 _indicated_altitude_node->setDoubleValue(0);
218 _indicated_vertical_speed_node->setDoubleValue(0);
219 _true_track_node->setDoubleValue(0);
220 _magnetic_track_node->setDoubleValue(0);
221 _speed_node->setDoubleValue(0);
222 _wp1_distance_node->setDoubleValue(0);
223 _wp1_bearing_node->setDoubleValue(0);
224 _wp1_longitude_node->setDoubleValue(0);
225 _wp1_latitude_node->setDoubleValue(0);
226 _wp1_course_node->setDoubleValue(0);
227 _odometer_node->setDoubleValue(0);
228 _trip_odometer_node->setDoubleValue(0);
229 _tracking_bug_node->setDoubleValue(0);
230 _true_bug_error_node->setDoubleValue(0);
231 _magnetic_bug_error_node->setDoubleValue(0);
232 _true_wp1_bearing_error_node->setDoubleValue(0);
233 _magnetic_wp1_bearing_error_node->setDoubleValue(0);
237 // Get the aircraft position
238 // TODO: Add noise and other errors.
239 double longitude_deg = _longitude_node->getDoubleValue();
240 double latitude_deg = _latitude_node->getDoubleValue();
241 double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
242 double magvar_deg = _magvar_node->getDoubleValue();
246 // Bias and random error
247 double random_factor = sg_random();
248 double random_error = 1.4;
249 double error_radius = 5.1;
250 double bias_max_radius = 5.1;
251 double random_max_radius = 1.4;
253 bias_length += (random_factor-0.5) * 1.0e-3;
254 if (bias_length <= 0.0) bias_length = 0.0;
255 else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
256 bias_angle += (random_factor-0.5) * 1.0e-3;
257 if (bias_angle <= 0.0) bias_angle = 0.0;
258 else if (bias_angle >= 360.0) bias_angle = 360.0;
260 double random_length = random_factor * random_max_radius;
261 double random_angle = random_factor * 360.0;
263 double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
264 double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
265 double random_x = random_length * cos(random_angle * SG_PI / 180.0);
266 double random_y = random_length * sin(random_angle * SG_PI / 180.0);
267 double error_x = bias_x + random_x;
268 double error_y = bias_y + random_y;
269 double error_length = sqrt(error_x*error_x + error_y*error_y);
270 double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
275 geo_direct_wgs_84 ( altitude_m, latitude_deg,
276 longitude_deg, error_angle,
277 error_length, &lat2, &lon2,
279 //cout << lat2 << " " << lon2 << endl;
280 printf("%f %f \n", bias_length, bias_angle);
281 printf("%3.7f %3.7f \n", lat2, lon2);
282 printf("%f %f \n", error_length, error_angle);
288 double speed_kt, vertical_speed_mpm;
290 _raim_node->setBoolValue(true);
291 _indicated_longitude_node->setDoubleValue(longitude_deg);
292 _indicated_latitude_node->setDoubleValue(latitude_deg);
293 _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
296 double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
297 geo_inverse_wgs_84(altitude_m,
298 _last_latitude_deg, _last_longitude_deg,
299 latitude_deg, longitude_deg,
300 &track1_deg, &track2_deg, &distance_m);
301 speed_kt = ((distance_m * SG_METER_TO_NM) *
302 ((1 / delta_time_sec) * 3600.0));
303 vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
305 _indicated_vertical_speed_node->setDoubleValue
306 (vertical_speed_mpm * SG_METER_TO_FEET);
307 _true_track_node->setDoubleValue(track1_deg);
308 mag_track_bearing = track1_deg - magvar_deg;
309 SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
310 _magnetic_track_node->setDoubleValue(mag_track_bearing);
311 speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
312 _last_speed_kts = speed_kt;
313 _speed_node->setDoubleValue(speed_kt);
315 odometer = _odometer_node->getDoubleValue();
316 _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
317 odometer = _trip_odometer_node->getDoubleValue();
318 _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
320 // Get waypoint 0 position
321 double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
322 double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
323 double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
325 double wp0_course_deg = _wp0_course_node->getDoubleValue();
326 double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
327 wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
328 string wp0_ID = _wp0_ID_node->getStringValue();
330 // Get waypoint 1 position
331 double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
332 double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
333 double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
335 double wp1_course_deg = _wp1_course_node->getDoubleValue();
336 double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
337 wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
338 string wp1_ID = _wp1_ID_node->getStringValue();
340 // If the get-nearest-airport-node is true.
341 // Get the nearest airport, and set it as waypoint 1.
342 if (_get_nearest_airport_node->getBoolValue()) {
344 //cout << "Airport found" << endl;
345 a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
346 _wp1_ID_node->setStringValue(a.getId().c_str());
347 wp1_longitude_deg = a.getLongitude();
348 wp1_latitude_deg = a.getLatitude();
349 _wp1_name_node->setStringValue(a.getName().c_str());
350 _get_nearest_airport_node->setBoolValue(false);
351 _last_wp1_ID = wp1_ID = a.getId().c_str();
354 // If the waypoint 0 ID has changed, try to find the new ID
355 // in the airport-, fix-, nav-database.
356 if ( !(_last_wp0_ID == wp0_ID) ) {
357 string waypont_type =
358 _wp0_waypoint_type_node->getStringValue();
359 if (waypont_type == "airport") {
361 a = globals->get_airports()->search( wp0_ID );
362 if ( a.getId() == wp0_ID ) {
363 //cout << "Airport found" << endl;
364 wp0_longitude_deg = a.getLongitude();
365 wp0_latitude_deg = a.getLatitude();
366 _wp0_name_node->setStringValue(a.getName().c_str());
369 else if (waypont_type == "nav") {
371 = globals->get_navlist()->findByIdent(wp0_ID.c_str(),
375 //cout << "Nav found" << endl;
376 wp0_longitude_deg = n->get_lon();
377 wp0_latitude_deg = n->get_lat();
378 _wp0_name_node->setStringValue(n->get_name().c_str());
381 else if (waypont_type == "fix") {
383 if ( globals->get_fixlist()->query(wp0_ID, &f) ) {
384 //cout << "Fix found" << endl;
385 wp0_longitude_deg = f.get_lon();
386 wp0_latitude_deg = f.get_lat();
387 _wp0_name_node->setStringValue(wp0_ID.c_str());
390 _last_wp0_ID = wp0_ID;
393 // If the waypoint 1 ID has changed, try to find the new ID
394 // in the airport-, fix-, nav-database.
395 if ( !(_last_wp1_ID == wp1_ID) ) {
396 string waypont_type =
397 _wp1_waypoint_type_node->getStringValue();
398 if (waypont_type == "airport") {
400 a = globals->get_airports()->search( wp1_ID );
401 if ( a.getId() == wp1_ID ) {
402 //cout << "Airport found" << endl;
403 wp1_longitude_deg = a.getLongitude();
404 wp1_latitude_deg = a.getLatitude();
405 _wp1_name_node->setStringValue(a.getName().c_str());
408 else if (waypont_type == "nav") {
410 = globals->get_navlist()->findByIdent(wp1_ID.c_str(),
414 //cout << "Nav found" << endl;
415 wp1_longitude_deg = n->get_lon();
416 wp1_latitude_deg = n->get_lat();
417 _wp1_name_node->setStringValue(n->get_name().c_str());
420 else if (waypont_type == "fix") {
422 if ( globals->get_fixlist()->query(wp1_ID, &f) ) {
423 //cout << "Fix found" << endl;
424 wp1_longitude_deg = f.get_lon();
425 wp1_latitude_deg = f.get_lat();
426 _wp1_name_node->setStringValue(wp1_ID.c_str());
429 _last_wp1_ID = wp1_ID;
434 // If any of the two waypoints have changed
435 // we need to calculate a new course between them,
436 // and values for vertical navigation.
437 if ( wp0_longitude_deg != _wp0_longitude_deg ||
438 wp0_latitude_deg != _wp0_latitude_deg ||
439 wp0_altitude_m != _wp0_altitude_m ||
440 wp1_longitude_deg != _wp1_longitude_deg ||
441 wp1_latitude_deg != _wp1_latitude_deg ||
442 wp1_altitude_m != _wp1_altitude_m )
444 // Update the global variables
445 _wp0_longitude_deg = wp0_longitude_deg;
446 _wp0_latitude_deg = wp0_latitude_deg;
447 _wp0_altitude_m = wp0_altitude_m;
448 _wp1_longitude_deg = wp1_longitude_deg;
449 _wp1_latitude_deg = wp1_latitude_deg;
450 _wp1_altitude_m = wp1_altitude_m;
452 // Get the course and distance from wp0 to wp1
453 SGWayPoint wp0(wp0_longitude_deg,
454 wp0_latitude_deg, wp0_altitude_m);
455 SGWayPoint wp1(wp1_longitude_deg,
456 wp1_latitude_deg, wp1_altitude_m);
458 wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
459 double leg_mag_course = _course_deg - magvar_deg;
460 SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
462 // Get the altitude / distance ratio
463 if ( distance_m > 0.0 ) {
464 double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
465 _alt_dist_ratio = alt_difference_m / _distance_m;
468 _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
469 _leg_course_node->setDoubleValue(_course_deg);
470 _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
471 _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
473 _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
474 _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
475 _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
476 _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
480 // Find the bearing and distance to waypoint 0.
481 SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
482 wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
483 &wp0_bearing_deg, &wp0_distance);
484 _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
485 _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
486 double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg;
487 SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0);
488 _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
489 wp0_bearing_error_deg = track1_deg - wp0_bearing_deg;
490 SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0);
491 _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
493 // Estimate time to waypoint 0.
494 // The estimation does not take track into consideration,
495 // so if you are going away from the waypoint the TTW will
496 // increase. Makes most sense when travelling directly towards
498 if (speed_kt > 0.0 && wp0_distance > 0.0) {
499 wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
504 unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
505 if (wp0_TTW_seconds < 356400) { // That's 99 hours
506 unsigned int wp0_TTW_minutes = 0;
507 unsigned int wp0_TTW_hours = 0;
509 while (wp0_TTW_seconds >= 3600) {
510 wp0_TTW_seconds -= 3600;
513 while (wp0_TTW_seconds >= 60) {
514 wp0_TTW_seconds -= 60;
517 snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d",
518 wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
519 _wp0_ttw_node->setStringValue(wp0_TTW_str);
522 _wp0_ttw_node->setStringValue("--:--:--");
524 // Course deviation is the diffenrence between the bearing
526 wp0_course_deviation_deg = wp0_bearing_deg -
528 SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -180.0, 180.0);
530 // If the course deviation is less than 90 degrees to either side,
531 // our desired course is towards the waypoint.
532 // It does not matter if we are actually moving
533 // towards or from the waypoint.
534 if (fabs(wp0_course_deviation_deg) < 90.0) {
535 _wp0_to_flag_node->setBoolValue(true); }
536 // If it's more than 90 degrees the desired
537 // course is from the waypoint.
538 else if (fabs(wp0_course_deviation_deg) > 90.0) {
539 _wp0_to_flag_node->setBoolValue(false);
540 // When the course is away from the waypoint,
541 // it makes sense to change the sign of the deviation.
542 wp0_course_deviation_deg *= -1.0;
543 SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -90.0, 90.0);
546 _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
548 // Cross track error.
549 wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
551 _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
556 // Find the bearing and distance to waypoint 1.
557 SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
558 wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
559 &wp1_bearing_deg, &wp1_distance);
560 _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
561 _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
562 double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg;
563 SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0);
564 _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
565 wp1_bearing_error_deg = track1_deg - wp1_bearing_deg;
566 SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0);
567 _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
569 // Estimate time to waypoint 1.
570 // The estimation does not take track into consideration,
571 // so if you are going away from the waypoint the TTW will
572 // increase. Makes most sense when travelling directly towards
574 if (speed_kt > 0.0 && wp1_distance > 0.0) {
575 wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
580 unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
581 if (wp1_TTW_seconds < 356400) { // That's 99 hours
582 unsigned int wp1_TTW_minutes = 0;
583 unsigned int wp1_TTW_hours = 0;
585 while (wp1_TTW_seconds >= 3600) {
586 wp1_TTW_seconds -= 3600;
589 while (wp1_TTW_seconds >= 60) {
590 wp1_TTW_seconds -= 60;
593 snprintf(wp1_TTW_str, 9, "%02d:%02d:%02d",
594 wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
595 _wp1_ttw_node->setStringValue(wp1_TTW_str);
598 _wp1_ttw_node->setStringValue("--:--:--");
600 // Course deviation is the diffenrence between the bearing
602 wp1_course_deviation_deg = wp1_bearing_deg - wp1_course_deg;
603 SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -180.0, 180.0);
605 // If the course deviation is less than 90 degrees to either side,
606 // our desired course is towards the waypoint.
607 // It does not matter if we are actually moving
608 // towards or from the waypoint.
609 if (fabs(wp1_course_deviation_deg) < 90.0) {
610 _wp1_to_flag_node->setBoolValue(true); }
611 // If it's more than 90 degrees the desired
612 // course is from the waypoint.
613 else if (fabs(wp1_course_deviation_deg) > 90.0) {
614 _wp1_to_flag_node->setBoolValue(false);
615 // When the course is away from the waypoint,
616 // it makes sense to change the sign of the deviation.
617 wp1_course_deviation_deg *= -1.0;
618 SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0);
621 _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
623 // Cross track error.
624 wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
626 _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
630 // Leg course deviation is the diffenrence between the bearing
632 double course_deviation_deg = wp1_bearing_deg - _course_deg;
633 SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
635 // If the course deviation is less than 90 degrees to either side,
636 // our desired course is towards the waypoint.
637 // It does not matter if we are actually moving
638 // towards or from the waypoint.
639 if (fabs(course_deviation_deg) < 90.0) {
640 _leg_to_flag_node->setBoolValue(true); }
641 // If it's more than 90 degrees the desired
642 // course is from the waypoint.
643 else if (fabs(course_deviation_deg) > 90.0) {
644 _leg_to_flag_node->setBoolValue(false);
645 // When the course is away from the waypoint,
646 // it makes sense to change the sign of the deviation.
647 course_deviation_deg *= -1.0;
648 SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
651 _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
653 // Cross track error.
654 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
656 _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
658 // Altitude deviation
659 double desired_altitude_m = wp1_altitude_m
660 + wp1_distance * _alt_dist_ratio;
661 double altitude_deviation_m = altitude_m - desired_altitude_m;
662 _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
667 double tracking_bug = _tracking_bug_node->getDoubleValue();
668 double true_bug_error = tracking_bug - track1_deg;
669 double magnetic_bug_error = tracking_bug - mag_track_bearing;
671 // Get the errors into the (-180,180) range.
672 SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
673 SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
675 _true_bug_error_node->setDoubleValue(true_bug_error);
676 _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
679 // Add WP 1 to the route.
680 if ( addWp->getBoolValue() )
682 addWp->setBoolValue(false);
684 SGWayPoint tempWp( _wp1_longitude_node->getDoubleValue(),
685 _wp1_latitude_node->getDoubleValue(),
686 _wp1_altitude_node->getDoubleValue(),
688 _wp1_ID_node->getStringValue(),
689 _wp1_name_node->getStringValue() );
691 route->add_waypoint(tempWp);
694 _route->getChild("Waypoint", route->size()-1, true);
695 SGPropertyNode *id = wp->getChild("ID", 0, true);
696 SGPropertyNode *name = wp->getChild("Name", 0, true);
697 SGPropertyNode *lat = wp->getChild("Latitude", 0, true);
698 SGPropertyNode *lon = wp->getChild("Longitude", 0, true);
699 SGPropertyNode *alt = wp->getChild("Altitude", 0, true);
701 id->setStringValue( tempWp.get_id().c_str() );
702 name->setStringValue( tempWp.get_name().c_str() );
703 lat->setDoubleValue( tempWp.get_target_lat() );
704 lon->setDoubleValue( tempWp.get_target_lon() );
705 alt->setDoubleValue( tempWp.get_target_alt() );
708 if ( popWp->getBoolValue() )
710 popWp->setBoolValue(false);
712 route->delete_first();
713 _route->removeChild("Waypoint", 0);
717 _true_track_node->setDoubleValue(0.0);
718 _magnetic_track_node->setDoubleValue(0.0);
719 _speed_node->setDoubleValue(0.0);
723 _last_longitude_deg = longitude_deg;
724 _last_latitude_deg = latitude_deg;
725 _last_altitude_m = altitude_m;