From 1a6ed2ecc886bdbcd649cb7dbe0e67530834d5ec Mon Sep 17 00:00:00 2001
From: curt <curt>
Date: Fri, 26 Jan 2001 00:20:18 +0000
Subject: [PATCH] Tweaks to autopilot, but I'm not 100% happy with the current
 state of things.

---
 src/Autopilot/newauto.cxx | 36 +++++++++++++++++++++++++++++++-----
 1 file changed, 31 insertions(+), 5 deletions(-)

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index 196791aef..b021a7860 100644
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -107,6 +107,14 @@ static inline double get_ground_speed() {
 
 
 void FGAutopilot::MakeTargetWPStr( double distance ) {
+    static time_t last_time = 0;
+    time_t current_time = time(NULL);
+    if ( last_time == current_time ) {
+	return;
+    }
+
+    last_time = current_time;
+
     double accum = 0.0;
 
     int size = globals->get_route()->size();
@@ -529,10 +537,28 @@ int FGAutopilot::run() {
 	    double x = current_radiostack->get_nav1_gs_dist();
 	    double y = (FGBFI::getAltitude() 
 			- current_radiostack->get_nav1_elev()) * FEET_TO_METER;
-	    double angle = atan2( y, x ) * RAD_TO_DEG;
-	    double gs_diff = current_radiostack->get_nav1_target_gs() - angle;
-	    climb_error_accum += gs_diff * 2.0;
-	    TargetClimbRate = gs_diff * 200.0 + climb_error_accum;
+	    double current_angle = atan2( y, x ) * RAD_TO_DEG;
+	    // cout << "current angle = " << current_angle << endl;
+
+	    double target_angle = current_radiostack->get_nav1_target_gs();
+	    // cout << "target angle = " << target_angle << endl;
+
+	    double gs_diff = target_angle - current_angle;
+	    // cout << "difference from desired = " << gs_diff << endl;
+
+	    // convert desired vertical path angle into a climb rate
+	    double des_angle = current_angle - 10 * gs_diff;
+	    // cout << "desired angle = " << des_angle << endl;
+
+	    // convert to meter/min
+	    // cout << "raw ground speed = " << cur_fdm_state->get_V_ground_speed() << endl;
+	    double horiz_vel = cur_fdm_state->get_V_ground_speed()
+		* FEET_TO_METER * 60.0;
+	    // cout << "Horizontal vel = " << horiz_vel << endl;
+	    TargetClimbRate = -sin( des_angle * DEG_TO_RAD ) * horiz_vel;
+	    // cout << "TargetClimbRate = " << TargetClimbRate << endl;
+	    /* climb_error_accum += gs_diff * 2.0; */
+	    /* TargetClimbRate = gs_diff * 200.0 + climb_error_accum; */
 	} else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 	    // brain dead ground hugging with no look ahead
 	    TargetClimbRate =
@@ -584,7 +610,7 @@ int FGAutopilot::run() {
 	// calculate integral error, and adjustment amount
 	int_error = alt_error_accum;
 	// printf("error = %.2f  int_error = %.2f\n", error, int_error);
-	int_adj = int_error / 8000.0;
+	int_adj = int_error / 20000.0;
 
 	// caclulate proportional error
 	prop_error = error;
-- 
2.39.5