The code to find the highest hit below you didn't work quite right when

[flightgear.git] / src / Autopilot / newauto.cxx

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index a881edecf801f2d33a81937cbffb787af77aa853..81708ff2aac6a7f2326267087dfce59d29b058c6 100644 (file)
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -746,9 +746,9 @@ FGAutopilot::update (double dt)
        if ( altitude_mode == FG_ALTITUDE_LOCK ) {
            climb_rate =
                ( TargetAltitude -
-                 fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * (TargetClimbRate->getDoubleValue() * 0.016);
+                 fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * 8.0;
         } else if ( altitude_mode == FG_TRUE_ALTITUDE_LOCK ) {
-            climb_rate = ( TargetAltitude - alt ) * (TargetClimbRate->getDoubleValue() * 0.016);
+            climb_rate = ( TargetAltitude - alt ) * 8.0;
        } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
            double x = current_radiostack->get_navcom1()->get_nav_gs_dist();
            double y = (altitude_node->getDoubleValue()
@@ -871,7 +871,7 @@ FGAutopilot::update (double dt)
                       double lookahead;
 
                        // estimate speed in 10 seconds
-                      lookahead =  airspeed_node->getFloatValue() + ( airspeed_node->getFloatValue() - previous_speed) * (10/dt);
+                      lookahead =  airspeed_node->getFloatValue() + ( airspeed_node->getFloatValue() - previous_speed) * (10/(dt + 0.000001));
        previous_speed =  airspeed_node->getFloatValue();
 
                       // compare targetspeed to anticipated airspeed
@@ -903,17 +903,17 @@ FGAutopilot::update (double dt)
        total_adj = (1.0 - throttle_integral->getFloatValue()) * prop_adj + 
                                      throttle_integral->getFloatValue() * int_adj;
 
-                      total_adj = current_throttle->getFloatValue() + total_adj;
+                      current_ap_throttle = current_ap_throttle + total_adj;
 
-       if ( total_adj > 1.0 ) {
-           total_adj = 1.0;
+       if ( current_ap_throttle > 1.0 ) {
+           current_ap_throttle = 1.0;
        }
-       else if ( total_adj < 0.0 ) {
-           total_adj = 0.0;
+       else if ( current_ap_throttle < 0.0 ) {
+           current_ap_throttle = 0.0;
        }
 
        globals->get_controls()->set_throttle( FGControls::ALL_ENGINES,
-                                              total_adj );
+                                              current_ap_throttle );
     }
 
 #ifdef THIS_CODE_IS_NOT_USED
@@ -1186,7 +1186,9 @@ void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
         if (TargetSpeed < 0.0001) {
           TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
         }
-       speed_error_accum = 0.0;
+        speed_error_accum = 0.0;
+        // initialize autothrottle at current control setting;
+        current_ap_throttle = current_throttle->getFloatValue();
     }
 
     update_old_control_values();