Erik Hofman:

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

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index a142a4874c17c4d7bc568b2163a6ac6bda36d716..1341368d90bf2502ac8d3593a513456ec31f9d7a 100644 (file)
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -28,6 +28,7 @@
 #endif
 
 #include <stdio.h>             // sprintf()
+#include <string.h>            // strcmp()
 
 #include <simgear/constants.h>
 #include <simgear/sg_inlines.h>
@@ -215,6 +216,8 @@ void FGAutopilot::init ()
     roll_node = fgGetNode("/orientation/roll-deg", true);
     pitch_node = fgGetNode("/orientation/pitch-deg", true);
 
+
+
     // bind config property nodes...
     TargetClimbRate
         = fgGetNode("/autopilot/config/target-climb-rate-fpm", true);
@@ -230,6 +233,11 @@ void FGAutopilot::init ()
         = fgGetNode("/autopilot/config/zero-pitch-throttle", true);
     zero_pitch_trim_full_throttle
         = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true);
+    max_aileron_node = fgGetNode("/autopilot/config/max-aileron", true);
+    max_roll_node = fgGetNode("/autopilot/config/max-roll-deg", true);
+    roll_out_node = fgGetNode("/autopilot/config/roll-out-deg", true);
+    roll_out_smooth_node = fgGetNode("/autopilot/config/roll-out-smooth-deg", true);
+
     current_throttle = fgGetNode("/controls/throttle");
 
     // initialize config properties with defaults (in case config isn't there)
@@ -249,12 +257,21 @@ void FGAutopilot::init ()
         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 );
+    if ( max_aileron_node->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/max-aileron", 0.2 );
+    if ( max_roll_node->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/max-roll-deg", 20 );
+    if ( roll_out_node->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/roll-out-deg", 20 );
+    if ( roll_out_smooth_node->getFloatValue() < 0.0000001 )
+        fgSetFloat( "/autopilot/config/roll-out-smooth-deg", 10 );
 
     /* set defaults */
     heading_hold = false ;      // turn the heading hold off
     altitude_hold = false ;     // turn the altitude hold off
     auto_throttle = false ;    // turn the auto throttle off
     heading_mode = DEFAULT_AP_HEADING_LOCK;
+    altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
 
     DGTargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
     TargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
@@ -281,9 +298,6 @@ void FGAutopilot::init ()
     // the deg from heading to start rolling out at, in Deg
     RollOut = 20;
 
-    // how far can I move the aleron from center.
-    MaxAileron = .2;
-
     // Smoothing distance for alerion control
     RollOutSmooth = 10;
 
@@ -294,7 +308,7 @@ void FGAutopilot::init ()
 #if !defined( USING_SLIDER_CLASS )
     MaxRollAdjust = 2 * MaxRoll;
     RollOutAdjust = 2 * RollOut;
-    MaxAileronAdjust = 2 * MaxAileron;
+    //MaxAileronAdjust = 2 * MaxAileron;
     RollOutSmoothAdjust = 2 * RollOutSmooth;
 #endif  // !defined( USING_SLIDER_CLASS )
 
@@ -419,7 +433,7 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub
 
 
 void
-FGAutopilot::update (int dt)
+FGAutopilot::update (double dt)
 {
     // Remove the following lines when the calling funcitons start
     // passing in the data pointer
@@ -430,6 +444,12 @@ FGAutopilot::update (int dt)
     double lon = longitude_node->getDoubleValue();
     double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 
+    // get config settings
+    MaxAileron = max_aileron_node->getDoubleValue();
+    MaxRoll = max_roll_node->getDoubleValue();
+    RollOut = roll_out_node->getDoubleValue();
+    RollOutSmooth = roll_out_smooth_node->getDoubleValue();
+
     SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run()  lat = " << lat <<
             "  lon = " << lon << "  alt = " << alt );
        
@@ -507,6 +527,13 @@ FGAutopilot::update (int dt)
                * (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM);
            SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
 
+            // clamp closer when inside cone when beyond 5km...
+            if (current_radiostack->get_nav1_loc_dist() > 5000) {
+              double clamp_angle = fabs(current_radiostack->get_nav1_heading_needle_deflection()) * 3;
+              if (clamp_angle < 30)
+                SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle);
+            }
+
            // determine the target heading to fly to intercept the
            // tgt_radial
            TargetHeading = tgt_radial + adjustment; 
@@ -595,8 +622,10 @@ FGAutopilot::update (int dt)
            // figure out how far off we are from desired heading
 
            // Now it is time to deterime how far we should be rolled.
-           SG_LOG( SG_AUTOPILOT, SG_DEBUG, "RelHeading: " << RelHeading );
-
+           SG_LOG( SG_AUTOPILOT, SG_DEBUG,
+                    "Heading = " << heading_node->getDoubleValue() <<
+                    " TargetHeading = " << TargetHeading <<
+                    " RelHeading = " << RelHeading );
 
            // Check if we are further from heading than the roll out point
            if ( fabs( RelHeading ) > RollOut ) {
@@ -734,11 +763,17 @@ FGAutopilot::update (int dt)
        // 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 / elevator_adj_factor->getFloatValue();
+        
+        // scale elev_adj_factor by speed of aircraft in relation to min climb 
+       double elev_adj_factor = elevator_adj_factor->getFloatValue();
+        elev_adj_factor *=
+         pow(float(speed / min_climb->getFloatValue()), 3.0f);
+
+       int_adj = int_error / elev_adj_factor;
 
        // caclulate proportional error
        prop_error = error;
-       prop_adj = prop_error / elevator_adj_factor->getDoubleValue();
+       prop_adj = prop_error / elev_adj_factor;
 
        total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj
             + (double) integral_contrib->getFloatValue() * int_adj;
@@ -906,7 +941,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
              * SG_FEET_TO_METER ) {
        }
 
-       if ( fgGetString("/sim/startup/units") == "feet" ) {
+       if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
            MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
        } else {
            MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
@@ -917,7 +952,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
        TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER;
 
-       if ( fgGetString("/sim/startup/units") == "feet" ) {
+       if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
            MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
        } else {
            MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
@@ -932,18 +967,18 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
 void FGAutopilot::AltitudeSet( double new_altitude ) {
     double target_alt = new_altitude;
 
-    if ( fgGetString("/sim/startup/units") == "feet" ) {
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt = new_altitude * SG_FEET_TO_METER;
     }
 
-    if( target_alt < scenery.get_cur_elev() ) {
-       target_alt = scenery.get_cur_elev();
+    if( target_alt < globals->get_scenery()->get_cur_elev() ) {
+       target_alt = globals->get_scenery()->get_cur_elev();
     }
 
     TargetAltitude = target_alt;
     altitude_mode = FG_ALTITUDE_LOCK;
 
-    if ( fgGetString("/sim/startup/units") == "feet" ) {
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt *= SG_METER_TO_FEET;
     }
     // ApAltitudeDialogInput->setValue((float)target_alt);
@@ -957,7 +992,7 @@ void FGAutopilot::AltitudeAdjust( double inc )
 {
     double target_alt, target_agl;
 
-    if ( fgGetString("/sim/startup/units") == "feet" ) {
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt = TargetAltitude * SG_METER_TO_FEET;
        target_agl = TargetAGL * SG_METER_TO_FEET;
     } else {
@@ -977,7 +1012,7 @@ void FGAutopilot::AltitudeAdjust( double inc )
        target_agl = ( int ) ( target_agl / inc ) * inc + inc;
     }
 
-    if ( fgGetString("/sim/startup/units") == "feet" ) {
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt *= SG_FEET_TO_METER;
        target_agl *= SG_FEET_TO_METER;
     }
@@ -985,9 +1020,9 @@ void FGAutopilot::AltitudeAdjust( double inc )
     TargetAltitude = target_alt;
     TargetAGL = target_agl;
        
-    if ( fgGetString("/sim/startup/units") == "feet" )
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
        target_alt *= SG_METER_TO_FEET;
-    if ( fgGetString("/sim/startup/units") == "feet" )
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
        target_agl *= SG_METER_TO_FEET;
 
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
@@ -1363,3 +1398,4 @@ FGAutopilot::setAPThrottleControl (double value)
 }
 
 // end of newauto.cxx
+