+
+ odometer = _odometer_node->getDoubleValue();
+ _odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+ odometer = _trip_odometer_node->getDoubleValue();
+ _trip_odometer_node->setDoubleValue(odometer + distance_m * SG_METER_TO_NM);
+
+ // 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 1.
+ if (_get_nearest_airport_node->getBoolValue()) {
+ const FGAirport* a = globals->get_airports()->search(longitude_deg, latitude_deg);
+ if(a) {
+ _wp1_ID_node->setStringValue(a->getId().c_str());
+ wp1_longitude_deg = a->getLongitude();
+ wp1_latitude_deg = a->getLatitude();
+ _wp1_name_node->setStringValue(a->getName().c_str());
+ _get_nearest_airport_node->setBoolValue(false);
+ _last_wp1_ID = wp1_ID = a->getId().c_str();
+ }
+ }
+
+ // If the waypoint 0 ID has changed, try to find the new ID
+ // in the airport-, fix-, nav-database.
+ if ( !(_last_wp0_ID == wp0_ID) ) {
+ string waypont_type =
+ _wp0_waypoint_type_node->getStringValue();
+ if (waypont_type == "airport") {
+ const FGAirport* a = globals->get_airports()->search( wp0_ID );
+ if ( a ) {
+ wp0_longitude_deg = a->getLongitude();
+ wp0_latitude_deg = a->getLatitude();
+ _wp0_name_node->setStringValue(a->getName().c_str());
+ }
+ }
+ else if (waypont_type == "nav") {
+ FGNavRecord *n
+ = globals->get_navlist()->findByIdent(wp0_ID.c_str(),
+ longitude_deg,
+ latitude_deg);
+ if ( n != 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 ( globals->get_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_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") {
+ const FGAirport* a = globals->get_airports()->search( wp1_ID );
+ if ( a ) {
+ wp1_longitude_deg = a->getLongitude();
+ wp1_latitude_deg = a->getLatitude();
+ _wp1_name_node->setStringValue(a->getName().c_str());
+ }
+ }
+ else if (waypont_type == "nav") {
+ FGNavRecord *n
+ = globals->get_navlist()->findByIdent(wp1_ID.c_str(),
+ longitude_deg,
+ latitude_deg);
+ if ( n != 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 ( globals->get_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);
+ double leg_mag_course = _course_deg - magvar_deg;
+ SG_NORMALIZE_RANGE(leg_mag_course, 0.0, 360.0);
+
+ // 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 * SG_METER_TO_NM);
+ _leg_course_node->setDoubleValue(_course_deg);
+ _leg_magnetic_course_node->setDoubleValue(leg_mag_course);
+ _alt_dist_ratio_node->setDoubleValue(_alt_dist_ratio);
+
+ _wp0_longitude_node->setDoubleValue(wp0_longitude_deg);
+ _wp0_latitude_node->setDoubleValue(wp0_latitude_deg);
+ _wp1_longitude_node->setDoubleValue(wp1_longitude_deg);
+ _wp1_latitude_node->setDoubleValue(wp1_latitude_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_distance_node->setDoubleValue(wp0_distance * SG_METER_TO_NM);
+ _wp0_bearing_node->setDoubleValue(wp0_bearing_deg);
+ double wp0_mag_bearing_deg = wp0_bearing_deg - magvar_deg;
+ SG_NORMALIZE_RANGE(wp0_mag_bearing_deg, 0.0, 360.0);
+ _wp0_mag_bearing_node->setDoubleValue(wp0_mag_bearing_deg);
+ wp0_bearing_error_deg = track1_deg - wp0_bearing_deg;
+ SG_NORMALIZE_RANGE(wp0_bearing_error_deg, -180.0, 180.0);
+ _true_wp0_bearing_error_node->setDoubleValue(wp0_bearing_error_deg);
+
+ // 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 && wp0_distance > 0.0) {
+ wp0_TTW = (wp0_distance * SG_METER_TO_NM) / (speed_kt / 3600);
+ }
+ else {
+ wp0_TTW = 0.0;
+ }
+ 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 (wp0_TTW_seconds >= 60) {
+ wp0_TTW_seconds -= 60;
+ wp0_TTW_minutes++;
+ }
+ snprintf(wp0_TTW_str, 9, "%02d:%02d:%02d",
+ wp0_TTW_hours, wp0_TTW_minutes, wp0_TTW_seconds);
+ _wp0_ttw_node->setStringValue(wp0_TTW_str);
+ }
+ else
+ _wp0_ttw_node->setStringValue("--:--:--");
+
+ // Course deviation is the diffenrence between the bearing
+ // and the course.
+ wp0_course_deviation_deg = wp0_bearing_deg -
+ wp0_course_deg;
+ SG_NORMALIZE_RANGE(wp0_course_deviation_deg, -180.0, 180.0);
+
+ // 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(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.
+ wp0_course_deviation_deg *= -1.0;
+ SG_NORMALIZE_RANGE(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);
+
+
+
+ // 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_distance_node->setDoubleValue(wp1_distance * SG_METER_TO_NM);
+ _wp1_bearing_node->setDoubleValue(wp1_bearing_deg);
+ double wp1_mag_bearing_deg = wp1_bearing_deg - magvar_deg;
+ SG_NORMALIZE_RANGE(wp1_mag_bearing_deg, 0.0, 360.0);
+ _wp1_mag_bearing_node->setDoubleValue(wp1_mag_bearing_deg);
+ wp1_bearing_error_deg = track1_deg - wp1_bearing_deg;
+ SG_NORMALIZE_RANGE(wp1_bearing_error_deg, -180.0, 180.0);
+ _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, 9, "%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;
+ SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -180.0, 180.0);
+
+ // 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;
+ SG_NORMALIZE_RANGE(wp1_course_deviation_deg, -90.0, 90.0);
+ }
+
+ _wp1_course_deviation_node->setDoubleValue(wp1_course_deviation_deg);
+
+ // Cross track error.
+ 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;
+ SG_NORMALIZE_RANGE(course_deviation_deg, -180.0, 180.0);
+
+ // 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;
+ SG_NORMALIZE_RANGE(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.
+ SG_NORMALIZE_RANGE(true_bug_error, -180.0, 180.0);
+ SG_NORMALIZE_RANGE(magnetic_bug_error, -180.0, 180.0);
+
+ _true_bug_error_node->setDoubleValue(true_bug_error);
+ _magnetic_bug_error_node->setDoubleValue(magnetic_bug_error);
+
+
+ // Add WP 1 to the route.
+ if ( addWp->getBoolValue() )
+ {
+ addWp->setBoolValue(false);
+
+ SGWayPoint tempWp( _wp1_longitude_node->getDoubleValue(),
+ _wp1_latitude_node->getDoubleValue(),
+ _wp1_altitude_node->getDoubleValue(),
+ SGWayPoint::WGS84,
+ _wp1_ID_node->getStringValue(),
+ _wp1_name_node->getStringValue() );
+
+ route->add_waypoint(tempWp);
+
+ SGPropertyNode *wp =
+ _route->getChild("Waypoint", route->size()-1, true);
+ SGPropertyNode *id = wp->getChild("ID", 0, true);
+ SGPropertyNode *name = wp->getChild("Name", 0, true);
+ SGPropertyNode *lat = wp->getChild("Latitude", 0, true);
+ SGPropertyNode *lon = wp->getChild("Longitude", 0, true);
+ SGPropertyNode *alt = wp->getChild("Altitude", 0, true);
+
+ id->setStringValue( tempWp.get_id().c_str() );
+ name->setStringValue( tempWp.get_name().c_str() );
+ lat->setDoubleValue( tempWp.get_target_lat() );
+ lon->setDoubleValue( tempWp.get_target_lon() );
+ alt->setDoubleValue( tempWp.get_target_alt() );
+ }
+
+ if ( popWp->getBoolValue() )
+ {
+ popWp->setBoolValue(false);
+
+ route->delete_first();
+ _route->removeChild("Waypoint", 0, false);
+ }
+