X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.cxx;h=1341368d90bf2502ac8d3593a513456ec31f9d7a;hb=a4e81f4ff075e6a3c0c2ea1b5a29c0bcdfdbc671;hp=ef308fc481b05f445f2f1cb58c1174874887b355;hpb=b62900e44cdcb38abf9ce74c94b0186d90112827;p=flightgear.git

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index ef308fc48..1341368d9 100644
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -28,28 +28,25 @@
 #endif
 
 #include <stdio.h>		// sprintf()
+#include <string.h>		// strcmp()
 
 #include <simgear/constants.h>
+#include <simgear/sg_inlines.h>
 #include <simgear/debug/logstream.hxx>
-#include <simgear/math/fg_geodesy.hxx>
+#include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/sg_random.h>
+#include <simgear/route/route.hxx>
 
+#include <Cockpit/steam.hxx>
+#include <Cockpit/radiostack.hxx>
 #include <Controls/controls.hxx>
 #include <FDM/flight.hxx>
-#include <Main/bfi.hxx>
-#include <Main/options.hxx>
+#include <Main/globals.hxx>
 #include <Scenery/scenery.hxx>
 
 #include "newauto.hxx"
 
 
-FGAutopilot *current_autopilot;
-
-
-// Climb speed constants
-const double min_climb = 70.0;	// kts
-const double best_climb = 75.0;	// kts
-const double ideal_climb_rate = 500.0; // fpm
-
 /// These statics will eventually go into the class
 /// they are just here while I am experimenting -- NHV :-)
 // AutoPilot Gain Adjuster members
@@ -58,18 +55,6 @@ static double RollOutAdjust;        // RollOutAdjust       = 2 * APData->RollOut
 static double MaxAileronAdjust;     // MaxAileronAdjust    = 2 * APData->MaxAileron;
 static double RollOutSmoothAdjust;  // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
 
-#if 0
-static float MaxRollValue;          // 0.1 -> 1.0
-static float RollOutValue;
-static float MaxAileronValue;
-static float RollOutSmoothValue;
-
-static float TmpMaxRollValue;       // for cancel operation
-static float TmpRollOutValue;
-static float TmpMaxAileronValue;
-static float TmpRollOutSmoothValue;
-#endif
-
 static char NewTgtAirportId[16];
 // static char NewTgtAirportLabel[] = "Enter New TgtAirport ID"; 
 
@@ -77,9 +62,18 @@ extern char *coord_format_lat(float);
 extern char *coord_format_lon(float);
 			
 
+// constructor
+FGAutopilot::FGAutopilot()
+{
+}
+
+// destructor
+FGAutopilot::~FGAutopilot() {}
+
+
 void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
-    sprintf( TargetLatitudeStr , "%s", coord_format_lat(TargetLatitude) );
-    sprintf( TargetLongitudeStr, "%s", coord_format_lon(TargetLongitude) );
+    sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
+    sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
     sprintf( TargetLatLonStr, "%s  %s", TargetLatitudeStr, TargetLongitudeStr );
 }
 
@@ -94,7 +88,7 @@ void FGAutopilot::MakeTargetAltitudeStr( double altitude ) {
 
 
 void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
-    if( bearing < 0. ) {
+    if ( bearing < 0. ) {
 	bearing += 360.;
     } else if (bearing > 360. ) {
 	bearing -= 360.;
@@ -103,34 +97,198 @@ void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
 }
 
 
+static inline double get_speed( void ) {
+    return( cur_fdm_state->get_V_equiv_kts() );
+}
+
+static inline double get_ground_speed() {
+    // starts in ft/s so we convert to kts
+    static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
+
+    double ft_s = cur_fdm_state->get_V_ground_speed() 
+        * speedup_node->getIntValue();
+    double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
+
+    return kts;
+}
+
+
+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();
+
+    // start by wiping the strings
+    TargetWP1Str[0] = 0;
+    TargetWP2Str[0] = 0;
+    TargetWP3Str[0] = 0;
+
+    // current route
+    if ( size > 0 ) {
+	SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 );
+	accum += distance;
+	double eta = accum * SG_METER_TO_NM / get_ground_speed();
+	if ( eta >= 100.0 ) { eta = 99.999; }
+	int major, minor;
+	if ( eta < (1.0/6.0) ) {
+	    // within 10 minutes, bump up to min/secs
+	    eta *= 60.0;
+	}
+	major = (int)eta;
+	minor = (int)((eta - (int)eta) * 60.0);
+	sprintf( TargetWP1Str, "%s %.2f NM  ETA %d:%02d",
+		 wp1.get_id().c_str(),
+		 accum*SG_METER_TO_NM, major, minor );
+    }
+
+    // next route
+    if ( size > 1 ) {
+	SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 );
+	accum += wp2.get_distance();
+	
+	double eta = accum * SG_METER_TO_NM / get_ground_speed();
+	if ( eta >= 100.0 ) { eta = 99.999; }
+	int major, minor;
+	if ( eta < (1.0/6.0) ) {
+	    // within 10 minutes, bump up to min/secs
+	    eta *= 60.0;
+	}
+	major = (int)eta;
+	minor = (int)((eta - (int)eta) * 60.0);
+	sprintf( TargetWP2Str, "%s %.2f NM  ETA %d:%02d",
+		 wp2.get_id().c_str(),
+		 accum*SG_METER_TO_NM, major, minor );
+    }
+
+    // next route
+    if ( size > 2 ) {
+	for ( int i = 2; i < size; ++i ) {
+	    accum += globals->get_route()->get_waypoint( i ).get_distance();
+	}
+	
+	SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 );
+
+	double eta = accum * SG_METER_TO_NM / get_ground_speed();
+	if ( eta >= 100.0 ) { eta = 99.999; }
+	int major, minor;
+	if ( eta < (1.0/6.0) ) {
+	    // within 10 minutes, bump up to min/secs
+	    eta *= 60.0;
+	}
+	major = (int)eta;
+	minor = (int)((eta - (int)eta) * 60.0);
+	sprintf( TargetWP3Str, "%s %.2f NM  ETA %d:%02d",
+		 wpn.get_id().c_str(),
+		 accum*SG_METER_TO_NM, major, minor );
+    }
+}
+
+
 void FGAutopilot::update_old_control_values() {
-    old_aileron = FGBFI::getAileron();
-    old_elevator = FGBFI::getElevator();
-    old_elevator_trim = FGBFI::getElevatorTrim();
-    old_rudder = FGBFI::getRudder();
+    old_aileron = globals->get_controls()->get_aileron();
+    old_elevator = globals->get_controls()->get_elevator();
+    old_elevator_trim = globals->get_controls()->get_elevator_trim();
+    old_rudder = globals->get_controls()->get_rudder();
 }
 
 
 // Initialize autopilot subsystem
-void FGAutopilot::init() {
-    FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
 
+void FGAutopilot::init ()
+{
+    SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" );
+
+    // bind data input property nodes...
+    latitude_node = fgGetNode("/position/latitude-deg", true);
+    longitude_node = fgGetNode("/position/longitude-deg", true);
+    altitude_node = fgGetNode("/position/altitude-ft", true);
+    altitude_agl_node = fgGetNode("/position/altitude-agl-ft", true);
+    vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true);
+    heading_node = fgGetNode("/orientation/heading-deg", true);
+    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);
+    TargetDescentRate
+        = fgGetNode("/autopilot/config/target-descent-rate-fpm", true);
+    min_climb = fgGetNode("/autopilot/config/min-climb-speed-kt", true);
+    best_climb = fgGetNode("/autopilot/config/best-climb-speed-kt", true);
+    elevator_adj_factor
+        = fgGetNode("/autopilot/config/elevator-adj-factor", true);
+    integral_contrib
+        = fgGetNode("/autopilot/config/integral-contribution", true);
+    zero_pitch_throttle
+        = 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)
+    if ( TargetClimbRate->getFloatValue() < 1 )
+        fgSetFloat( "/autopilot/config/target-climb-rate-fpm", 500);
+    if ( TargetDescentRate->getFloatValue() < 1 )
+        fgSetFloat( "/autopilot/config/target-descent-rate-fpm", 1000 );
+    if ( min_climb->getFloatValue() < 1)
+        fgSetFloat( "/autopilot/config/min-climb-speed-kt", 70 );
+    if (best_climb->getFloatValue() < 1)
+        fgSetFloat( "/autopilot/config/best-climb-speed-kt", 120 );
+    if (elevator_adj_factor->getFloatValue() < 1)
+        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 );
+    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");
+    TargetAltitude = fgGetDouble("/autopilot/settings/altitude-ft") * SG_FEET_TO_METER;
 
     // Initialize target location to startup location
-    old_lat = TargetLatitude = FGBFI::getLatitude();
-    old_lon = TargetLongitude = FGBFI::getLongitude();
+    old_lat = latitude_node->getDoubleValue();
+    old_lon = longitude_node->getDoubleValue();
+    // set_WayPoint( old_lon, old_lat, 0.0, "default" );
 
-    MakeTargetLatLonStr( TargetLatitude, TargetLongitude);
+    MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 	
-    TargetHeading = 0.0;	// default direction, due north
-    TargetAltitude = 3000;	// default altitude in meters
     alt_error_accum = 0.0;
+    climb_error_accum = 0.0;
 
-    MakeTargetAltitudeStr( 3000.0);
-    MakeTargetHeadingStr( 0.0 );
+    MakeTargetAltitudeStr( TargetAltitude );
+    MakeTargetHeadingStr( TargetHeading );
 	
     // These eventually need to be read from current_aircaft somehow.
 
@@ -140,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;
 
@@ -153,19 +308,68 @@ 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 )
 
     update_old_control_values();
-	
-    // Initialize GUI components of autopilot
-    // NewTgtAirportInit();
-    // fgAPAdjustInit() ;
-    // NewHeadingInit();
-    // NewAltitudeInit();
 };
 
+void
+FGAutopilot::bind ()
+{
+    // Autopilot control property get/set bindings
+    fgTie("/autopilot/locks/altitude", this,
+	  &FGAutopilot::getAPAltitudeLock, &FGAutopilot::setAPAltitudeLock);
+    fgSetArchivable("/autopilot/locks/altitude");
+    fgTie("/autopilot/settings/altitude-ft", this,
+	  &FGAutopilot::getAPAltitude, &FGAutopilot::setAPAltitude);
+    fgSetArchivable("/autopilot/settings/altitude-ft");
+    fgTie("/autopilot/locks/glide-slope", this,
+	  &FGAutopilot::getAPGSLock, &FGAutopilot::setAPGSLock);
+    fgSetArchivable("/autopilot/locks/glide-slope");
+    fgSetDouble("/autopilot/settings/altitude-ft", 3000.0f);
+    fgTie("/autopilot/locks/terrain", this,
+	  &FGAutopilot::getAPTerrainLock, &FGAutopilot::setAPTerrainLock);
+    fgSetArchivable("/autopilot/locks/terrain");
+    fgTie("/autopilot/settings/climb-rate-fpm", this,
+	  &FGAutopilot::getAPClimb, &FGAutopilot::setAPClimb, false);
+    fgSetArchivable("/autopilot/settings/climb-rate-fpm");
+    fgTie("/autopilot/locks/heading", this,
+	  &FGAutopilot::getAPHeadingLock, &FGAutopilot::setAPHeadingLock);
+    fgSetArchivable("/autopilot/locks/heading");
+    fgTie("/autopilot/settings/heading-bug-deg", this,
+	  &FGAutopilot::getAPHeadingBug, &FGAutopilot::setAPHeadingBug);
+    fgSetArchivable("/autopilot/settings/heading-bug-deg");
+    fgSetDouble("/autopilot/settings/heading-bug-deg", 0.0f);
+    fgTie("/autopilot/locks/wing-leveler", this,
+	  &FGAutopilot::getAPWingLeveler, &FGAutopilot::setAPWingLeveler);
+    fgSetArchivable("/autopilot/locks/wing-leveler");
+    fgTie("/autopilot/locks/nav[0]", this,
+	  &FGAutopilot::getAPNAV1Lock, &FGAutopilot::setAPNAV1Lock);
+    fgSetArchivable("/autopilot/locks/nav[0]");
+    fgTie("/autopilot/locks/auto-throttle", this,
+	  &FGAutopilot::getAPAutoThrottleLock,
+	  &FGAutopilot::setAPAutoThrottleLock);
+    fgSetArchivable("/autopilot/locks/auto-throttle");
+    fgTie("/autopilot/control-overrides/rudder", this,
+	  &FGAutopilot::getAPRudderControl,
+	  &FGAutopilot::setAPRudderControl);
+    fgSetArchivable("/autopilot/control-overrides/rudder");
+    fgTie("/autopilot/control-overrides/elevator", this,
+	  &FGAutopilot::getAPElevatorControl,
+	  &FGAutopilot::setAPElevatorControl);
+    fgSetArchivable("/autopilot/control-overrides/elevator");
+    fgTie("/autopilot/control-overrides/throttle", this,
+	  &FGAutopilot::getAPThrottleControl,
+	  &FGAutopilot::setAPThrottleControl);
+    fgSetArchivable("/autopilot/control-overrides/throttle");
+}
+
+void
+FGAutopilot::unbind ()
+{
+}
 
 // Reset the autopilot system
 void FGAutopilot::reset() {
@@ -173,57 +377,22 @@ void FGAutopilot::reset() {
     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;
 
-    TargetHeading = 0.0;	// default direction, due north
+    // TargetHeading = 0.0;	// default direction, due north
     MakeTargetHeadingStr( TargetHeading );			
 	
-    TargetAltitude = 3000;   // default altitude in meters
+    // TargetAltitude = 3000;   // default altitude in meters
     MakeTargetAltitudeStr( TargetAltitude );
 	
     alt_error_accum = 0.0;
+    climb_error_accum = 0.0;
 	
     update_old_control_values();
 
-    sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
+    sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id") );
 	
-    TargetLatitude = FGBFI::getLatitude();
-    TargetLongitude = FGBFI::getLongitude();
-    MakeTargetLatLonStr( TargetLatitude, TargetLongitude );
-}
-
-
-static inline double get_speed( void ) {
-    return( cur_fdm_state->get_V_equiv_kts() );
-}
-
-static inline double get_ground_speed() {
-    // starts in ft/s so we convert to kts
-    double ft_s = cur_fdm_state->get_V_ground_speed() 
-	* current_options.get_speed_up();;
-    double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
-
-    return kts;
-}
-
-
-void FGAutopilot::MakeTargetDistanceStr( double distance ) {
-    double eta = distance*METER_TO_NM / get_ground_speed();
-    if ( eta >= 100.0 ) { eta = 99.999; }
-    int major, minor;
-    if ( eta < (1.0/6.0) ) {
-	// within 10 minutes, bump up to min/secs
-	eta *= 60.0;
-    }
-    major = (int)eta;
-    minor = (int)((eta - (int)eta) * 60.0);
-    sprintf( TargetDistanceStr, "APDistance %.2f NM  ETA %d:%02d",
-	     distance*METER_TO_NM, major, minor );
-    // cout << "distance = " << distance*METER_TO_NM
-    //      << "  gndsp = " << get_ground_speed() 
-    //      << "  time = " << eta
-    //      << "  major = " << major
-    //      << "  minor = " << minor
-    //      << endl;
+    MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 }
 
 
@@ -231,13 +400,10 @@ static double NormalizeDegrees( double Input ) {
     // normalize the input to the range (-180,180]
     // Input should not be greater than -360 to 360.
     // Current rules send the output to an undefined state.
-    if ( Input > 180 )
-	while(Input > 180 )
-	    Input -= 360;
-    else if ( Input <= -180 )
-	while ( Input <= -180 )
-	    Input += 360;
-    return ( Input );
+    while ( Input > 180.0 ) { Input -= 360.0; }
+    while ( Input <= -180.0 ) { Input += 360.0; }
+
+    return Input;
 };
 
 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
@@ -247,7 +413,7 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub
 	// Could be
 	// static double y = 0.0;
 	// double dx = x2 -x1;
-	// if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
+	// if( (dx < -SG_EPSILON ) || ( dx > SG_EPSILON ) )
 	// {
 
     double m, b, y;          // the constants to find in y=mx+b
@@ -266,19 +432,27 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub
 };
 
 
-int FGAutopilot::run() {
+void
+FGAutopilot::update (double dt)
+{
     // Remove the following lines when the calling funcitons start
     // passing in the data pointer
 
     // get control settings 
-    // double aileron = FGBFI::getAileron();
-    // double elevator = FGBFI::getElevator();
-    // double elevator_trim = FGBFI::getElevatorTrim();
-    // double rudder = FGBFI::getRudder();
 	
-    double lat = FGBFI::getLatitude();
-    double lon = FGBFI::getLongitude();
-
+    double lat = latitude_node->getDoubleValue();
+    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 );
+	
 #ifdef FG_FORCE_AUTO_DISENGAGE
     // see if somebody else has changed them
     if( fabs(aileron - old_aileron) > disengage_threshold ||
@@ -304,15 +478,73 @@ int FGAutopilot::run() {
     }
 #endif
 	
-    // heading hold enabled?
+    // heading hold
     if ( heading_hold == true ) {
+	if ( heading_mode == FG_DG_HEADING_LOCK ) {
+	    TargetHeading = DGTargetHeading + FGSteam::get_DG_err();
+	    while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
+	    while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
+	    MakeTargetHeadingStr( TargetHeading );
+	} else if ( heading_mode == FG_TC_HEADING_LOCK ) {
+	    // we don't set a specific target heading in
+	    // TC_HEADING_LOCK mode, we instead try to keep the turn
+	    // coordinator zero'd
+	} else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
+	    // leave "true" target heading as is
+            while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
+            while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
+            MakeTargetHeadingStr( TargetHeading );
+	} else if ( heading_mode == FG_HEADING_NAV1 ) {
+	    // track the NAV1 heading needle deflection
+
+	    // determine our current radial position relative to the
+	    // navaid in "true" heading.
+	    double cur_radial = current_radiostack->get_nav1_heading();
+	    if ( current_radiostack->get_nav1_loc() ) {
+		// ILS localizers radials are already "true" in our
+		// database
+	    } else {
+		cur_radial += current_radiostack->get_nav1_magvar();
+	    }
+	    if ( current_radiostack->get_nav1_from_flag() ) {
+		cur_radial += 180.0;
+		while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
+	    }
+
+	    // determine the target radial in "true" heading
+	    double tgt_radial = current_radiostack->get_nav1_radial();
+	    if ( current_radiostack->get_nav1_loc() ) {
+		// ILS localizers radials are already "true" in our
+		// database
+	    } else {
+		// VOR radials need to have that vor's offset added in
+		tgt_radial += current_radiostack->get_nav1_magvar();
+	    }
 
-	if ( heading_mode == FG_HEADING_LOCK ) {
-	    // leave target heading alone
+	    // determine the heading adjustment needed.
+	    double adjustment = 
+		current_radiostack->get_nav1_heading_needle_deflection()
+		* (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; 
+	    while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
+	    while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
+
+	    MakeTargetHeadingStr( TargetHeading );
 	} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 	    // update target heading to waypoint
 
-	    double wp_course, wp_reverse, wp_distance;
+	    double wp_course, wp_distance;
 
 #ifdef DO_fgAP_CORRECTED_COURSE
 	    // compute course made good
@@ -331,159 +563,198 @@ int FGAutopilot::run() {
 
 	    // compute course to way_point
 	    // need to test for iter
-	    if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(),
-				      lat,
-				      lon,
-				      TargetLatitude,
-				      TargetLongitude,
-				      &wp_course,
-				      &wp_reverse,
-				      &wp_distance ) ) {
-		
+	    SGWayPoint wp = globals->get_route()->get_first();
+	    wp.CourseAndDistance( lon, lat, alt, 
+				  &wp_course, &wp_distance );
+
 #ifdef DO_fgAP_CORRECTED_COURSE
-		corrected_course = course - wp_course;
-		if( fabs(corrected_course) > 0.1 )
-		    printf("fgAP: course %f  wp_course %f  %f  %f\n",
-			   course, wp_course, fabs(corrected_course),
-			   distance );
+	    corrected_course = course - wp_course;
+	    if( fabs(corrected_course) > 0.1 )
+		printf("fgAP: course %f  wp_course %f  %f  %f\n",
+		       course, wp_course, fabs(corrected_course),
+		       distance );
 #endif // DO_fgAP_CORRECTED_COURSE
 		
-		if ( wp_distance > 100 ) {
-		    // corrected_course = course - wp_course;
-		    TargetHeading = NormalizeDegrees(wp_course);
+	    if ( wp_distance > 100 ) {
+		// corrected_course = course - wp_course;
+		TargetHeading = NormalizeDegrees(wp_course);
+	    } else {
+		// pop off this waypoint from the list
+		if ( globals->get_route()->size() ) {
+		    globals->get_route()->delete_first();
+		}
+
+		// see if there are more waypoints on the list
+		if ( globals->get_route()->size() ) {
+		    // more waypoints
+		    set_HeadingMode( FG_HEADING_WAYPOINT );
 		} else {
-		    printf("distance(%f) to close\n", wp_distance);
-		    // Real Close -- set heading hold to current heading
-		    // and Ring the arival bell !!
-		    heading_mode = FG_HEADING_LOCK;
+		    // end of the line
+		    heading_mode = FG_TRUE_HEADING_LOCK;
 		    // use current heading
-		    TargetHeading = FGBFI::getHeading();
+		    TargetHeading = heading_node->getDoubleValue();
 		}
-		MakeTargetHeadingStr( TargetHeading );
-		// Force this just in case
-		TargetDistance = wp_distance;
-		MakeTargetDistanceStr( wp_distance );
 	    }
+	    MakeTargetHeadingStr( TargetHeading );
+	    // Force this just in case
+	    TargetDistance = wp_distance;
+	    MakeTargetWPStr( wp_distance );
 	}
 
-	double RelHeading;
-	double TargetRoll;
-	double RelRoll;
-	double AileronSet;
-
-	RelHeading = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
-	// figure out how far off we are from desired heading
-
-	// Now it is time to deterime how far we should be rolled.
-	FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
-
-
-	// Check if we are further from heading than the roll out point
-	if ( fabs( RelHeading ) > RollOut ) {
-	    // set Target Roll to Max in desired direction
-	    if ( RelHeading < 0 ) {
-		TargetRoll = 0 - MaxRoll;
-	    } else {
-		TargetRoll = MaxRoll;
-	    }
+	if ( heading_mode == FG_TC_HEADING_LOCK ) {
+	    // drive the turn coordinator to zero
+	    double turn = FGSteam::get_TC_std();
+	    double AileronSet = -turn / 2.0;
+            SG_CLAMP_RANGE( AileronSet, -1.0, 1.0 );
+	    globals->get_controls()->set_aileron( AileronSet );
+	    globals->get_controls()->set_rudder( AileronSet / 4.0 );
 	} else {
-	    // We have to calculate the Target roll
+	    // steer towards the target heading
+
+	    double RelHeading;
+	    double TargetRoll;
+	    double RelRoll;
+	    double AileronSet;
+
+	    RelHeading
+		= NormalizeDegrees( TargetHeading
+				    - heading_node->getDoubleValue() );
+	    // 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,
+                    "Heading = " << heading_node->getDoubleValue() <<
+                    " TargetHeading = " << TargetHeading <<
+                    " RelHeading = " << RelHeading );
+
+	    // Check if we are further from heading than the roll out point
+	    if ( fabs( RelHeading ) > RollOut ) {
+		// set Target Roll to Max in desired direction
+		if ( RelHeading < 0 ) {
+		    TargetRoll = 0 - MaxRoll;
+		} else {
+		    TargetRoll = MaxRoll;
+		}
+	    } else {
+		// We have to calculate the Target roll
 
-	    // This calculation engine thinks that the Target roll
-	    // should be a line from (RollOut,MaxRoll) to (-RollOut,
-	    // -MaxRoll) I hope this works well.  If I get ambitious
-	    // some day this might become a fancier curve or
-	    // something.
+		// This calculation engine thinks that the Target roll
+		// should be a line from (RollOut,MaxRoll) to (-RollOut,
+		// -MaxRoll) I hope this works well.  If I get ambitious
+		// some day this might become a fancier curve or
+		// something.
 
-	    TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
-					    -MaxRoll, RollOut,
-					    MaxRoll );
-	}
+		TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
+						-MaxRoll, RollOut,
+						MaxRoll );
+	    }
 
-	// Target Roll has now been Found.
+	    // Target Roll has now been Found.
 
-	// Compare Target roll to Current Roll, Generate Rel Roll
+	    // Compare Target roll to Current Roll, Generate Rel Roll
 
-	FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
+	    SG_LOG( SG_COCKPIT, SG_BULK, "TargetRoll: " << TargetRoll );
 
-	RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() );
+	    RelRoll = NormalizeDegrees( TargetRoll 
+					- roll_node->getDoubleValue() );
 
-	// Check if we are further from heading than the roll out smooth point
-	if ( fabs( RelRoll ) > RollOutSmooth ) {
-	    // set Target Roll to Max in desired direction
-	    if ( RelRoll < 0 ) {
+	    // Check if we are further from heading than the roll out
+	    // smooth point
+	    if ( fabs( RelRoll ) > RollOutSmooth ) {
+		// set Target Roll to Max in desired direction
+		if ( RelRoll < 0 ) {
 		AileronSet = 0 - MaxAileron;
+		} else {
+		    AileronSet = MaxAileron;
+		}
 	    } else {
-		AileronSet = MaxAileron;
+		AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
+						-MaxAileron,
+						RollOutSmooth,
+						MaxAileron );
 	    }
-	} else {
-	    AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
-					    -MaxAileron,
-					    RollOutSmooth,
-					    MaxAileron );
-	}
 
-	controls.set_aileron( AileronSet );
-	controls.set_rudder( AileronSet / 4.0 );
-	// controls.set_rudder( 0.0 );
+	    globals->get_controls()->set_aileron( AileronSet );
+	    globals->get_controls()->set_rudder( AileronSet / 4.0 );
+	    // controls.set_rudder( 0.0 );
+	}
     }
 
-    // altitude hold?
+    // altitude hold
     if ( altitude_hold ) {
+	double climb_rate;
 	double speed, max_climb, error;
 	double prop_error, int_error;
 	double prop_adj, int_adj, total_adj;
 
 	if ( altitude_mode == FG_ALTITUDE_LOCK ) {
-	    // normal altitude hold
-	    // cout << "TargetAltitude = " << TargetAltitude
-	    //      << "Altitude = " << FGBFI::getAltitude() * FEET_TO_METER
-	    //      << endl;
-	    TargetClimbRate =
-		( TargetAltitude - FGBFI::getAltitude() * FEET_TO_METER ) * 8.0;
+	    climb_rate =
+		( TargetAltitude - FGSteam::get_ALT_ft() * SG_FEET_TO_METER ) * 8.0;
+	} else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
+	    double x = current_radiostack->get_nav1_gs_dist();
+	    double y = (altitude_node->getDoubleValue()
+			- current_radiostack->get_nav1_elev()) * SG_FEET_TO_METER;
+	    double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
+
+	    double target_angle = current_radiostack->get_nav1_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;
+	    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; */
 	} else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 	    // brain dead ground hugging with no look ahead
-	    TargetClimbRate =
-		( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0;
-	    // cout << "target agl = " << TargetAGL 
-	    //      << "  current agl = " << fgAPget_agl() 
-	    //      << "  target climb rate = " << TargetClimbRate 
-	    //      << endl;
+	    climb_rate =
+		( TargetAGL - altitude_agl_node->getDoubleValue()
+                  * SG_FEET_TO_METER ) * 16.0;
 	} else {
 	    // just try to zero out rate of climb ...
-	    TargetClimbRate = 0.0;
+	    climb_rate = 0.0;
 	}
 
 	speed = get_speed();
 
-	if ( speed < min_climb ) {
+	if ( speed < min_climb->getFloatValue() ) {
 	    max_climb = 0.0;
-	} else if ( speed < best_climb ) {
-	    max_climb = ((best_climb - min_climb) - (best_climb - speed)) 
-		* ideal_climb_rate 
-		/ (best_climb - min_climb);
+	} else if ( speed < best_climb->getFloatValue() ) {
+	    max_climb = ((best_climb->getFloatValue()
+                          - min_climb->getFloatValue())
+                         - (best_climb->getFloatValue() - speed)) 
+		* fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) 
+		/ (best_climb->getFloatValue() - min_climb->getFloatValue());
 	} else {			
-	    max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
+	    max_climb = ( speed - best_climb->getFloatValue() ) * 10.0
+                + fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
 	}
 
 	// this first one could be optional if we wanted to allow
 	// better climb performance assuming we have the airspeed to
 	// support it.
-	if ( TargetClimbRate > ideal_climb_rate ) {
-	    TargetClimbRate = ideal_climb_rate;
+	if ( climb_rate >
+             fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) ) {
+	    climb_rate
+                = fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
 	}
 
-	if ( TargetClimbRate > max_climb ) {
-	    TargetClimbRate = max_climb;
+	if ( climb_rate > max_climb ) {
+	    climb_rate = max_climb;
 	}
 
-	if ( TargetClimbRate < -ideal_climb_rate ) {
-	    TargetClimbRate = -ideal_climb_rate;
+	if ( climb_rate <
+             -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER) ) {
+	    climb_rate
+                = -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER);
 	}
 
-	error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate;
-	// cout << "climb rate = " << fgAPget_climb() 
-	//      << "  error = " << error << endl;
+	error = vertical_speed_node->getDoubleValue() * 60
+            - climb_rate * SG_METER_TO_FEET;
 
 	// accumulate the error under the curve ... this really should
 	// be *= delta t
@@ -492,28 +763,37 @@ 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;
+        
+        // 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 / 2000.0;
+	prop_adj = prop_error / elev_adj_factor;
 
-	total_adj = 0.9 * prop_adj + 0.1 * 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;
-	}
+	total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj
+            + (double) integral_contrib->getFloatValue() * int_adj;
 
-	controls.set_elevator( total_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();
+
+	globals->get_controls()->set_elevator_trim( total_adj );
     }
 
-    // auto throttle enabled?
+    // auto throttle
     if ( auto_throttle ) {
 	double error;
 	double prop_error, int_error;
@@ -549,7 +829,8 @@ int FGAutopilot::run() {
 	    total_adj = 0.0;
 	}
 
-	controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
+	globals->get_controls()->set_throttle( FGControls::ALL_ENGINES,
+					       total_adj );
     }
 
 #ifdef THIS_CODE_IS_NOT_USED
@@ -581,53 +862,69 @@ int FGAutopilot::run() {
 
     // stash this runs control settings
     //	update_old_control_values();
-    old_aileron = controls.get_aileron();
-    old_elevator = controls.get_elevator();
-    old_elevator_trim = controls.get_elevator_trim();
-    old_rudder = controls.get_rudder();
+    old_aileron = globals->get_controls()->get_aileron();
+    old_elevator = globals->get_controls()->get_elevator();
+    old_elevator_trim = globals->get_controls()->get_elevator_trim();
+    old_rudder = globals->get_controls()->get_rudder();
 
     // for cross track error
     old_lat = lat;
     old_lon = lon;
 	
-	// Ok, we are done
-    return 0;
+    // Ok, we are done
+    SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run( returns )" );
 }
 
 
 void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
     heading_mode = mode;
 
-    if ( heading_mode == FG_HEADING_LOCK ) {
+    if ( heading_mode == FG_DG_HEADING_LOCK ) {
+	// set heading hold to current heading (as read from DG)
+	// ... no, leave target heading along ... just use the current
+	// heading bug value
+        //  DGTargetHeading = FGSteam::get_DG_deg();
+    } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
+	// set autopilot to hold a zero turn (as reported by the TC)
+    } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
 	// set heading hold to current heading
-	TargetHeading = FGBFI::getHeading();
+	TargetHeading = heading_node->getDoubleValue();
     } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
-	double course, reverse, distance;
-	// turn on location hold
-	// turn on heading hold
-	old_lat = FGBFI::getLatitude();
-	old_lon = FGBFI::getLongitude();
-			
-	// need to test for iter
-	if( !geo_inverse_wgs_84( FGBFI::getAltitude() * FEET_TO_METER,
-				 FGBFI::getLatitude(),
-				 FGBFI::getLongitude(),
-				 TargetLatitude,
-				 TargetLongitude,
-				 &course,
-				 &reverse,
-				 &distance ) ) {
+	if ( globals->get_route()->size() ) {
+	    double course, distance;
+
+	    old_lat = latitude_node->getDoubleValue();
+	    old_lon = longitude_node->getDoubleValue();
+
+	    waypoint = globals->get_route()->get_first();
+	    waypoint.CourseAndDistance( longitude_node->getDoubleValue(),
+					latitude_node->getDoubleValue(),
+					altitude_node->getDoubleValue()
+                                        * SG_FEET_TO_METER,
+					&course, &distance );
 	    TargetHeading = course;
 	    TargetDistance = distance;
-	    MakeTargetDistanceStr( distance );
-	}
+	    MakeTargetLatLonStr( waypoint.get_target_lat(),
+				 waypoint.get_target_lon() );
+	    MakeTargetWPStr( distance );
+
+	    if ( waypoint.get_target_alt() > 0.0 ) {
+		TargetAltitude = waypoint.get_target_alt();
+		altitude_mode = FG_ALTITUDE_LOCK;
+		set_AltitudeEnabled( true );
+		MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
+	    }
 
-	FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
-		<< TargetLatitude  << " "
-		<< TargetLongitude << " ) "
-		);
+	    SG_LOG( SG_COCKPIT, SG_INFO, " set_HeadingMode: ( "
+		    << get_TargetLatitude()  << " "
+		    << get_TargetLongitude() << " ) "
+		    );
+	} else {
+	    // no more way points, default to heading lock.
+	    heading_mode = FG_TC_HEADING_LOCK;
+	}
     }
-	
+
     MakeTargetHeadingStr( TargetHeading );			
     update_old_control_values();
 }
@@ -636,101 +933,53 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
     altitude_mode = mode;
 
+    alt_error_accum = 0.0;
+
+
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
-	// lock at current altitude
-	TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
-	alt_error_accum = 0.0;
+	if ( TargetAltitude < altitude_agl_node->getDoubleValue()
+             * SG_FEET_TO_METER ) {
+	}
 
-	if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	    MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
+	if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	    MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 	} else {
-	    MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
+	    MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 	}
+    } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
+	climb_error_accum = 0.0;
+
     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
-	TargetAGL = FGBFI::getAGL() * FEET_TO_METER;
-	alt_error_accum = 0.0;
+	TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER;
 
-	if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	    MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
+	if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	    MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
 	} else {
-	    MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
+	    MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
 	}
     }
     
     update_old_control_values();
-    FG_LOG( FG_COCKPIT, FG_INFO, " set_AltitudeMode():" );
-}
-
-
-#if 0
-static inline double get_aoa( void ) {
-    return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
-}
-
-static inline double fgAPget_latitude( void ) {
-    return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
-}
-
-static inline double fgAPget_longitude( void ) {
-    return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
-}
-
-static inline double fgAPget_roll( void ) {
-    return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
-}
-
-static inline double get_pitch( void ) {
-    return( cur_fdm_state->get_Theta() );
-}
-
-double fgAPget_heading( void ) {
-    return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
+    SG_LOG( SG_COCKPIT, SG_INFO, " set_AltitudeMode():" );
 }
 
-static inline double fgAPget_altitude( void ) {
-    return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
-}
-
-static inline double fgAPget_climb( void ) {
-    // return in meters per minute
-    return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
-}
-
-static inline double get_sideslip( void ) {
-    return( cur_fdm_state->get_Beta() );
-}
-
-static inline double fgAPget_agl( void ) {
-    double agl;
-
-    agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
-	- scenery.cur_elev;
-
-    return( agl );
-}
-#endif
-
 
 void FGAutopilot::AltitudeSet( double new_altitude ) {
     double target_alt = new_altitude;
 
-    // cout << "new altitude = " << new_altitude << endl;
-
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	target_alt = new_altitude * FEET_TO_METER;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	target_alt = new_altitude * SG_FEET_TO_METER;
     }
 
-    if( target_alt < scenery.cur_elev ) {
-	target_alt = scenery.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;
 
-    // cout << "TargetAltitude = " << TargetAltitude << endl;
-
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	target_alt *= METER_TO_FEET;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	target_alt *= SG_METER_TO_FEET;
     }
     // ApAltitudeDialogInput->setValue((float)target_alt);
     MakeTargetAltitudeStr( target_alt );
@@ -743,29 +992,38 @@ void FGAutopilot::AltitudeAdjust( double inc )
 {
     double target_alt, target_agl;
 
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	target_alt = TargetAltitude * METER_TO_FEET;
-	target_agl = TargetAGL * METER_TO_FEET;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	target_alt = TargetAltitude * SG_METER_TO_FEET;
+	target_agl = TargetAGL * SG_METER_TO_FEET;
     } else {
 	target_alt = TargetAltitude;
 	target_agl = TargetAGL;
     }
 
-    target_alt = ( int ) ( target_alt / inc ) * inc + inc;
-    target_agl = ( int ) ( target_agl / inc ) * inc + inc;
+    if ( fabs((int)(target_alt / inc) * inc - target_alt) < SG_EPSILON ) {
+	target_alt += inc;
+    } else {
+	target_alt = ( int ) ( target_alt / inc ) * inc + inc;
+    }
+
+    if ( fabs((int)(target_agl / inc) * inc - target_agl) < SG_EPSILON ) {
+	target_agl += inc;
+    } else {
+	target_agl = ( int ) ( target_agl / inc ) * inc + inc;
+    }
 
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
-	target_alt *= FEET_TO_METER;
-	target_agl *= FEET_TO_METER;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
+	target_alt *= SG_FEET_TO_METER;
+	target_agl *= SG_FEET_TO_METER;
     }
 
     TargetAltitude = target_alt;
     TargetAGL = target_agl;
 	
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
-	target_alt *= METER_TO_FEET;
-    if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
-	target_agl *= METER_TO_FEET;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
+	target_alt *= SG_METER_TO_FEET;
+    if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
+	target_agl *= SG_METER_TO_FEET;
 
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 	MakeTargetAltitudeStr( target_alt );
@@ -778,26 +1036,38 @@ void FGAutopilot::AltitudeAdjust( double inc )
 
 
 void FGAutopilot::HeadingAdjust( double inc ) {
-    heading_mode = FG_HEADING_LOCK;
-	
-    double target = ( int ) ( TargetHeading / inc ) * inc + inc;
+    if ( heading_mode != FG_DG_HEADING_LOCK
+	 && heading_mode != FG_TRUE_HEADING_LOCK )
+    {
+	heading_mode = FG_DG_HEADING_LOCK;
+    }
+
+    if ( heading_mode == FG_DG_HEADING_LOCK ) {
+	double target = ( int ) ( DGTargetHeading / inc ) * inc + inc;
+	DGTargetHeading = NormalizeDegrees( target );
+    } else {
+	double target = ( int ) ( TargetHeading / inc ) * inc + inc;
+	TargetHeading = NormalizeDegrees( target );
+    }
 
-    TargetHeading = NormalizeDegrees( target );
-    // following cast needed ambiguous plib
-    // ApHeadingDialogInput -> setValue ((float)TargetHeading );
-    MakeTargetHeadingStr( TargetHeading );			
     update_old_control_values();
 }
 
 
 void FGAutopilot::HeadingSet( double new_heading ) {
-    heading_mode = FG_HEADING_LOCK;
-	
-    new_heading = NormalizeDegrees( new_heading );
-    TargetHeading = new_heading;
-    // following cast needed ambiguous plib
-    // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
-    MakeTargetHeadingStr( TargetHeading );			
+    if( heading_mode == FG_TRUE_HEADING_LOCK ) {
+        new_heading = NormalizeDegrees( new_heading );
+        TargetHeading = new_heading;
+        MakeTargetHeadingStr( TargetHeading );
+    } else {
+        heading_mode = FG_DG_HEADING_LOCK;
+
+        new_heading = NormalizeDegrees( new_heading );
+        DGTargetHeading = new_heading;
+        // following cast needed ambiguous plib
+        // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
+        MakeTargetHeadingStr( DGTargetHeading );
+    }
     update_old_control_values();
 }
 
@@ -811,12 +1081,321 @@ void FGAutopilot::AutoThrottleAdjust( double inc ) {
 void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
     auto_throttle = value;
 
-    if ( auto_throttle = true ) {
-	TargetSpeed = FGBFI::getAirspeed();
+    if ( auto_throttle == true ) {
+        TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
 	speed_error_accum = 0.0;
     }
 
     update_old_control_values();
-    FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
+    SG_LOG( SG_COCKPIT, SG_INFO, " fgAPSetAutoThrottle: ("
 	    << auto_throttle << ") " << TargetSpeed );
 }
+
+
+
+
+////////////////////////////////////////////////////////////////////////
+// Kludged methods for tying to properties.
+//
+// These should change eventually; they all used to be static
+// functions.
+////////////////////////////////////////////////////////////////////////
+
+/**
+ * Get the autopilot altitude lock (true=on).
+ */
+bool
+FGAutopilot::getAPAltitudeLock () const
+{
+    return (get_AltitudeEnabled() &&
+	    get_AltitudeMode()
+	    == FGAutopilot::FG_ALTITUDE_LOCK);
+}
+
+
+/**
+ * Set the autopilot altitude lock (true=on).
+ */
+void
+FGAutopilot::setAPAltitudeLock (bool lock)
+{
+  if (lock)
+    set_AltitudeMode(FGAutopilot::FG_ALTITUDE_LOCK);
+  if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_LOCK)
+    set_AltitudeEnabled(lock);
+}
+
+
+/**
+ * Get the autopilot target altitude in feet.
+ */
+double
+FGAutopilot::getAPAltitude () const
+{
+  return get_TargetAltitude() * SG_METER_TO_FEET;
+}
+
+
+/**
+ * Set the autopilot target altitude in feet.
+ */
+void
+FGAutopilot::setAPAltitude (double altitude)
+{
+  set_TargetAltitude( altitude * SG_FEET_TO_METER );
+}
+
+/**
+ * Get the autopilot altitude lock (true=on).
+ */
+bool
+FGAutopilot::getAPGSLock () const
+{
+    return (get_AltitudeEnabled() &&
+	    (get_AltitudeMode()
+	     == FGAutopilot::FG_ALTITUDE_GS1));
+}
+
+
+/**
+ * Set the autopilot altitude lock (true=on).
+ */
+void
+FGAutopilot::setAPGSLock (bool lock)
+{
+  if (lock)
+    set_AltitudeMode(FGAutopilot::FG_ALTITUDE_GS1);
+  if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_GS1)
+    set_AltitudeEnabled(lock);
+}
+
+
+/**
+ * Get the autopilot terrain lock (true=on).
+ */
+bool
+FGAutopilot::getAPTerrainLock () const
+{
+    return (get_AltitudeEnabled() &&
+	    (get_AltitudeMode()
+	     == FGAutopilot::FG_ALTITUDE_TERRAIN));
+}
+
+
+/**
+ * Set the autopilot terrain lock (true=on).
+ */
+void
+FGAutopilot::setAPTerrainLock (bool lock)
+{
+  if (lock) {
+    set_AltitudeMode(FGAutopilot::FG_ALTITUDE_TERRAIN);
+    set_TargetAGL(fgGetFloat("/position/altitude-agl-ft") *
+		  SG_FEET_TO_METER);
+  }
+  if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_TERRAIN)
+    set_AltitudeEnabled(lock);
+}
+
+
+/**
+ * Get the autopilot target altitude in feet.
+ */
+double
+FGAutopilot::getAPClimb () const
+{
+  return get_TargetClimbRate() * SG_METER_TO_FEET;
+}
+
+
+/**
+ * Set the autopilot target altitude in feet.
+ */
+void
+FGAutopilot::setAPClimb (double rate)
+{
+    set_TargetClimbRate( rate * SG_FEET_TO_METER );
+}
+
+
+/**
+ * Get the autopilot heading lock (true=on).
+ */
+bool
+FGAutopilot::getAPHeadingLock () const
+{
+    return
+      (get_HeadingEnabled() &&
+       get_HeadingMode() == DEFAULT_AP_HEADING_LOCK);
+}
+
+
+/**
+ * Set the autopilot heading lock (true=on).
+ */
+void
+FGAutopilot::setAPHeadingLock (bool lock)
+{
+  if (lock)
+    set_HeadingMode(DEFAULT_AP_HEADING_LOCK);
+  if (get_HeadingMode() == DEFAULT_AP_HEADING_LOCK)
+    set_HeadingEnabled(lock);
+}
+
+
+/**
+ * Get the autopilot heading bug in degrees.
+ */
+double
+FGAutopilot::getAPHeadingBug () const
+{
+  return get_DGTargetHeading();
+}
+
+
+/**
+ * Set the autopilot heading bug in degrees.
+ */
+void
+FGAutopilot::setAPHeadingBug (double heading)
+{
+  set_DGTargetHeading( heading );
+}
+
+
+/**
+ * Get the autopilot wing leveler lock (true=on).
+ */
+bool
+FGAutopilot::getAPWingLeveler () const
+{
+    return
+      (get_HeadingEnabled() &&
+       get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK);
+}
+
+
+/**
+ * Set the autopilot wing leveler lock (true=on).
+ */
+void
+FGAutopilot::setAPWingLeveler (bool lock)
+{
+    if (lock)
+	set_HeadingMode(FGAutopilot::FG_TC_HEADING_LOCK);
+    if (get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK)
+      set_HeadingEnabled(lock);
+}
+
+/**
+ * Return true if the autopilot is locked to NAV1.
+ */
+bool
+FGAutopilot::getAPNAV1Lock () const
+{
+  return
+    (get_HeadingEnabled() &&
+     get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1);
+}
+
+
+/**
+ * Set the autopilot NAV1 lock.
+ */
+void
+FGAutopilot::setAPNAV1Lock (bool lock)
+{
+  if (lock)
+    set_HeadingMode(FGAutopilot::FG_HEADING_NAV1);
+  if (get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1)
+    set_HeadingEnabled(lock);
+}
+
+/**
+ * Get the autopilot autothrottle lock.
+ */
+bool
+FGAutopilot::getAPAutoThrottleLock () const
+{
+  return get_AutoThrottleEnabled();
+}
+
+
+/**
+ * Set the autothrottle lock.
+ */
+void
+FGAutopilot::setAPAutoThrottleLock (bool lock)
+{
+  set_AutoThrottleEnabled(lock);
+}
+
+
+// kludge
+double
+FGAutopilot::getAPRudderControl () const
+{
+    if (getAPHeadingLock())
+        return get_TargetHeading();
+    else
+        return globals->get_controls()->get_rudder();
+}
+
+// kludge
+void
+FGAutopilot::setAPRudderControl (double value)
+{
+    if (getAPHeadingLock()) {
+        SG_LOG(SG_GENERAL, SG_DEBUG, "setAPRudderControl " << value );
+        value -= get_TargetHeading();
+        HeadingAdjust(value < 0.0 ? -1.0 : 1.0);
+    } else {
+        globals->get_controls()->set_rudder(value);
+    }
+}
+
+// kludge
+double
+FGAutopilot::getAPElevatorControl () const
+{
+  if (getAPAltitudeLock())
+      return get_TargetAltitude();
+  else
+    return globals->get_controls()->get_elevator();
+}
+
+// kludge
+void
+FGAutopilot::setAPElevatorControl (double value)
+{
+    if (value != 0 && getAPAltitudeLock()) {
+        SG_LOG(SG_GENERAL, SG_DEBUG, "setAPElevatorControl " << value );
+        value -= get_TargetAltitude();
+        AltitudeAdjust(value < 0.0 ? 100.0 : -100.0);
+    } else {
+        globals->get_controls()->set_elevator(value);
+    }
+}
+
+// kludge
+double
+FGAutopilot::getAPThrottleControl () const
+{
+  if (getAPAutoThrottleLock())
+    return 0.0;			// always resets
+  else
+    return globals->get_controls()->get_throttle(0);
+}
+
+// kludge
+void
+FGAutopilot::setAPThrottleControl (double value)
+{
+  if (getAPAutoThrottleLock())
+    AutoThrottleAdjust(value < 0.0 ? -0.01 : 0.01);
+  else
+    globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
+}
+
+// end of newauto.cxx
+