From c7d6fa216434f95aa0baec5a4f41e1c442d2ee7d Mon Sep 17 00:00:00 2001 From: ehofman Date: Sat, 1 May 2004 09:40:09 +0000 Subject: [PATCH] Roy Vegard Ovesen: I've added a vertical navigation capability to the GPS module. One can input two waypoints, wp[0] and wp[1], with altitude. If the altitudes differ, then the altitude deviation from a "straigth" line from wp[0] to wp[1] is calculated. The true course and course deviation from wp[0] to wp[1] is also calculated. All this can be found in the wp subdir where one also finds the wp[0] and wp[1] subdirs. All this has to be done through the property browser. Maybe I should make a gui window for the GPS! --- src/Instrumentation/gps.cxx | 623 +++++++++++++++++++++++++++--------- src/Instrumentation/gps.hxx | 85 +++-- 2 files changed, 539 insertions(+), 169 deletions(-) diff --git a/src/Instrumentation/gps.cxx b/src/Instrumentation/gps.cxx index 2322fecb8..8096bcd00 100644 --- a/src/Instrumentation/gps.cxx +++ b/src/Instrumentation/gps.cxx @@ -8,11 +8,12 @@ #endif #include - #include -#include +#include +#include #include + #include
#include
#include
@@ -47,20 +48,36 @@ GPS::init () _magvar_node = fgGetNode("/environment/magnetic-variation-deg", true); _serviceable_node = fgGetNode("/instrumentation/gps/serviceable", true); _electrical_node = fgGetNode("/systems/electrical/outputs/gps", true); - _wp_longitude_node = - fgGetNode("/instrumentation/gps/wp-longitude-deg", true); - _wp_latitude_node = - fgGetNode("/instrumentation/gps/wp-latitude-deg", true); - _wp_ID_node = - fgGetNode("/instrumentation/gps/wp-ID", true); - _wp_name_node = - fgGetNode("/instrumentation/gps/wp-name", true); - _wp_course_node = - fgGetNode("/instrumentation/gps/desired-course-deg", true); + _wp0_longitude_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/longitude-deg", true); + _wp0_latitude_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/latitude-deg", true); + _wp0_altitude_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/altitude-ft", true); + _wp0_ID_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/ID", true); + _wp0_name_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/name", true); + _wp0_course_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/desired-course-deg", true); + _wp0_waypoint_type_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/waypoint-type", true); + _wp1_longitude_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/longitude-deg", true); + _wp1_latitude_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/latitude-deg", true); + _wp1_altitude_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/altitude-ft", true); + _wp1_ID_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/ID", true); + _wp1_name_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/name", true); + _wp1_course_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/desired-course-deg", true); + _wp1_waypoint_type_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/waypoint-type", true); _get_nearest_airport_node = - fgGetNode("/instrumentation/gps/get-nearest-airport", true); - _waypoint_type_node = - fgGetNode("/instrumentation/gps/waypoint-type", true); + fgGetNode("/instrumentation/gps/wp/wp[1]/get-nearest-airport", true); _tracking_bug_node = fgGetNode("/instrumentation/gps/tracking-bug", true); @@ -71,26 +88,42 @@ GPS::init () fgGetNode("/instrumentation/gps/indicated-latitude-deg", true); _indicated_altitude_node = fgGetNode("/instrumentation/gps/indicated-altitude-ft", true); + _indicated_vertical_speed_node = + fgGetNode("/instrumentation/gps/indicated-vertical-speed", true); _true_track_node = fgGetNode("/instrumentation/gps/indicated-track-true-deg", true); _magnetic_track_node = fgGetNode("/instrumentation/gps/indicated-track-magnetic-deg", true); _speed_node = fgGetNode("/instrumentation/gps/indicated-ground-speed-kt", true); - _wp_distance_node = - fgGetNode("/instrumentation/gps/wp-distance-nm", true); - _wp_ttw_node = - fgGetNode("/instrumentation/gps/TTW",true); - _wp_bearing_node = - fgGetNode("/instrumentation/gps/wp-bearing-deg", true); - _wp_mag_bearing_node = - fgGetNode("/instrumentation/gps/wp-bearing-mag-deg", true); - _wp_course_deviation_node = - fgGetNode("/instrumentation/gps/course-deviation-deg", true); - _wp_course_error_nm_node = - fgGetNode("/instrumentation/gps/course-error-nm", true); - _wp_to_flag_node = - fgGetNode("/instrumentation/gps/to-flag", true); + _wp0_distance_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/distance-nm", true); + _wp0_ttw_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/TTW",true); + _wp0_bearing_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-true-deg", true); + _wp0_mag_bearing_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/bearing-mag-deg", true); + _wp0_course_deviation_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/course-deviation-deg", true); + _wp0_course_error_nm_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/course-error-nm", true); + _wp0_to_flag_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/to-flag", true); + _wp1_distance_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/distance-nm", true); + _wp1_ttw_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/TTW",true); + _wp1_bearing_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-true-deg", true); + _wp1_mag_bearing_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/bearing-mag-deg", true); + _wp1_course_deviation_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/course-deviation-deg", true); + _wp1_course_error_nm_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true); + _wp1_to_flag_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/to-flag", true); _odometer_node = fgGetNode("/instrumentation/gps/odometer", true); _trip_odometer_node = @@ -99,7 +132,30 @@ GPS::init () fgGetNode("/instrumentation/gps/true-bug-error-deg", true); _magnetic_bug_error_node = fgGetNode("/instrumentation/gps/magnetic-bug-error-deg", true); - + _true_wp0_bearing_error_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/true-bearing-error-deg", true); + _magnetic_wp0_bearing_error_node = + fgGetNode("/instrumentation/gps/wp/wp[0]/magnetic-bearing-error-deg", true); + _true_wp1_bearing_error_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/true-bearing-error-deg", true); + _magnetic_wp1_bearing_error_node = + fgGetNode("/instrumentation/gps/wp/wp[1]/magnetic-bearing-error-deg", true); + _leg_distance_node = + fgGetNode("/instrumentation/gps/wp/leg-distance-nm", true); + _leg_course_node = + fgGetNode("/instrumentation/gps/wp/leg-true-course-deg", true); + _leg_magnetic_course_node = + fgGetNode("/instrumentation/gps/wp/leg-mag-course-deg", true); + _alt_dist_ratio_node = + fgGetNode("/instrumentation/gps/wp/alt-dist-ratio", true); + _leg_course_deviation_node = + fgGetNode("/instrumentation/gps/wp/leg-course-deviation-deg", true); + _leg_course_error_nm_node = + fgGetNode("/instrumentation/gps/wp/leg-course-error-nm", true); + _leg_to_flag_node = + fgGetNode("/instrumentation/gps/wp/leg-to-flag", true); + _alt_deviation_node = + fgGetNode("/instrumentation/gps/wp/alt-deviation-ft", true); } void @@ -117,19 +173,22 @@ GPS::update (double delta_time_sec) _indicated_longitude_node->setDoubleValue(0); _indicated_latitude_node->setDoubleValue(0); _indicated_altitude_node->setDoubleValue(0); + _indicated_vertical_speed_node->setDoubleValue(0); _true_track_node->setDoubleValue(0); _magnetic_track_node->setDoubleValue(0); _speed_node->setDoubleValue(0); - _wp_distance_node->setDoubleValue(0); - _wp_bearing_node->setDoubleValue(0); - _wp_longitude_node->setDoubleValue(0); - _wp_latitude_node->setDoubleValue(0); - _wp_course_node->setDoubleValue(0); + _wp1_distance_node->setDoubleValue(0); + _wp1_bearing_node->setDoubleValue(0); + _wp1_longitude_node->setDoubleValue(0); + _wp1_latitude_node->setDoubleValue(0); + _wp1_course_node->setDoubleValue(0); _odometer_node->setDoubleValue(0); _trip_odometer_node->setDoubleValue(0); _tracking_bug_node->setDoubleValue(0); _true_bug_error_node->setDoubleValue(0); _magnetic_bug_error_node->setDoubleValue(0); + _true_wp1_bearing_error_node->setDoubleValue(0); + _magnetic_wp1_bearing_error_node->setDoubleValue(0); return; } @@ -140,7 +199,51 @@ GPS::update (double delta_time_sec) double altitude_m = _altitude_node->getDoubleValue() * SG_FEET_TO_METER; double magvar_deg = _magvar_node->getDoubleValue(); - double speed_kt; +/* + + // Bias and random error + double random_factor = sg_random(); + double random_error = 1.4; + double error_radius = 5.1; + double bias_max_radius = 5.1; + double random_max_radius = 1.4; + + bias_length += (random_factor-0.5) * 1.0e-3; + if (bias_length <= 0.0) bias_length = 0.0; + else if (bias_length >= bias_max_radius) bias_length = bias_max_radius; + bias_angle += (random_factor-0.5) * 1.0e-3; + if (bias_angle <= 0.0) bias_angle = 0.0; + else if (bias_angle >= 360.0) bias_angle = 360.0; + + double random_length = random_factor * random_max_radius; + double random_angle = random_factor * 360.0; + + double bias_x = bias_length * cos(bias_angle * SG_PI / 180.0); + double bias_y = bias_length * sin(bias_angle * SG_PI / 180.0); + double random_x = random_length * cos(random_angle * SG_PI / 180.0); + double random_y = random_length * sin(random_angle * SG_PI / 180.0); + double error_x = bias_x + random_x; + double error_y = bias_y + random_y; + double error_length = sqrt(error_x*error_x + error_y*error_y); + double error_angle = atan(error_y / error_x) * 180.0 / SG_PI; + + double lat2; + double lon2; + double az2; + geo_direct_wgs_84 ( altitude_m, latitude_deg, + longitude_deg, error_angle, + error_length, &lat2, &lon2, + &az2 ); + //cout << lat2 << " " << lon2 << endl; + printf("%f %f \n", bias_length, bias_angle); + printf("%3.7f %3.7f \n", lat2, lon2); + printf("%f %f \n", error_length, error_angle); + +*/ + + + + double speed_kt, vertical_speed_mpm; _raim_node->setBoolValue(true); _indicated_longitude_node->setDoubleValue(longitude_deg); @@ -155,8 +258,12 @@ GPS::update (double delta_time_sec) &track1_deg, &track2_deg, &distance_m); speed_kt = ((distance_m * SG_METER_TO_NM) * ((1 / delta_time_sec) * 3600.0)); + vertical_speed_mpm = ((altitude_m - _last_altitude_m) * 60 / + delta_time_sec); + _indicated_vertical_speed_node->setDoubleValue + (vertical_speed_mpm * SG_METER_TO_FEET); _true_track_node->setDoubleValue(track1_deg); - mag_track_bearing = deg360(track1_deg - magvar_deg); + mag_track_bearing = degrange360(track1_deg - magvar_deg); _magnetic_track_node->setDoubleValue(mag_track_bearing); speed_kt = fgGetLowPass(_last_speed_kts, speed_kt, delta_time_sec/20.0); _last_speed_kts = speed_kt; @@ -167,156 +274,354 @@ GPS::update (double delta_time_sec) odometer = _trip_odometer_node->getDoubleValue(); _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM); - // Get waypoint position - double wp_longitude_deg = _wp_longitude_node->getDoubleValue(); - double wp_latitude_deg = _wp_latitude_node->getDoubleValue(); - double wp_course_deg = - _wp_course_node->getDoubleValue(); - double wp_distance, wp_bearing_deg, wp_course_deviation_deg, - wp_course_error_m, wp_TTW; - string wp_ID = _wp_ID_node->getStringValue(); - + // Get waypoint 0 position + double wp0_longitude_deg = _wp0_longitude_node->getDoubleValue(); + double wp0_latitude_deg = _wp0_latitude_node->getDoubleValue(); + double wp0_altitude_m = _wp0_altitude_node->getDoubleValue() + * SG_FEET_TO_METER; + double wp0_course_deg = _wp0_course_node->getDoubleValue(); + double wp0_distance, wp0_bearing_deg, wp0_course_deviation_deg, + wp0_course_error_m, wp0_TTW, wp0_bearing_error_deg; + string wp0_ID = _wp0_ID_node->getStringValue(); + + // Get waypoint 1 position + double wp1_longitude_deg = _wp1_longitude_node->getDoubleValue(); + double wp1_latitude_deg = _wp1_latitude_node->getDoubleValue(); + double wp1_altitude_m = _wp1_altitude_node->getDoubleValue() + * SG_FEET_TO_METER; + double wp1_course_deg = _wp1_course_node->getDoubleValue(); + double wp1_distance, wp1_bearing_deg, wp1_course_deviation_deg, + wp1_course_error_m, wp1_TTW, wp1_bearing_error_deg; + string wp1_ID = _wp1_ID_node->getStringValue(); + // If the get-nearest-airport-node is true. - // Get the nearest airport, and set it as waypoint. + // Get the nearest airport, and set it as waypoint 1. if (_get_nearest_airport_node->getBoolValue()) { - FGAirport a; - cout << "Airport found" << endl; - a = globals->get_airports()->search(longitude_deg, latitude_deg, false); - _wp_ID_node->setStringValue(a.id.c_str()); - wp_longitude_deg = a.longitude; - wp_latitude_deg = a.latitude; - _wp_name_node->setStringValue(a.name.c_str()); - _get_nearest_airport_node->setBoolValue(false); - _last_wp_ID = wp_ID = a.id.c_str(); + FGAirport a; + //cout << "Airport found" << endl; + a = globals->get_airports()->search(longitude_deg, latitude_deg, false); + _wp1_ID_node->setStringValue(a.id.c_str()); + wp1_longitude_deg = a.longitude; + wp1_latitude_deg = a.latitude; + _wp1_name_node->setStringValue(a.name.c_str()); + _get_nearest_airport_node->setBoolValue(false); + _last_wp1_ID = wp1_ID = a.id.c_str(); } - - // If the waypoint ID has changed, try to find the new ID + + // If the waypoint 0 ID has changed, try to find the new ID // in the airport-, fix-, nav-database. - if ( !(_last_wp_ID == wp_ID) ) { - string waypont_type = - _waypoint_type_node->getStringValue(); - if (waypont_type == "airport") { - FGAirport a; - a = globals->get_airports()->search( wp_ID ); - if ( a.id == wp_ID ) { - cout << "Airport found" << endl; - wp_longitude_deg = a.longitude; - wp_latitude_deg = a.latitude; - _wp_name_node->setStringValue(a.name.c_str()); + if ( !(_last_wp0_ID == wp0_ID) ) { + string waypont_type = + _wp0_waypoint_type_node->getStringValue(); + if (waypont_type == "airport") { + FGAirport a; + a = globals->get_airports()->search( wp0_ID ); + if ( a.id == wp0_ID ) { + //cout << "Airport found" << endl; + wp0_longitude_deg = a.longitude; + wp0_latitude_deg = a.latitude; + _wp0_name_node->setStringValue(a.name.c_str()); + } } - } - else if (waypont_type == "nav") { - FGNav * n; - if ( (n = current_navlist->findByIdent(wp_ID.c_str(), - longitude_deg, - latitude_deg)) != NULL) { - cout << "Nav found" << endl; - wp_longitude_deg = n->get_lon(); - wp_latitude_deg = n->get_lat(); - _wp_name_node->setStringValue(n->get_name().c_str()); + else if (waypont_type == "nav") { + FGNav * n; + if ( (n = current_navlist->findByIdent(wp0_ID.c_str(), + longitude_deg, + latitude_deg)) != NULL) { + //cout << "Nav found" << endl; + wp0_longitude_deg = n->get_lon(); + wp0_latitude_deg = n->get_lat(); + _wp0_name_node->setStringValue(n->get_name().c_str()); + } } - } - else if (waypont_type == "fix") { - FGFix f; - if ( current_fixlist->query(wp_ID, &f) ) { - cout << "Fix found" << endl; - wp_longitude_deg = f.get_lon(); - wp_latitude_deg = f.get_lat(); - _wp_name_node->setStringValue(wp_ID.c_str()); + else if (waypont_type == "fix") { + FGFix f; + if ( current_fixlist->query(wp0_ID, &f) ) { + //cout << "Fix found" << endl; + wp0_longitude_deg = f.get_lon(); + wp0_latitude_deg = f.get_lat(); + _wp0_name_node->setStringValue(wp0_ID.c_str()); + } } - } - _last_wp_ID = wp_ID; + _last_wp0_ID = wp0_ID; + } + + // If the waypoint 1 ID has changed, try to find the new ID + // in the airport-, fix-, nav-database. + if ( !(_last_wp1_ID == wp1_ID) ) { + string waypont_type = + _wp1_waypoint_type_node->getStringValue(); + if (waypont_type == "airport") { + FGAirport a; + a = globals->get_airports()->search( wp1_ID ); + if ( a.id == wp1_ID ) { + //cout << "Airport found" << endl; + wp1_longitude_deg = a.longitude; + wp1_latitude_deg = a.latitude; + _wp1_name_node->setStringValue(a.name.c_str()); + } + } + else if (waypont_type == "nav") { + FGNav * n; + if ( (n = current_navlist->findByIdent(wp1_ID.c_str(), + longitude_deg, + latitude_deg)) != NULL) { + //cout << "Nav found" << endl; + wp1_longitude_deg = n->get_lon(); + wp1_latitude_deg = n->get_lat(); + _wp1_name_node->setStringValue(n->get_name().c_str()); + } + } + else if (waypont_type == "fix") { + FGFix f; + if ( current_fixlist->query(wp1_ID, &f) ) { + //cout << "Fix found" << endl; + wp1_longitude_deg = f.get_lon(); + wp1_latitude_deg = f.get_lat(); + _wp1_name_node->setStringValue(wp1_ID.c_str()); + } + } + _last_wp1_ID = wp1_ID; + } + + + + // If any of the two waypoints have changed + // we need to calculate a new course between them, + // and values for vertical navigation. + if ( wp0_longitude_deg != _wp0_longitude_deg || + wp0_latitude_deg != _wp0_latitude_deg || + wp0_altitude_m != _wp0_altitude_m || + wp1_longitude_deg != _wp1_longitude_deg || + wp1_latitude_deg != _wp1_latitude_deg || + wp1_altitude_m != _wp1_altitude_m ) + { + // Update the global variables + _wp0_longitude_deg = wp0_longitude_deg; + _wp0_latitude_deg = wp0_latitude_deg; + _wp0_altitude_m = wp0_altitude_m; + _wp1_longitude_deg = wp1_longitude_deg; + _wp1_latitude_deg = wp1_latitude_deg; + _wp1_altitude_m = wp1_altitude_m; + + // Get the course and distance from wp0 to wp1 + SGWayPoint wp0(wp0_longitude_deg, + wp0_latitude_deg, wp0_altitude_m); + SGWayPoint wp1(wp1_longitude_deg, + wp1_latitude_deg, wp1_altitude_m); + + wp1.CourseAndDistance(wp0, &_course_deg, &_distance_m); + + // Get the altitude / distance ratio + if ( distance_m > 0.0 ) { + double alt_difference_m = wp0_altitude_m - wp1_altitude_m; + _alt_dist_ratio = alt_difference_m / _distance_m; + } + + _leg_distance_node->setDoubleValue(_distance_m); + _leg_course_node->setDoubleValue(_course_deg); + _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio); } - // Find the bearing and distance to the waypoint. - SGWayPoint wp(wp_longitude_deg, wp_latitude_deg, altitude_m); - wp.CourseAndDistance(longitude_deg, latitude_deg, altitude_m, - &wp_bearing_deg, &wp_distance); - _wp_longitude_node->setDoubleValue(wp_longitude_deg); - _wp_latitude_node->setDoubleValue(wp_latitude_deg); - _wp_distance_node->setDoubleValue(wp_distance * SG_METER_TO_NM); - _wp_bearing_node->setDoubleValue(wp_bearing_deg); - _wp_mag_bearing_node->setDoubleValue(deg360(wp_bearing_deg - magvar_deg)); + + // Find the bearing and distance to waypoint 0. + SGWayPoint wp0(wp0_longitude_deg, wp0_latitude_deg, wp0_altitude_m); + wp0.CourseAndDistance(longitude_deg, latitude_deg, altitude_m, + &wp0_bearing_deg, &wp0_distance); + _wp0_longitude_node->setDoubleValue(wp0_longitude_deg); + _wp0_latitude_node->setDoubleValue(wp0_latitude_deg); + _wp0_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM); + _wp0_bearing_node->setDoubleValue(wp0_bearing_deg); + double wp0_mag_bearing_deg = degrange360(wp0_bearing_deg - magvar_deg); + _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg); + wp0_bearing_error_deg = degrange180(track1_deg - wp0_bearing_deg); + _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg); - // Estimate time to waypoint. + // Estimate time to waypoint 0. // The estimation does not take track into consideration, // so if you are going away from the waypoint the TTW will // increase. Makes most sense when travelling directly towards // the waypoint. - if (speed_kt > 0.0 && wp_distance > 0.0) { - wp_TTW = (wp_distance * SG_METER_TO_NM) / (speed_kt / 3600); + if (speed_kt > 0.0 && wp0_distance > 0.0) { + wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600); } else { - wp_TTW = 0.0; + wp0_TTW = 0.0; } - unsigned int wp_TTW_seconds = (int) (wp_TTW + 0.5); - if (wp_TTW_seconds < 356400) { // That's 99 hours - unsigned int wp_TTW_minutes = 0; - unsigned int wp_TTW_hours = 0; - char wp_TTW_str[8]; - while (wp_TTW_seconds >= 3600) { - wp_TTW_seconds -= 3600; - wp_TTW_hours++; + unsigned int wp0_TTW_seconds = (int) (wp0_TTW + 0.5); + if (wp0_TTW_seconds < 356400) { // That's 99 hours + unsigned int wp0_TTW_minutes = 0; + unsigned int wp0_TTW_hours = 0; + char wp0_TTW_str[9]; + while (wp0_TTW_seconds >= 3600) { + wp0_TTW_seconds -= 3600; + wp0_TTW_hours++; } - while (wp_TTW_seconds >= 60) { - wp_TTW_seconds -= 60; - wp_TTW_minutes++; + while (wp0_TTW_seconds >= 60) { + wp0_TTW_seconds -= 60; + wp0_TTW_minutes++; } - sprintf(wp_TTW_str, "%02d:%02d:%02d", - wp_TTW_hours, wp_TTW_minutes, wp_TTW_seconds); - _wp_ttw_node->setStringValue(wp_TTW_str); + snprintf(wp0_TTW_str, 8, "%02d:%02d:%02d", + wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds); + _wp0_ttw_node->setStringValue(wp0_TTW_str); } else - _wp_ttw_node->setStringValue("--:--:--"); + _wp0_ttw_node->setStringValue("--:--:--"); // Course deviation is the diffenrence between the bearing // and the course. - wp_course_deviation_deg = wp_bearing_deg - - wp_course_deg; - while (wp_course_deviation_deg < -180.0) { - wp_course_deviation_deg += 360.0; } - while (wp_course_deviation_deg > 180.0) { - wp_course_deviation_deg -= 360.0; } + wp0_course_deviation_deg = wp0_bearing_deg - + wp0_course_deg; + wp0_course_deviation_deg = degrange180(wp0_course_deviation_deg); // If the course deviation is less than 90 degrees to either side, // our desired course is towards the waypoint. - // It does not matter if we are actually moving towards or from the waypoint. - if (fabs(wp_course_deviation_deg) < 90.0) { - _wp_to_flag_node->setBoolValue(true); } - // If it's more than 90 degrees the desired course is from the waypoint. - else if (fabs(wp_course_deviation_deg) > 90.0) { - _wp_to_flag_node->setBoolValue(false); + // It does not matter if we are actually moving + // towards or from the waypoint. + if (fabs(wp0_course_deviation_deg) < 90.0) { + _wp0_to_flag_node->setBoolValue(true); } + // If it's more than 90 degrees the desired + // course is from the waypoint. + else if (fabs(wp0_course_deviation_deg) > 90.0) { + _wp0_to_flag_node->setBoolValue(false); // When the course is away from the waypoint, // it makes sense to change the sign of the deviation. - wp_course_deviation_deg *= -1.0; - while (wp_course_deviation_deg < -90.0) - wp_course_deviation_deg += 180.0; - while (wp_course_deviation_deg > 90.0) - wp_course_deviation_deg -= 180.0; } + wp0_course_deviation_deg *= -1.0; + wp0_course_deviation_deg = + degrange(wp0_course_deviation_deg, -90.0, 90.0); + } + + _wp0_course_deviation_node->setDoubleValue(wp0_course_deviation_deg); + + // Cross track error. + wp0_course_error_m = sin(wp0_course_deviation_deg * SG_PI / 180.0) + * (wp0_distance); + _wp0_course_error_nm_node->setDoubleValue(wp0_course_error_m + * SG_METER_TO_NM); + - _wp_course_deviation_node->setDoubleValue(wp_course_deviation_deg); + + // Find the bearing and distance to waypoint 1. + SGWayPoint wp1(wp1_longitude_deg, wp1_latitude_deg, wp1_altitude_m); + wp1.CourseAndDistance(longitude_deg, latitude_deg, altitude_m, + &wp1_bearing_deg, &wp1_distance); + _wp1_longitude_node->setDoubleValue(wp1_longitude_deg); + _wp1_latitude_node->setDoubleValue(wp1_latitude_deg); + _wp1_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM); + _wp1_bearing_node->setDoubleValue(wp1_bearing_deg); + double wp1_mag_bearing_deg = degrange360(wp1_bearing_deg - magvar_deg); + _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg); + wp1_bearing_error_deg = degrange180(track1_deg - wp1_bearing_deg); + _true_wp1_bearing_error_node->setDoubleValue(wp1_bearing_error_deg); + + // Estimate time to waypoint 1. + // The estimation does not take track into consideration, + // so if you are going away from the waypoint the TTW will + // increase. Makes most sense when travelling directly towards + // the waypoint. + if (speed_kt > 0.0 && wp1_distance > 0.0) { + wp1_TTW = (wp1_distance * SG_METER_TO_NM) / (speed_kt / 3600); + } + else { + wp1_TTW = 0.0; + } + unsigned int wp1_TTW_seconds = (int) (wp1_TTW + 0.5); + if (wp1_TTW_seconds < 356400) { // That's 99 hours + unsigned int wp1_TTW_minutes = 0; + unsigned int wp1_TTW_hours = 0; + char wp1_TTW_str[9]; + while (wp1_TTW_seconds >= 3600) { + wp1_TTW_seconds -= 3600; + wp1_TTW_hours++; + } + while (wp1_TTW_seconds >= 60) { + wp1_TTW_seconds -= 60; + wp1_TTW_minutes++; + } + snprintf(wp1_TTW_str, 8, "%02d:%02d:%02d", + wp1_TTW_hours, wp1_TTW_minutes, wp1_TTW_seconds); + _wp1_ttw_node->setStringValue(wp1_TTW_str); + } + else + _wp1_ttw_node->setStringValue("--:--:--"); + + // Course deviation is the diffenrence between the bearing + // and the course. + wp1_course_deviation_deg = wp1_bearing_deg - + wp1_course_deg; + wp1_course_deviation_deg = degrange180(wp1_course_deviation_deg); + + // If the course deviation is less than 90 degrees to either side, + // our desired course is towards the waypoint. + // It does not matter if we are actually moving + // towards or from the waypoint. + if (fabs(wp1_course_deviation_deg) < 90.0) { + _wp1_to_flag_node->setBoolValue(true); } + // If it's more than 90 degrees the desired + // course is from the waypoint. + else if (fabs(wp1_course_deviation_deg) > 90.0) { + _wp1_to_flag_node->setBoolValue(false); + // When the course is away from the waypoint, + // it makes sense to change the sign of the deviation. + wp1_course_deviation_deg *= -1.0; + wp1_course_deviation_deg = + degrange(wp1_course_deviation_deg, -90.0, 90.0); + } + + _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg); // Cross track error. - wp_course_error_m = sin(wp_course_deviation_deg * SG_PI / 180.0) - * (wp_distance); - _wp_course_error_nm_node->setDoubleValue(wp_course_error_m + wp1_course_error_m = sin(wp1_course_deviation_deg * SG_PI / 180.0) + * (wp1_distance); + _wp1_course_error_nm_node->setDoubleValue(wp1_course_error_m * SG_METER_TO_NM); + + // Leg course deviation is the diffenrence between the bearing + // and the course. + double course_deviation_deg = wp1_bearing_deg - _course_deg; + course_deviation_deg = degrange180(course_deviation_deg); + + // If the course deviation is less than 90 degrees to either side, + // our desired course is towards the waypoint. + // It does not matter if we are actually moving + // towards or from the waypoint. + if (fabs(course_deviation_deg) < 90.0) { + _leg_to_flag_node->setBoolValue(true); } + // If it's more than 90 degrees the desired + // course is from the waypoint. + else if (fabs(course_deviation_deg) > 90.0) { + _leg_to_flag_node->setBoolValue(false); + // When the course is away from the waypoint, + // it makes sense to change the sign of the deviation. + course_deviation_deg *= -1.0; + course_deviation_deg = + degrange(course_deviation_deg, -90.0, 90.0); + } + + _leg_course_deviation_node->setDoubleValue(course_deviation_deg); + + // Cross track error. + double course_error_m = sin(course_deviation_deg * SG_PI / 180.0) + * (_distance_m); + _leg_course_error_nm_node->setDoubleValue(course_error_m * SG_METER_TO_NM); + + // Altitude deviation + double desired_altitude_m = wp1_altitude_m + + wp1_distance * _alt_dist_ratio; + double altitude_deviation_m = altitude_m - desired_altitude_m; + _alt_deviation_node->setDoubleValue(altitude_deviation_m * SG_METER_TO_FEET); + + + // Tracking bug. double tracking_bug = _tracking_bug_node->getDoubleValue(); double true_bug_error = tracking_bug - track1_deg; double magnetic_bug_error = tracking_bug - mag_track_bearing; // Get the errors into the (-180,180) range. - while (true_bug_error < -180.0) - true_bug_error += 360.0; - while (true_bug_error > 180.0) - true_bug_error -= 360.0; - - while (magnetic_bug_error < -180.0) - magnetic_bug_error += 360.0; - while (magnetic_bug_error > 180.0) - magnetic_bug_error -= 360.0; + true_bug_error = degrange180(true_bug_error); + magnetic_bug_error = degrange180(magnetic_bug_error); _true_bug_error_node->setDoubleValue(true_bug_error); _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error); @@ -333,9 +638,9 @@ GPS::update (double delta_time_sec) _last_altitude_m = altitude_m; } -double GPS::deg360 (double deg) +double GPS::degrange360 (double deg) { - while (deg <= 0.0) { + while (deg < 0.0) { deg += 360.0; } while (deg > 360.0) { deg -= 360.0; } @@ -343,7 +648,25 @@ double GPS::deg360 (double deg) return deg; } - +double GPS::degrange180 (double deg) +{ + while (deg < -180.0) + deg += 360.0; + while (deg > 180.0) + deg -= 360.0; + + return deg; +} + +double GPS::degrange (double deg, double min, double max) +{ + double span = max - min; + while (deg < min) + deg += span; + while (deg > max) + deg -= span; + return deg; +} // end of gps.cxx diff --git a/src/Instrumentation/gps.hxx b/src/Instrumentation/gps.hxx index 5283b916e..cb3df424e 100644 --- a/src/Instrumentation/gps.hxx +++ b/src/Instrumentation/gps.hxx @@ -29,6 +29,7 @@ * * /instrumentation/gps/wp-longitude-deg * /instrumentation/gps/wp-latitude-deg + * /instrumentation/gps/wp-altitude-ft * /instrumentation/gps/wp-ID * /instrumentation/gps/wp-name * /instrumentation/gps/desired-course-deg @@ -41,6 +42,7 @@ * /instrumentation/gps/indicated-longitude-deg * /instrumentation/gps/indicated-latitude-deg * /instrumentation/gps/indicated-altitude-ft + * /instrumentation/gps/indicated-vertical-speed-fpm * /instrumentation/gps/indicated-track-true-deg * /instrumentation/gps/indicated-track-magnetic-deg * /instrumentation/gps/indicated-ground-speed-kt @@ -56,6 +58,8 @@ * /instrumentation/gps/trip-odometer * /instrumentation/gps/true-bug-error-deg * /instrumentation/gps/magnetic-bug-error-deg + * /instrumentation/gps/true-bearing-error-deg + * /instrumentation/gps/magnetic-bearing-error-deg */ class GPS : public SGSubsystem { @@ -73,9 +77,9 @@ private: void search (double frequency, double longitude_rad, double latitude_rad, double altitude_m); - double deg360 (double deg); - double deg180 (double deg); - double deg90 (double deg); + double degrange360 (double deg); + double degrange180 (double deg); + double degrange (double deg, double min, double max); SGPropertyNode_ptr _longitude_node; SGPropertyNode_ptr _latitude_node; @@ -83,33 +87,61 @@ private: SGPropertyNode_ptr _magvar_node; SGPropertyNode_ptr _serviceable_node; SGPropertyNode_ptr _electrical_node; - SGPropertyNode_ptr _wp_longitude_node; - SGPropertyNode_ptr _wp_latitude_node; - SGPropertyNode_ptr _wp_ID_node; - SGPropertyNode_ptr _wp_name_node; - SGPropertyNode_ptr _wp_course_node; + SGPropertyNode_ptr _wp0_longitude_node; + SGPropertyNode_ptr _wp0_latitude_node; + SGPropertyNode_ptr _wp0_altitude_node; + SGPropertyNode_ptr _wp0_ID_node; + SGPropertyNode_ptr _wp0_name_node; + SGPropertyNode_ptr _wp0_course_node; SGPropertyNode_ptr _get_nearest_airport_node; - SGPropertyNode_ptr _waypoint_type_node; + SGPropertyNode_ptr _wp0_waypoint_type_node; + SGPropertyNode_ptr _wp1_longitude_node; + SGPropertyNode_ptr _wp1_latitude_node; + SGPropertyNode_ptr _wp1_altitude_node; + SGPropertyNode_ptr _wp1_ID_node; + SGPropertyNode_ptr _wp1_name_node; + SGPropertyNode_ptr _wp1_course_node; + SGPropertyNode_ptr _wp1_waypoint_type_node; SGPropertyNode_ptr _tracking_bug_node; SGPropertyNode_ptr _raim_node; SGPropertyNode_ptr _indicated_longitude_node; SGPropertyNode_ptr _indicated_latitude_node; SGPropertyNode_ptr _indicated_altitude_node; + SGPropertyNode_ptr _indicated_vertical_speed_node; SGPropertyNode_ptr _true_track_node; SGPropertyNode_ptr _magnetic_track_node; SGPropertyNode_ptr _speed_node; - SGPropertyNode_ptr _wp_distance_node; - SGPropertyNode_ptr _wp_ttw_node; - SGPropertyNode_ptr _wp_bearing_node; - SGPropertyNode_ptr _wp_mag_bearing_node; - SGPropertyNode_ptr _wp_course_deviation_node; - SGPropertyNode_ptr _wp_course_error_nm_node; - SGPropertyNode_ptr _wp_to_flag_node; + SGPropertyNode_ptr _wp0_distance_node; + SGPropertyNode_ptr _wp0_ttw_node; + SGPropertyNode_ptr _wp0_bearing_node; + SGPropertyNode_ptr _wp0_mag_bearing_node; + SGPropertyNode_ptr _wp0_course_deviation_node; + SGPropertyNode_ptr _wp0_course_error_nm_node; + SGPropertyNode_ptr _wp0_to_flag_node; + SGPropertyNode_ptr _wp1_distance_node; + SGPropertyNode_ptr _wp1_ttw_node; + SGPropertyNode_ptr _wp1_bearing_node; + SGPropertyNode_ptr _wp1_mag_bearing_node; + SGPropertyNode_ptr _wp1_course_deviation_node; + SGPropertyNode_ptr _wp1_course_error_nm_node; + SGPropertyNode_ptr _wp1_to_flag_node; SGPropertyNode_ptr _odometer_node; SGPropertyNode_ptr _trip_odometer_node; SGPropertyNode_ptr _true_bug_error_node; SGPropertyNode_ptr _magnetic_bug_error_node; + SGPropertyNode_ptr _true_wp0_bearing_error_node; + SGPropertyNode_ptr _magnetic_wp0_bearing_error_node; + SGPropertyNode_ptr _true_wp1_bearing_error_node; + SGPropertyNode_ptr _magnetic_wp1_bearing_error_node; + SGPropertyNode_ptr _leg_distance_node; + SGPropertyNode_ptr _leg_course_node; + SGPropertyNode_ptr _leg_magnetic_course_node; + SGPropertyNode_ptr _alt_dist_ratio_node; + SGPropertyNode_ptr _leg_course_deviation_node; + SGPropertyNode_ptr _leg_course_error_nm_node; + SGPropertyNode_ptr _leg_to_flag_node; + SGPropertyNode_ptr _alt_deviation_node; bool _last_valid; double _last_longitude_deg; @@ -117,9 +149,24 @@ private: double _last_altitude_m; double _last_speed_kts; - double _wp_latitude; - double _wp_longitude; - string _last_wp_ID; + double _wp0_latitude_deg; + double _wp0_longitude_deg; + double _wp0_altitude_m; + double _wp1_latitude_deg; + double _wp1_longitude_deg; + double _wp1_altitude_m; + string _last_wp0_ID; + string _last_wp1_ID; + + double _alt_dist_ratio; + double _distance_m; + double _course_deg; + + double bias_length; + double bias_angle; + double azimuth_error; + double range_error; + double elapsed_time; }; -- 2.39.5