ignore resets for now because every z/Z key press would trigger a call to NOAA. We...

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

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index e0bed6acdcd6b2ad15f0cb92a9c523e7d46aa7cc..b699c8c9d8e4bee1806a728192512f38381b6efb 100644 (file)
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -756,15 +756,23 @@ FGAutopilot::update (double dt)
            double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 
            double target_angle = current_radiostack->get_navcom1()->get_nav_target_gs();
-
            double gs_diff = target_angle - current_angle;
 
            // convert desired vertical path angle into a climb rate
            double des_angle = current_angle - 10 * gs_diff;
 
-           // convert to meter/min
-           double horiz_vel = cur_fdm_state->get_V_ground_speed()
-               * SG_FEET_TO_METER * 60.0;
+            // estimate horizontal speed towards ILS in meters per minute
+            static double horiz_vel = 0.0;
+            static double last_x = 0.0;
+            double dist = last_x - x;
+            last_x = x;
+            double new_vel = ( dist / dt ) * 60.0;
+            horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
+           // double horiz_vel = cur_fdm_state->get_V_ground_speed()
+            //    * SG_FEET_TO_METER * 60.0;
+            // double horiz_vel = airspeed_node->getFloatValue()
+            //    * SG_FEET_TO_METER * 60.0;
+
            climb_rate = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) * horiz_vel;
            /* climb_error_accum += gs_diff * 2.0; */
            /* climb_rate = gs_diff * 200.0 + climb_error_accum; */
@@ -871,7 +879,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 +911,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 +1194,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();