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>
32 _last_longitude_deg(0),
33 _last_latitude_deg(0),
46 _longitude_node = fgGetNode("/position/longitude-deg", true);
47 _latitude_node = fgGetNode("/position/latitude-deg", true);
48 _altitude_node = fgGetNode("/position/altitude-ft", true);
49 _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
50 _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
51 _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
53 fgGetNode("/instrumentation/gps/wp/wp[0]/longitude-deg", true);
55 fgGetNode("/instrumentation/gps/wp/wp[0]/latitude-deg", true);
57 fgGetNode("/instrumentation/gps/wp/wp[0]/altitude-ft", true);
59 fgGetNode("/instrumentation/gps/wp/wp[0]/ID", true);
61 fgGetNode("/instrumentation/gps/wp/wp[0]/name", true);
63 fgGetNode("/instrumentation/gps/wp/wp[0]/desired-course-deg", true);
64 _wp0_waypoint_type_node =
65 fgGetNode("/instrumentation/gps/wp/wp[0]/waypoint-type", true);
67 fgGetNode("/instrumentation/gps/wp/wp[1]/longitude-deg", true);
69 fgGetNode("/instrumentation/gps/wp/wp[1]/latitude-deg", true);
71 fgGetNode("/instrumentation/gps/wp/wp[1]/altitude-ft", true);
73 fgGetNode("/instrumentation/gps/wp/wp[1]/ID", true);
75 fgGetNode("/instrumentation/gps/wp/wp[1]/name", true);
77 fgGetNode("/instrumentation/gps/wp/wp[1]/desired-course-deg", true);
78 _wp1_waypoint_type_node =
79 fgGetNode("/instrumentation/gps/wp/wp[1]/waypoint-type", true);
80 _get_nearest_airport_node =
81 fgGetNode("/instrumentation/gps/wp/wp[1]/get-nearest-airport", true);
83 fgGetNode("/instrumentation/gps/tracking-bug", true);
85 _raim_node = fgGetNode("/instrumentation/gps/raim", true);
86 _indicated_longitude_node =
87 fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
88 _indicated_latitude_node =
89 fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
90 _indicated_altitude_node =
91 fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
92 _indicated_vertical_speed_node =
93 fgGetNode("/instrumentation/gps/indicated-vertical-speed", true);
95 fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
96 _magnetic_track_node =
97 fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
99 fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
101 fgGetNode("/instrumentation/gps/wp/wp[0]/distance-nm", true);
103 fgGetNode("/instrumentation/gps/wp/wp[0]/TTW",true);
105 fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-true-deg", true);
106 _wp0_mag_bearing_node =
107 fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-mag-deg", true);
108 _wp0_course_deviation_node =
109 fgGetNode("/instrumentation/gps/wp/wp[0]/course-deviation-deg", true);
110 _wp0_course_error_nm_node =
111 fgGetNode("/instrumentation/gps/wp/wp[0]/course-error-nm", true);
113 fgGetNode("/instrumentation/gps/wp/wp[0]/to-flag", true);
115 fgGetNode("/instrumentation/gps/wp/wp[1]/distance-nm", true);
117 fgGetNode("/instrumentation/gps/wp/wp[1]/TTW",true);
119 fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-true-deg", true);
120 _wp1_mag_bearing_node =
121 fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-mag-deg", true);
122 _wp1_course_deviation_node =
123 fgGetNode("/instrumentation/gps/wp/wp[1]/course-deviation-deg", true);
124 _wp1_course_error_nm_node =
125 fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
127 fgGetNode("/instrumentation/gps/wp/wp[1]/to-flag", true);
129 fgGetNode("/instrumentation/gps/odometer", true);
130 _trip_odometer_node =
131 fgGetNode("/instrumentation/gps/trip-odometer", true);
132 _true_bug_error_node =
133 fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
134 _magnetic_bug_error_node =
135 fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
136 _true_wp0_bearing_error_node =
137 fgGetNode("/instrumentation/gps/wp/wp[0]/true-bearing-error-deg", true);
138 _magnetic_wp0_bearing_error_node =
139 fgGetNode("/instrumentation/gps/wp/wp[0]/magnetic-bearing-error-deg", true);
140 _true_wp1_bearing_error_node =
141 fgGetNode("/instrumentation/gps/wp/wp[1]/true-bearing-error-deg", true);
142 _magnetic_wp1_bearing_error_node =
143 fgGetNode("/instrumentation/gps/wp/wp[1]/magnetic-bearing-error-deg", true);
145 fgGetNode("/instrumentation/gps/wp/leg-distance-nm", true);
147 fgGetNode("/instrumentation/gps/wp/leg-true-course-deg", true);
148 _leg_magnetic_course_node =
149 fgGetNode("/instrumentation/gps/wp/leg-mag-course-deg", true);
150 _alt_dist_ratio_node =
151 fgGetNode("/instrumentation/gps/wp/alt-dist-ratio", true);
152 _leg_course_deviation_node =
153 fgGetNode("/instrumentation/gps/wp/leg-course-deviation-deg", true);
154 _leg_course_error_nm_node =
155 fgGetNode("/instrumentation/gps/wp/leg-course-error-nm", true);
157 fgGetNode("/instrumentation/gps/wp/leg-to-flag", true);
158 _alt_deviation_node =
159 fgGetNode("/instrumentation/gps/wp/alt-deviation-ft", true);
163 GPS::update (double delta_time_sec)
165 // If it's off, don't bother.
166 if (!_serviceable_node->getBoolValue() ||
167 !_electrical_node->getBoolValue()) {
169 _last_longitude_deg = 0;
170 _last_latitude_deg = 0;
171 _last_altitude_m = 0;
173 _raim_node->setDoubleValue(false);
174 _indicated_longitude_node->setDoubleValue(0);
175 _indicated_latitude_node->setDoubleValue(0);
176 _indicated_altitude_node->setDoubleValue(0);
177 _indicated_vertical_speed_node->setDoubleValue(0);
178 _true_track_node->setDoubleValue(0);
179 _magnetic_track_node->setDoubleValue(0);
180 _speed_node->setDoubleValue(0);
181 _wp1_distance_node->setDoubleValue(0);
182 _wp1_bearing_node->setDoubleValue(0);
183 _wp1_longitude_node->setDoubleValue(0);
184 _wp1_latitude_node->setDoubleValue(0);
185 _wp1_course_node->setDoubleValue(0);
186 _odometer_node->setDoubleValue(0);
187 _trip_odometer_node->setDoubleValue(0);
188 _tracking_bug_node->setDoubleValue(0);
189 _true_bug_error_node->setDoubleValue(0);
190 _magnetic_bug_error_node->setDoubleValue(0);
191 _true_wp1_bearing_error_node->setDoubleValue(0);
192 _magnetic_wp1_bearing_error_node->setDoubleValue(0);
196 // Get the aircraft position
197 // TODO: Add noise and other errors.
198 double longitude_deg = _longitude_node->getDoubleValue();
199 double latitude_deg = _latitude_node->getDoubleValue();
200 double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
201 double magvar_deg = _magvar_node->getDoubleValue();
205 // Bias and random error
206 double random_factor = sg_random();
207 double random_error = 1.4;
208 double error_radius = 5.1;
209 double bias_max_radius = 5.1;
210 double random_max_radius = 1.4;
212 bias_length += (random_factor-0.5) * 1.0e-3;
213 if (bias_length <= 0.0) bias_length = 0.0;
214 else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
215 bias_angle += (random_factor-0.5) * 1.0e-3;
216 if (bias_angle <= 0.0) bias_angle = 0.0;
217 else if (bias_angle >= 360.0) bias_angle = 360.0;
219 double random_length = random_factor * random_max_radius;
220 double random_angle = random_factor * 360.0;
222 double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
223 double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
224 double random_x = random_length * cos(random_angle * SG_PI / 180.0);
225 double random_y = random_length * sin(random_angle * SG_PI / 180.0);
226 double error_x = bias_x + random_x;
227 double error_y = bias_y + random_y;
228 double error_length = sqrt(error_x*error_x + error_y*error_y);
229 double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
234 geo_direct_wgs_84 ( altitude_m, latitude_deg,
235 longitude_deg, error_angle,
236 error_length, &lat2, &lon2,
238 //cout << lat2 << " " << lon2 << endl;
239 printf("%f %f \n", bias_length, bias_angle);
240 printf("%3.7f %3.7f \n", lat2, lon2);
241 printf("%f %f \n", error_length, error_angle);
247 double speed_kt, vertical_speed_mpm;
249 _raim_node->setBoolValue(true);
250 _indicated_longitude_node->setDoubleValue(longitude_deg);
251 _indicated_latitude_node->setDoubleValue(latitude_deg);
252 _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
255 double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
256 geo_inverse_wgs_84(altitude_m,
257 _last_latitude_deg, _last_longitude_deg,
258 latitude_deg, longitude_deg,
259 &track1_deg, &track2_deg, &distance_m);
260 speed_kt = ((distance_m * SG_METER_TO_NM) *
261 ((1 / delta_time_sec) * 3600.0));
262 vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
264 _indicated_vertical_speed_node->setDoubleValue
265 (vertical_speed_mpm * SG_METER_TO_FEET);
266 _true_track_node->setDoubleValue(track1_deg);
267 mag_track_bearing = track1_deg - magvar_deg;
268 SG_NORMALIZE_RANGE(mag_track_bearing, 0.0, 360.0);
269 _magnetic_track_node->setDoubleValue(mag_track_bearing);
270 speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
271 _last_speed_kts = speed_kt;
272 _speed_node->setDoubleValue(speed_kt);
274 odometer = _odometer_node->getDoubleValue();
275 _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
276 odometer = _trip_odometer_node->getDoubleValue();
277 _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
279 // Get waypoint 0 position
280 double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
281 double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
282 double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
284 double wp0_course_deg = _wp0_course_node->getDoubleValue();
285 double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
286 wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
287 string wp0_ID = _wp0_ID_node->getStringValue();
289 // Get waypoint 1 position
290 double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
291 double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
292 double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
294 double wp1_course_deg = _wp1_course_node->getDoubleValue();
295 double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
296 wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
297 string wp1_ID = _wp1_ID_node->getStringValue();
299 // If the get-nearest-airport-node is true.
300 // Get the nearest airport, and set it as waypoint 1.
301 if (_get_nearest_airport_node->getBoolValue()) {
303 //cout << "Airport found" << endl;
304 a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
305 _wp1_ID_node->setStringValue(a.id.c_str());
306 wp1_longitude_deg = a.longitude;
307 wp1_latitude_deg = a.latitude;
308 _wp1_name_node->setStringValue(a.name.c_str());
309 _get_nearest_airport_node->setBoolValue(false);
310 _last_wp1_ID = wp1_ID = a.id.c_str();
313 // If the waypoint 0 ID has changed, try to find the new ID
314 // in the airport-, fix-, nav-database.
315 if ( !(_last_wp0_ID == wp0_ID) ) {
316 string waypont_type =
317 _wp0_waypoint_type_node->getStringValue();
318 if (waypont_type == "airport") {
320 a = globals->get_airports()->search( wp0_ID );
321 if ( a.id == wp0_ID ) {
322 //cout << "Airport found" << endl;
323 wp0_longitude_deg = a.longitude;
324 wp0_latitude_deg = a.latitude;
325 _wp0_name_node->setStringValue(a.name.c_str());
328 else if (waypont_type == "nav") {
330 = globals->get_navlist()->findByIdent(wp0_ID.c_str(),
334 //cout << "Nav found" << endl;
335 wp0_longitude_deg = n->get_lon();
336 wp0_latitude_deg = n->get_lat();
337 _wp0_name_node->setStringValue(n->get_name().c_str());
340 else if (waypont_type == "fix") {
342 if ( globals->get_fixlist()->query(wp0_ID, &f) ) {
343 //cout << "Fix found" << endl;
344 wp0_longitude_deg = f.get_lon();
345 wp0_latitude_deg = f.get_lat();
346 _wp0_name_node->setStringValue(wp0_ID.c_str());
349 _last_wp0_ID = wp0_ID;
352 // If the waypoint 1 ID has changed, try to find the new ID
353 // in the airport-, fix-, nav-database.
354 if ( !(_last_wp1_ID == wp1_ID) ) {
355 string waypont_type =
356 _wp1_waypoint_type_node->getStringValue();
357 if (waypont_type == "airport") {
359 a = globals->get_airports()->search( wp1_ID );
360 if ( a.id == wp1_ID ) {
361 //cout << "Airport found" << endl;
362 wp1_longitude_deg = a.longitude;
363 wp1_latitude_deg = a.latitude;
364 _wp1_name_node->setStringValue(a.name.c_str());
367 else if (waypont_type == "nav") {
369 = globals->get_navlist()->findByIdent(wp1_ID.c_str(),
373 //cout << "Nav found" << endl;
374 wp1_longitude_deg = n->get_lon();
375 wp1_latitude_deg = n->get_lat();
376 _wp1_name_node->setStringValue(n->get_name().c_str());
379 else if (waypont_type == "fix") {
381 if ( globals->get_fixlist()->query(wp1_ID, &f) ) {
382 //cout << "Fix found" << endl;
383 wp1_longitude_deg = f.get_lon();
384 wp1_latitude_deg = f.get_lat();
385 _wp1_name_node->setStringValue(wp1_ID.c_str());
388 _last_wp1_ID = wp1_ID;
393 // If any of the two waypoints have changed
394 // we need to calculate a new course between them,
395 // and values for vertical navigation.
396 if ( wp0_longitude_deg != _wp0_longitude_deg ||
397 wp0_latitude_deg != _wp0_latitude_deg ||
398 wp0_altitude_m != _wp0_altitude_m ||
399 wp1_longitude_deg != _wp1_longitude_deg ||
400 wp1_latitude_deg != _wp1_latitude_deg ||
401 wp1_altitude_m != _wp1_altitude_m )
403 // Update the global variables
404 _wp0_longitude_deg = wp0_longitude_deg;
405 _wp0_latitude_deg = wp0_latitude_deg;
406 _wp0_altitude_m = wp0_altitude_m;
407 _wp1_longitude_deg = wp1_longitude_deg;
408 _wp1_latitude_deg = wp1_latitude_deg;
409 _wp1_altitude_m = wp1_altitude_m;
411 // Get the course and distance from wp0 to wp1
412 SGWayPoint wp0(wp0_longitude_deg,
413 wp0_latitude_deg, wp0_altitude_m);
414 SGWayPoint wp1(wp1_longitude_deg,
415 wp1_latitude_deg, wp1_altitude_m);
417 wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
418 double leg_mag_course = _course_deg - magvar_deg;
419 SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
421 // Get the altitude / distance ratio
422 if ( distance_m > 0.0 ) {
423 double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
424 _alt_dist_ratio = alt_difference_m / _distance_m;
427 _leg_distance_node->setDoubleValue(_distance_m * SG_METER_TO_NM);
428 _leg_course_node->setDoubleValue(_course_deg);
429 _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
430 _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
432 _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
433 _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
434 _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
435 _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
439 // Find the bearing and distance to waypoint 0.
440 SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
441 wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
442 &wp0_bearing_deg, &wp0_distance);
443 _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
444 _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
445 double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg;
446 SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0);
447 _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
448 wp0_bearing_error_deg = track1_deg - wp0_bearing_deg;
449 SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0);
450 _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
452 // Estimate time to waypoint 0.
453 // The estimation does not take track into consideration,
454 // so if you are going away from the waypoint the TTW will
455 // increase. Makes most sense when travelling directly towards
457 if (speed_kt > 0.0 && wp0_distance > 0.0) {
458 wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
463 unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
464 if (wp0_TTW_seconds < 356400) { // That's 99 hours
465 unsigned int wp0_TTW_minutes = 0;
466 unsigned int wp0_TTW_hours = 0;
468 while (wp0_TTW_seconds >= 3600) {
469 wp0_TTW_seconds -= 3600;
472 while (wp0_TTW_seconds >= 60) {
473 wp0_TTW_seconds -= 60;
476 snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d",
477 wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
478 _wp0_ttw_node->setStringValue(wp0_TTW_str);
481 _wp0_ttw_node->setStringValue("--:--:--");
483 // Course deviation is the diffenrence between the bearing
485 wp0_course_deviation_deg = wp0_bearing_deg -
487 SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -180.0, 180.0);
489 // If the course deviation is less than 90 degrees to either side,
490 // our desired course is towards the waypoint.
491 // It does not matter if we are actually moving
492 // towards or from the waypoint.
493 if (fabs(wp0_course_deviation_deg) < 90.0) {
494 _wp0_to_flag_node->setBoolValue(true); }
495 // If it's more than 90 degrees the desired
496 // course is from the waypoint.
497 else if (fabs(wp0_course_deviation_deg) > 90.0) {
498 _wp0_to_flag_node->setBoolValue(false);
499 // When the course is away from the waypoint,
500 // it makes sense to change the sign of the deviation.
501 wp0_course_deviation_deg *= -1.0;
502 SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -90.0, 90.0);
505 _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
507 // Cross track error.
508 wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
510 _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
515 // Find the bearing and distance to waypoint 1.
516 SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
517 wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
518 &wp1_bearing_deg, &wp1_distance);
519 _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
520 _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
521 double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg;
522 SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0);
523 _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
524 wp1_bearing_error_deg = track1_deg - wp1_bearing_deg;
525 SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0);
526 _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
528 // Estimate time to waypoint 1.
529 // The estimation does not take track into consideration,
530 // so if you are going away from the waypoint the TTW will
531 // increase. Makes most sense when travelling directly towards
533 if (speed_kt > 0.0 && wp1_distance > 0.0) {
534 wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
539 unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
540 if (wp1_TTW_seconds < 356400) { // That's 99 hours
541 unsigned int wp1_TTW_minutes = 0;
542 unsigned int wp1_TTW_hours = 0;
544 while (wp1_TTW_seconds >= 3600) {
545 wp1_TTW_seconds -= 3600;
548 while (wp1_TTW_seconds >= 60) {
549 wp1_TTW_seconds -= 60;
552 snprintf(wp1_TTW_str, 9, "%02d:%02d:%02d",
553 wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
554 _wp1_ttw_node->setStringValue(wp1_TTW_str);
557 _wp1_ttw_node->setStringValue("--:--:--");
559 // Course deviation is the diffenrence between the bearing
561 wp1_course_deviation_deg = wp1_bearing_deg - wp1_course_deg;
562 SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -180.0, 180.0);
564 // If the course deviation is less than 90 degrees to either side,
565 // our desired course is towards the waypoint.
566 // It does not matter if we are actually moving
567 // towards or from the waypoint.
568 if (fabs(wp1_course_deviation_deg) < 90.0) {
569 _wp1_to_flag_node->setBoolValue(true); }
570 // If it's more than 90 degrees the desired
571 // course is from the waypoint.
572 else if (fabs(wp1_course_deviation_deg) > 90.0) {
573 _wp1_to_flag_node->setBoolValue(false);
574 // When the course is away from the waypoint,
575 // it makes sense to change the sign of the deviation.
576 wp1_course_deviation_deg *= -1.0;
577 SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0);
580 _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
582 // Cross track error.
583 wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
585 _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
589 // Leg course deviation is the diffenrence between the bearing
591 double course_deviation_deg = wp1_bearing_deg - _course_deg;
592 SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
594 // If the course deviation is less than 90 degrees to either side,
595 // our desired course is towards the waypoint.
596 // It does not matter if we are actually moving
597 // towards or from the waypoint.
598 if (fabs(course_deviation_deg) < 90.0) {
599 _leg_to_flag_node->setBoolValue(true); }
600 // If it's more than 90 degrees the desired
601 // course is from the waypoint.
602 else if (fabs(course_deviation_deg) > 90.0) {
603 _leg_to_flag_node->setBoolValue(false);
604 // When the course is away from the waypoint,
605 // it makes sense to change the sign of the deviation.
606 course_deviation_deg *= -1.0;
607 SG_NORMALIZE_RANGE(course_deviation_deg, -90.0, 90.0);
610 _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
612 // Cross track error.
613 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
615 _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
617 // Altitude deviation
618 double desired_altitude_m = wp1_altitude_m
619 + wp1_distance * _alt_dist_ratio;
620 double altitude_deviation_m = altitude_m - desired_altitude_m;
621 _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
626 double tracking_bug = _tracking_bug_node->getDoubleValue();
627 double true_bug_error = tracking_bug - track1_deg;
628 double magnetic_bug_error = tracking_bug - mag_track_bearing;
630 // Get the errors into the (-180,180) range.
631 SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
632 SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
634 _true_bug_error_node->setDoubleValue(true_bug_error);
635 _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
638 _true_track_node->setDoubleValue(0.0);
639 _magnetic_track_node->setDoubleValue(0.0);
640 _speed_node->setDoubleValue(0.0);
644 _last_longitude_deg = longitude_deg;
645 _last_latitude_deg = latitude_deg;
646 _last_altitude_m = altitude_m;