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 <Airports/simple.hxx>
17 #include <Main/fg_init.hxx>
18 #include <Main/globals.hxx>
19 #include <Main/fg_props.hxx>
20 #include <Main/util.hxx>
21 #include <Navaids/fixlist.hxx>
22 #include <Navaids/navlist.hxx>
31 _last_longitude_deg(0),
32 _last_latitude_deg(0),
45 _longitude_node = fgGetNode("/position/longitude-deg", true);
46 _latitude_node = fgGetNode("/position/latitude-deg", true);
47 _altitude_node = fgGetNode("/position/altitude-ft", true);
48 _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true);
49 _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true);
50 _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true);
52 fgGetNode("/instrumentation/gps/wp/wp[0]/longitude-deg", true);
54 fgGetNode("/instrumentation/gps/wp/wp[0]/latitude-deg", true);
56 fgGetNode("/instrumentation/gps/wp/wp[0]/altitude-ft", true);
58 fgGetNode("/instrumentation/gps/wp/wp[0]/ID", true);
60 fgGetNode("/instrumentation/gps/wp/wp[0]/name", true);
62 fgGetNode("/instrumentation/gps/wp/wp[0]/desired-course-deg", true);
63 _wp0_waypoint_type_node =
64 fgGetNode("/instrumentation/gps/wp/wp[0]/waypoint-type", true);
66 fgGetNode("/instrumentation/gps/wp/wp[1]/longitude-deg", true);
68 fgGetNode("/instrumentation/gps/wp/wp[1]/latitude-deg", true);
70 fgGetNode("/instrumentation/gps/wp/wp[1]/altitude-ft", true);
72 fgGetNode("/instrumentation/gps/wp/wp[1]/ID", true);
74 fgGetNode("/instrumentation/gps/wp/wp[1]/name", true);
76 fgGetNode("/instrumentation/gps/wp/wp[1]/desired-course-deg", true);
77 _wp1_waypoint_type_node =
78 fgGetNode("/instrumentation/gps/wp/wp[1]/waypoint-type", true);
79 _get_nearest_airport_node =
80 fgGetNode("/instrumentation/gps/wp/wp[1]/get-nearest-airport", true);
82 fgGetNode("/instrumentation/gps/tracking-bug", true);
84 _raim_node = fgGetNode("/instrumentation/gps/raim", true);
85 _indicated_longitude_node =
86 fgGetNode("/instrumentation/gps/indicated-longitude-deg", true);
87 _indicated_latitude_node =
88 fgGetNode("/instrumentation/gps/indicated-latitude-deg", true);
89 _indicated_altitude_node =
90 fgGetNode("/instrumentation/gps/indicated-altitude-ft", true);
91 _indicated_vertical_speed_node =
92 fgGetNode("/instrumentation/gps/indicated-vertical-speed", true);
94 fgGetNode("/instrumentation/gps/indicated-track-true-deg", true);
95 _magnetic_track_node =
96 fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true);
98 fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true);
100 fgGetNode("/instrumentation/gps/wp/wp[0]/distance-nm", true);
102 fgGetNode("/instrumentation/gps/wp/wp[0]/TTW",true);
104 fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-true-deg", true);
105 _wp0_mag_bearing_node =
106 fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-mag-deg", true);
107 _wp0_course_deviation_node =
108 fgGetNode("/instrumentation/gps/wp/wp[0]/course-deviation-deg", true);
109 _wp0_course_error_nm_node =
110 fgGetNode("/instrumentation/gps/wp/wp[0]/course-error-nm", true);
112 fgGetNode("/instrumentation/gps/wp/wp[0]/to-flag", true);
114 fgGetNode("/instrumentation/gps/wp/wp[1]/distance-nm", true);
116 fgGetNode("/instrumentation/gps/wp/wp[1]/TTW",true);
118 fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-true-deg", true);
119 _wp1_mag_bearing_node =
120 fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-mag-deg", true);
121 _wp1_course_deviation_node =
122 fgGetNode("/instrumentation/gps/wp/wp[1]/course-deviation-deg", true);
123 _wp1_course_error_nm_node =
124 fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
126 fgGetNode("/instrumentation/gps/wp/wp[1]/to-flag", true);
128 fgGetNode("/instrumentation/gps/odometer", true);
129 _trip_odometer_node =
130 fgGetNode("/instrumentation/gps/trip-odometer", true);
131 _true_bug_error_node =
132 fgGetNode("/instrumentation/gps/true-bug-error-deg", true);
133 _magnetic_bug_error_node =
134 fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true);
135 _true_wp0_bearing_error_node =
136 fgGetNode("/instrumentation/gps/wp/wp[0]/true-bearing-error-deg", true);
137 _magnetic_wp0_bearing_error_node =
138 fgGetNode("/instrumentation/gps/wp/wp[0]/magnetic-bearing-error-deg", true);
139 _true_wp1_bearing_error_node =
140 fgGetNode("/instrumentation/gps/wp/wp[1]/true-bearing-error-deg", true);
141 _magnetic_wp1_bearing_error_node =
142 fgGetNode("/instrumentation/gps/wp/wp[1]/magnetic-bearing-error-deg", true);
144 fgGetNode("/instrumentation/gps/wp/leg-distance-nm", true);
146 fgGetNode("/instrumentation/gps/wp/leg-true-course-deg", true);
147 _leg_magnetic_course_node =
148 fgGetNode("/instrumentation/gps/wp/leg-mag-course-deg", true);
149 _alt_dist_ratio_node =
150 fgGetNode("/instrumentation/gps/wp/alt-dist-ratio", true);
151 _leg_course_deviation_node =
152 fgGetNode("/instrumentation/gps/wp/leg-course-deviation-deg", true);
153 _leg_course_error_nm_node =
154 fgGetNode("/instrumentation/gps/wp/leg-course-error-nm", true);
156 fgGetNode("/instrumentation/gps/wp/leg-to-flag", true);
157 _alt_deviation_node =
158 fgGetNode("/instrumentation/gps/wp/alt-deviation-ft", true);
162 GPS::update (double delta_time_sec)
164 // If it's off, don't bother.
165 if (!_serviceable_node->getBoolValue() ||
166 !_electrical_node->getBoolValue()) {
168 _last_longitude_deg = 0;
169 _last_latitude_deg = 0;
170 _last_altitude_m = 0;
172 _raim_node->setDoubleValue(false);
173 _indicated_longitude_node->setDoubleValue(0);
174 _indicated_latitude_node->setDoubleValue(0);
175 _indicated_altitude_node->setDoubleValue(0);
176 _indicated_vertical_speed_node->setDoubleValue(0);
177 _true_track_node->setDoubleValue(0);
178 _magnetic_track_node->setDoubleValue(0);
179 _speed_node->setDoubleValue(0);
180 _wp1_distance_node->setDoubleValue(0);
181 _wp1_bearing_node->setDoubleValue(0);
182 _wp1_longitude_node->setDoubleValue(0);
183 _wp1_latitude_node->setDoubleValue(0);
184 _wp1_course_node->setDoubleValue(0);
185 _odometer_node->setDoubleValue(0);
186 _trip_odometer_node->setDoubleValue(0);
187 _tracking_bug_node->setDoubleValue(0);
188 _true_bug_error_node->setDoubleValue(0);
189 _magnetic_bug_error_node->setDoubleValue(0);
190 _true_wp1_bearing_error_node->setDoubleValue(0);
191 _magnetic_wp1_bearing_error_node->setDoubleValue(0);
195 // Get the aircraft position
196 // TODO: Add noise and other errors.
197 double longitude_deg = _longitude_node->getDoubleValue();
198 double latitude_deg = _latitude_node->getDoubleValue();
199 double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER;
200 double magvar_deg = _magvar_node->getDoubleValue();
204 // Bias and random error
205 double random_factor = sg_random();
206 double random_error = 1.4;
207 double error_radius = 5.1;
208 double bias_max_radius = 5.1;
209 double random_max_radius = 1.4;
211 bias_length += (random_factor-0.5) * 1.0e-3;
212 if (bias_length <= 0.0) bias_length = 0.0;
213 else if (bias_length >= bias_max_radius) bias_length = bias_max_radius;
214 bias_angle += (random_factor-0.5) * 1.0e-3;
215 if (bias_angle <= 0.0) bias_angle = 0.0;
216 else if (bias_angle >= 360.0) bias_angle = 360.0;
218 double random_length = random_factor * random_max_radius;
219 double random_angle = random_factor * 360.0;
221 double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0);
222 double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0);
223 double random_x = random_length * cos(random_angle * SG_PI / 180.0);
224 double random_y = random_length * sin(random_angle * SG_PI / 180.0);
225 double error_x = bias_x + random_x;
226 double error_y = bias_y + random_y;
227 double error_length = sqrt(error_x*error_x + error_y*error_y);
228 double error_angle = atan(error_y / error_x) * 180.0 / SG_PI;
233 geo_direct_wgs_84 ( altitude_m, latitude_deg,
234 longitude_deg, error_angle,
235 error_length, &lat2, &lon2,
237 //cout << lat2 << " " << lon2 << endl;
238 printf("%f %f \n", bias_length, bias_angle);
239 printf("%3.7f %3.7f \n", lat2, lon2);
240 printf("%f %f \n", error_length, error_angle);
246 double speed_kt, vertical_speed_mpm;
248 _raim_node->setBoolValue(true);
249 _indicated_longitude_node->setDoubleValue(longitude_deg);
250 _indicated_latitude_node->setDoubleValue(latitude_deg);
251 _indicated_altitude_node->setDoubleValue(altitude_m * SG_METER_TO_FEET);
254 double track1_deg, track2_deg, distance_m, odometer, mag_track_bearing;
255 geo_inverse_wgs_84(altitude_m,
256 _last_latitude_deg, _last_longitude_deg,
257 latitude_deg, longitude_deg,
258 &track1_deg, &track2_deg, &distance_m);
259 speed_kt = ((distance_m * SG_METER_TO_NM) *
260 ((1 / delta_time_sec) * 3600.0));
261 vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 /
263 _indicated_vertical_speed_node->setDoubleValue
264 (vertical_speed_mpm * SG_METER_TO_FEET);
265 _true_track_node->setDoubleValue(track1_deg);
266 mag_track_bearing = degrange360(track1_deg - magvar_deg);
267 _magnetic_track_node->setDoubleValue(mag_track_bearing);
268 speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0);
269 _last_speed_kts = speed_kt;
270 _speed_node->setDoubleValue(speed_kt);
272 odometer = _odometer_node->getDoubleValue();
273 _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
274 odometer = _trip_odometer_node->getDoubleValue();
275 _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
277 // Get waypoint 0 position
278 double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue();
279 double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue();
280 double wp0_altitude_m = _wp0_altitude_node->getDoubleValue()
282 double wp0_course_deg = _wp0_course_node->getDoubleValue();
283 double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg,
284 wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg;
285 string wp0_ID = _wp0_ID_node->getStringValue();
287 // Get waypoint 1 position
288 double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue();
289 double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue();
290 double wp1_altitude_m = _wp1_altitude_node->getDoubleValue()
292 double wp1_course_deg = _wp1_course_node->getDoubleValue();
293 double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg,
294 wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg;
295 string wp1_ID = _wp1_ID_node->getStringValue();
297 // If the get-nearest-airport-node is true.
298 // Get the nearest airport, and set it as waypoint 1.
299 if (_get_nearest_airport_node->getBoolValue()) {
301 //cout << "Airport found" << endl;
302 a = globals->get_airports()->search(longitude_deg, latitude_deg, false);
303 _wp1_ID_node->setStringValue(a.id.c_str());
304 wp1_longitude_deg = a.longitude;
305 wp1_latitude_deg = a.latitude;
306 _wp1_name_node->setStringValue(a.name.c_str());
307 _get_nearest_airport_node->setBoolValue(false);
308 _last_wp1_ID = wp1_ID = a.id.c_str();
311 // If the waypoint 0 ID has changed, try to find the new ID
312 // in the airport-, fix-, nav-database.
313 if ( !(_last_wp0_ID == wp0_ID) ) {
314 string waypont_type =
315 _wp0_waypoint_type_node->getStringValue();
316 if (waypont_type == "airport") {
318 a = globals->get_airports()->search( wp0_ID );
319 if ( a.id == wp0_ID ) {
320 //cout << "Airport found" << endl;
321 wp0_longitude_deg = a.longitude;
322 wp0_latitude_deg = a.latitude;
323 _wp0_name_node->setStringValue(a.name.c_str());
326 else if (waypont_type == "nav") {
328 if ( (n = current_navlist->findByIdent(wp0_ID.c_str(),
330 latitude_deg)) != NULL) {
331 //cout << "Nav found" << endl;
332 wp0_longitude_deg = n->get_lon();
333 wp0_latitude_deg = n->get_lat();
334 _wp0_name_node->setStringValue(n->get_name().c_str());
337 else if (waypont_type == "fix") {
339 if ( current_fixlist->query(wp0_ID, &f) ) {
340 //cout << "Fix found" << endl;
341 wp0_longitude_deg = f.get_lon();
342 wp0_latitude_deg = f.get_lat();
343 _wp0_name_node->setStringValue(wp0_ID.c_str());
346 _last_wp0_ID = wp0_ID;
349 // If the waypoint 1 ID has changed, try to find the new ID
350 // in the airport-, fix-, nav-database.
351 if ( !(_last_wp1_ID == wp1_ID) ) {
352 string waypont_type =
353 _wp1_waypoint_type_node->getStringValue();
354 if (waypont_type == "airport") {
356 a = globals->get_airports()->search( wp1_ID );
357 if ( a.id == wp1_ID ) {
358 //cout << "Airport found" << endl;
359 wp1_longitude_deg = a.longitude;
360 wp1_latitude_deg = a.latitude;
361 _wp1_name_node->setStringValue(a.name.c_str());
364 else if (waypont_type == "nav") {
366 if ( (n = current_navlist->findByIdent(wp1_ID.c_str(),
368 latitude_deg)) != NULL) {
369 //cout << "Nav found" << endl;
370 wp1_longitude_deg = n->get_lon();
371 wp1_latitude_deg = n->get_lat();
372 _wp1_name_node->setStringValue(n->get_name().c_str());
375 else if (waypont_type == "fix") {
377 if ( current_fixlist->query(wp1_ID, &f) ) {
378 //cout << "Fix found" << endl;
379 wp1_longitude_deg = f.get_lon();
380 wp1_latitude_deg = f.get_lat();
381 _wp1_name_node->setStringValue(wp1_ID.c_str());
384 _last_wp1_ID = wp1_ID;
389 // If any of the two waypoints have changed
390 // we need to calculate a new course between them,
391 // and values for vertical navigation.
392 if ( wp0_longitude_deg != _wp0_longitude_deg ||
393 wp0_latitude_deg != _wp0_latitude_deg ||
394 wp0_altitude_m != _wp0_altitude_m ||
395 wp1_longitude_deg != _wp1_longitude_deg ||
396 wp1_latitude_deg != _wp1_latitude_deg ||
397 wp1_altitude_m != _wp1_altitude_m )
399 // Update the global variables
400 _wp0_longitude_deg = wp0_longitude_deg;
401 _wp0_latitude_deg = wp0_latitude_deg;
402 _wp0_altitude_m = wp0_altitude_m;
403 _wp1_longitude_deg = wp1_longitude_deg;
404 _wp1_latitude_deg = wp1_latitude_deg;
405 _wp1_altitude_m = wp1_altitude_m;
407 // Get the course and distance from wp0 to wp1
408 SGWayPoint wp0(wp0_longitude_deg,
409 wp0_latitude_deg, wp0_altitude_m);
410 SGWayPoint wp1(wp1_longitude_deg,
411 wp1_latitude_deg, wp1_altitude_m);
413 wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m);
415 // Get the altitude / distance ratio
416 if ( distance_m > 0.0 ) {
417 double alt_difference_m = wp0_altitude_m - wp1_altitude_m;
418 _alt_dist_ratio = alt_difference_m / _distance_m;
421 _leg_distance_node->setDoubleValue(_distance_m);
422 _leg_course_node->setDoubleValue(_course_deg);
423 _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
427 // Find the bearing and distance to waypoint 0.
428 SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m);
429 wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
430 &wp0_bearing_deg, &wp0_distance);
431 _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
432 _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
433 _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
434 _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
435 double wp0_mag_bearing_deg = degrange360(wp0_bearing_deg - magvar_deg);
436 _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
437 wp0_bearing_error_deg = degrange180(track1_deg - wp0_bearing_deg);
438 _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
440 // Estimate time to waypoint 0.
441 // The estimation does not take track into consideration,
442 // so if you are going away from the waypoint the TTW will
443 // increase. Makes most sense when travelling directly towards
445 if (speed_kt > 0.0 && wp0_distance > 0.0) {
446 wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
451 unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5);
452 if (wp0_TTW_seconds < 356400) { // That's 99 hours
453 unsigned int wp0_TTW_minutes = 0;
454 unsigned int wp0_TTW_hours = 0;
456 while (wp0_TTW_seconds >= 3600) {
457 wp0_TTW_seconds -= 3600;
460 while (wp0_TTW_seconds >= 60) {
461 wp0_TTW_seconds -= 60;
464 snprintf(wp0_TTW_str, 8, "%02d:%02d:%02d",
465 wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
466 _wp0_ttw_node->setStringValue(wp0_TTW_str);
469 _wp0_ttw_node->setStringValue("--:--:--");
471 // Course deviation is the diffenrence between the bearing
473 wp0_course_deviation_deg = wp0_bearing_deg -
475 wp0_course_deviation_deg = degrange180(wp0_course_deviation_deg);
477 // If the course deviation is less than 90 degrees to either side,
478 // our desired course is towards the waypoint.
479 // It does not matter if we are actually moving
480 // towards or from the waypoint.
481 if (fabs(wp0_course_deviation_deg) < 90.0) {
482 _wp0_to_flag_node->setBoolValue(true); }
483 // If it's more than 90 degrees the desired
484 // course is from the waypoint.
485 else if (fabs(wp0_course_deviation_deg) > 90.0) {
486 _wp0_to_flag_node->setBoolValue(false);
487 // When the course is away from the waypoint,
488 // it makes sense to change the sign of the deviation.
489 wp0_course_deviation_deg *= -1.0;
490 wp0_course_deviation_deg =
491 degrange(wp0_course_deviation_deg, -90.0, 90.0);
494 _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg);
496 // Cross track error.
497 wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0)
499 _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m
504 // Find the bearing and distance to waypoint 1.
505 SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m);
506 wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m,
507 &wp1_bearing_deg, &wp1_distance);
508 _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
509 _wp1_latitude_node->setDoubleValue(wp1_latitude_deg);
510 _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
511 _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
512 double wp1_mag_bearing_deg = degrange360(wp1_bearing_deg - magvar_deg);
513 _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
514 wp1_bearing_error_deg = degrange180(track1_deg - wp1_bearing_deg);
515 _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg);
517 // Estimate time to waypoint 1.
518 // The estimation does not take track into consideration,
519 // so if you are going away from the waypoint the TTW will
520 // increase. Makes most sense when travelling directly towards
522 if (speed_kt > 0.0 && wp1_distance > 0.0) {
523 wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600);
528 unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5);
529 if (wp1_TTW_seconds < 356400) { // That's 99 hours
530 unsigned int wp1_TTW_minutes = 0;
531 unsigned int wp1_TTW_hours = 0;
533 while (wp1_TTW_seconds >= 3600) {
534 wp1_TTW_seconds -= 3600;
537 while (wp1_TTW_seconds >= 60) {
538 wp1_TTW_seconds -= 60;
541 snprintf(wp1_TTW_str, 8, "%02d:%02d:%02d",
542 wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds);
543 _wp1_ttw_node->setStringValue(wp1_TTW_str);
546 _wp1_ttw_node->setStringValue("--:--:--");
548 // Course deviation is the diffenrence between the bearing
550 wp1_course_deviation_deg = wp1_bearing_deg -
552 wp1_course_deviation_deg = degrange180(wp1_course_deviation_deg);
554 // If the course deviation is less than 90 degrees to either side,
555 // our desired course is towards the waypoint.
556 // It does not matter if we are actually moving
557 // towards or from the waypoint.
558 if (fabs(wp1_course_deviation_deg) < 90.0) {
559 _wp1_to_flag_node->setBoolValue(true); }
560 // If it's more than 90 degrees the desired
561 // course is from the waypoint.
562 else if (fabs(wp1_course_deviation_deg) > 90.0) {
563 _wp1_to_flag_node->setBoolValue(false);
564 // When the course is away from the waypoint,
565 // it makes sense to change the sign of the deviation.
566 wp1_course_deviation_deg *= -1.0;
567 wp1_course_deviation_deg =
568 degrange(wp1_course_deviation_deg, -90.0, 90.0);
571 _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
573 // Cross track error.
574 wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0)
576 _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m
580 // Leg course deviation is the diffenrence between the bearing
582 double course_deviation_deg = wp1_bearing_deg - _course_deg;
583 course_deviation_deg = degrange180(course_deviation_deg);
585 // If the course deviation is less than 90 degrees to either side,
586 // our desired course is towards the waypoint.
587 // It does not matter if we are actually moving
588 // towards or from the waypoint.
589 if (fabs(course_deviation_deg) < 90.0) {
590 _leg_to_flag_node->setBoolValue(true); }
591 // If it's more than 90 degrees the desired
592 // course is from the waypoint.
593 else if (fabs(course_deviation_deg) > 90.0) {
594 _leg_to_flag_node->setBoolValue(false);
595 // When the course is away from the waypoint,
596 // it makes sense to change the sign of the deviation.
597 course_deviation_deg *= -1.0;
598 course_deviation_deg =
599 degrange(course_deviation_deg, -90.0, 90.0);
602 _leg_course_deviation_node->setDoubleValue(course_deviation_deg);
604 // Cross track error.
605 double course_error_m = sin(course_deviation_deg * SG_PI / 180.0)
607 _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM);
609 // Altitude deviation
610 double desired_altitude_m = wp1_altitude_m
611 + wp1_distance * _alt_dist_ratio;
612 double altitude_deviation_m = altitude_m - desired_altitude_m;
613 _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET);
618 double tracking_bug = _tracking_bug_node->getDoubleValue();
619 double true_bug_error = tracking_bug - track1_deg;
620 double magnetic_bug_error = tracking_bug - mag_track_bearing;
622 // Get the errors into the (-180,180) range.
623 true_bug_error = degrange180(true_bug_error);
624 magnetic_bug_error = degrange180(magnetic_bug_error);
626 _true_bug_error_node->setDoubleValue(true_bug_error);
627 _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
630 _true_track_node->setDoubleValue(0.0);
631 _magnetic_track_node->setDoubleValue(0.0);
632 _speed_node->setDoubleValue(0.0);
636 _last_longitude_deg = longitude_deg;
637 _last_latitude_deg = latitude_deg;
638 _last_altitude_m = altitude_m;
641 double GPS::degrange360 (double deg)
645 while (deg > 360.0) {
651 double GPS::degrange180 (double deg)
661 double GPS::degrange (double deg, double min, double max)
663 double span = max - min;