Additional autopilot tuning taking into consideration more data.

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index c427fb93759f888a40d0045ffd5a893e431276c7..d553097035d62b5a1d4e19fa5e5e7776727f9320 100644 (file)
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -81,7 +81,11 @@ FGAutopilot::FGAutopilot()
         = fgGetNode("/autopilot/config/elevator-adj-factor", true);
     integral_contrib
         = fgGetNode("/autopilot/config/integral-contribution", true);
-    cout << "elevadj = " << elevator_adj_factor->getFloatValue() << endl;
+    zero_pitch_throttle
+        = fgGetNode("/autopilot/config/zero-pitch-throttle", true);
+    zero_pitch_trim_full_throttle
+        = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true);
+    current_throttle = fgGetNode("/controls/throttle");
     // initialize with defaults (in case config isn't there)
     if ( TargetClimbRate->getFloatValue() < 1 )
         fgSetFloat( "/autopilot/config/target-climb-rate-fpm", 500);
@@ -95,6 +99,10 @@ FGAutopilot::FGAutopilot()
         fgSetFloat( "/autopilot/config/elevator-adj-factor", 5000 );
     if ( integral_contrib->getFloatValue() < 0.0000001 )
         fgSetFloat( "/autopilot/config/integral-contribution", 0.01 );
+    if ( zero_pitch_throttle->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/zero-pitch-throttle", 0.60 );
+    if ( zero_pitch_trim_full_throttle->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/zero-pitch-trim-full-throttle", 0.15 );
 }
 
 // destructor
@@ -265,7 +273,7 @@ void FGAutopilot::init() {
        
     TargetHeading = 0.0;       // default direction, due north
     TargetAltitude = 3000;     // default altitude in meters
-    alt_error_accum = 0.0;
+    alt_error_accum = 0;    
     climb_error_accum = 0.0;
 
     MakeTargetAltitudeStr( TargetAltitude );
@@ -320,7 +328,8 @@ void FGAutopilot::reset() {
     // TargetAltitude = 3000;   // default altitude in meters
     MakeTargetAltitudeStr( TargetAltitude );
        
-    alt_error_accum = 0.0;
+    alt_error_accum = 0;    
+
     climb_error_accum = 0.0;
        
     update_old_control_values();
@@ -695,7 +704,7 @@ int FGAutopilot::run() {
                 = -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER);
        }
 
-       // cout << "Target climb rate = " << TargetClimbRate << endl;
+       // cout << "Target climb rate = " << TargetClimbRate->getFloatValue() << endl;
        // cout << "given our speed, modified desired climb rate = "
        //      << climb_rate * SG_METER_TO_FEET 
        //      << " fpm" << endl;
@@ -716,20 +725,29 @@ int FGAutopilot::run() {
 
        // caclulate proportional error
        prop_error = error;
-       prop_adj = prop_error / elevator_adj_factor->getFloatValue();
-
-       total_adj = (1.0 - integral_contrib->getFloatValue()) * prop_adj
-            + integral_contrib->getFloatValue() * int_adj;
-       // if ( total_adj > 0.6 ) {
-       //     total_adj = 0.6;
-       // } else if ( total_adj < -0.2 ) {
-       //     total_adj = -0.2;
-       // }
-       if ( total_adj > 1.0 ) {
-           total_adj = 1.0;
-       } else if ( total_adj < -1.0 ) {
-           total_adj = -1.0;
-       }
+       prop_adj = prop_error / elevator_adj_factor->getDoubleValue();
+
+        // cout << "Error=" << error << endl;
+        // cout << "integral_error=" << int_error << endl;
+        // cout << "integral_contrib=" << integral_contrib->getFloatValue() << endl;
+        // cout << "Proportional Adj=" << prop_adj << endl;
+        // cout << "Integral Adj" << int_adj << endl;
+       total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj
+            + (double) integral_contrib->getFloatValue() * int_adj;
+
+        // stop on autopilot trim at 30% +/-
+       if ( total_adj > 0.3 ) {
+            total_adj = 0.3;
+        } else if ( total_adj < -0.3 ) {
+            total_adj = -0.3;
+        }
+
+        // adjust for throttle pitch gain
+        total_adj += ((current_throttle->getFloatValue() - zero_pitch_throttle->getFloatValue())
+                     / (1 - zero_pitch_throttle->getFloatValue()))
+                     * zero_pitch_trim_full_throttle->getFloatValue();
+
+        // cout << "Total Adj" << total_adj << endl;
 
        globals->get_controls()->set_elevator_trim( total_adj );
     }
@@ -876,7 +894,8 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
     altitude_mode = mode;
 
-    alt_error_accum = 0.0;
+    alt_error_accum = 0;    
+
 
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
        if ( TargetAltitude < altitude_agl_node->getDoubleValue()

diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx
index 22c1db947fab8bdbff8a8714810950eadb73c823..252280adeec9cf35d7c467d2fcb76cf38393901c 100644 (file)
--- a/src/Autopilot/newauto.hxx
+++ b/src/Autopilot/newauto.hxx
@@ -76,10 +76,13 @@ private:
     SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
     SGPropertyNode *integral_contrib;    // amount of contribution of the integral
                                 // component of the pid
+    SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
+    SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
     SGPropertyNode *TargetClimbRate;   // target climb rate
     SGPropertyNode *TargetDescentRate; // target decent rate
+    SGPropertyNode *current_throttle; // current throttle (engine 0)
     double TargetSpeed;                // speed to shoot for
-    double alt_error_accum;    // altitude error accumulator
+    double alt_error_accum;     // altitude error accumulator
     double climb_error_accum;  // climb error accumulator (for GS)
     double speed_error_accum;  // speed error accumulator