+
+
+ // 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);
+
+
+