X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.cxx;h=a54eaf8fe67ea72395271e73d06fee02a0ea90f6;hb=1d35fab8134a32dcbe3f0cedb29ddf24f7b21175;hp=d32ee9aebb6ef2cdac6d09b98ac7a5176d6049ab;hpb=c210a7806719f9f20fb41ac66ce13e4b4d7cd424;p=flightgear.git

diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx
index d32ee9aeb..a54eaf8fe 100644
--- a/src/Autopilot/newauto.cxx
+++ b/src/Autopilot/newauto.cxx
@@ -32,12 +32,12 @@
 #include <simgear/constants.h>
 #include <simgear/debug/logstream.hxx>
 #include <simgear/math/sg_geodesy.hxx>
+#include <simgear/math/sg_random.h>
 
 #include <Cockpit/steam.hxx>
 #include <Cockpit/radiostack.hxx>
 #include <Controls/controls.hxx>
 #include <FDM/flight.hxx>
-#include <Main/bfi.hxx>
 #include <Main/globals.hxx>
 #include <Scenery/scenery.hxx>
 
@@ -50,7 +50,8 @@ 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 * FEET_TO_METER; // fpm -> mpm
+// const double ideal_climb_rate = 500.0 * SG_FEET_TO_METER; // fpm -> mpm
+// const double ideal_decent_rate = 1000.0 * SG_FEET_TO_METER; // fpm -> mpm
 
 /// These statics will eventually go into the class
 /// they are just here while I am experimenting -- NHV :-)
@@ -67,6 +68,17 @@ extern char *coord_format_lat(float);
 extern char *coord_format_lon(float);
 			
 
+// constructor
+FGAutopilot::FGAutopilot():
+TargetClimbRate(500 * SG_FEET_TO_METER),
+TargetDecentRate(1000 * SG_FEET_TO_METER)
+{
+}
+
+// destructor
+FGAutopilot::~FGAutopilot() {}
+
+
 void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
     sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
     sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
@@ -99,9 +111,11 @@ static inline double get_speed( void ) {
 
 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() 
-      * fgGetInt("/sim/speed-up"); // FIXME: inefficient
-    double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
+        * speedup_node->getIntValue();
+    double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
 
     return kts;
 }
@@ -129,7 +143,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
     if ( size > 0 ) {
 	SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 );
 	accum += distance;
-	double eta = accum * METER_TO_NM / get_ground_speed();
+	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) ) {
@@ -140,8 +154,8 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
 	minor = (int)((eta - (int)eta) * 60.0);
 	sprintf( TargetWP1Str, "%s %.2f NM  ETA %d:%02d",
 		 wp1.get_id().c_str(),
-		 accum*METER_TO_NM, major, minor );
-	// cout << "distance = " << distance*METER_TO_NM
+		 accum*SG_METER_TO_NM, major, minor );
+	// cout << "distance = " << distance*SG_METER_TO_NM
 	//      << "  gndsp = " << get_ground_speed() 
 	//      << "  time = " << eta
 	//      << "  major = " << major
@@ -154,7 +168,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
 	SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 );
 	accum += wp2.get_distance();
 	
-	double eta = accum * METER_TO_NM / get_ground_speed();
+	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) ) {
@@ -165,7 +179,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
 	minor = (int)((eta - (int)eta) * 60.0);
 	sprintf( TargetWP2Str, "%s %.2f NM  ETA %d:%02d",
 		 wp2.get_id().c_str(),
-		 accum*METER_TO_NM, major, minor );
+		 accum*SG_METER_TO_NM, major, minor );
     }
 
     // next route
@@ -176,7 +190,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
 	
 	SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 );
 
-	double eta = accum * METER_TO_NM / get_ground_speed();
+	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) ) {
@@ -187,30 +201,42 @@ void FGAutopilot::MakeTargetWPStr( double distance ) {
 	minor = (int)((eta - (int)eta) * 60.0);
 	sprintf( TargetWP3Str, "%s %.2f NM  ETA %d:%02d",
 		 wpn.get_id().c_str(),
-		 accum*METER_TO_NM, major, minor );
+		 accum*SG_METER_TO_NM, major, minor );
     }
 }
 
 
 void FGAutopilot::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();
 }
 
 
 // Initialize autopilot subsystem
 void FGAutopilot::init() {
-    FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
+    SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" );
+
+    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);
 
     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;
+
+    sg_srandom_time();
+    DGTargetHeading = sg_random() * 360.0;
 
     // Initialize target location to startup location
-    old_lat = FGBFI::getLatitude();
-    old_lon = FGBFI::getLongitude();
+    old_lat = latitude_node->getDoubleValue();
+    old_lon = longitude_node->getDoubleValue();
     // set_WayPoint( old_lon, old_lat, 0.0, "default" );
 
     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
@@ -220,8 +246,8 @@ void FGAutopilot::init() {
     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.
 
@@ -264,11 +290,12 @@ 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;
@@ -278,10 +305,6 @@ void FGAutopilot::reset() {
 
     sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id").c_str() );
 	
-    // TargetLatitude = FGBFI::getLatitude();
-    // TargetLongitude = FGBFI::getLongitude();
-    // set_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" );
-
     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 }
 
@@ -306,7 +329,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
@@ -330,15 +353,14 @@ int FGAutopilot::run() {
     // 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 alt = FGBFI::getAltitude() * FEET_TO_METER;
+    double lat = latitude_node->getDoubleValue();
+    double lon = longitude_node->getDoubleValue();
+    double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 
+    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 ||
@@ -374,51 +396,55 @@ int FGAutopilot::run() {
 	    while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
 	    while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
 	    MakeTargetHeadingStr( TargetHeading );
-	} else if ( heading_mode == FG_HEADING_LOCK ) {
-	    // leave target heading alone
+	} 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 ) {
-	    if ( current_radiostack->get_nav1_to_flag() ||
-		 current_radiostack->get_nav1_from_flag() ) {
-		// We have an appropriate radial selected that the
-		// autopilot can follow
-		double tgt_radial;
-		double cur_radial;
-		if ( current_radiostack->get_nav1_loc() ) {
-		    // localizers radials are "true"
-		    tgt_radial = current_radiostack->get_nav1_radial();
-		} else {
-		    tgt_radial = current_radiostack->get_nav1_radial() +
-			current_radiostack->get_nav1_magvar();
-		}
-		cur_radial = current_radiostack->get_nav1_heading() +
-		    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; }
-		}
-		// cout << "target rad (true) = " << tgt_radial 
-		//      << "  current rad (true) = " << cur_radial
-		//      << endl;
+	    // 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; }
+	    }
 
-		double diff = (tgt_radial - cur_radial);
-		while ( diff < -180.0 ) { diff += 360.0; }
-		while ( diff > 180.0 ) { diff -= 360.0; }
-		
-		diff *= (current_radiostack->get_nav1_loc_dist() * METER_TO_NM);
-		if ( diff < -30.0 ) { diff = -30.0; }
-		if ( diff >  30.0 ) { diff =  30.0; }
-
-		if ( current_radiostack->get_nav1_to_flag() ) {
-		    TargetHeading = cur_radial - diff;
-		} else if ( current_radiostack->get_nav1_from_flag() ) {
-		    TargetHeading = cur_radial + diff;
-		}
-		while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
-		while ( TargetHeading > 360.0 ) { TargetHeading -= 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 {
-		// neither TO, or FROM, maintain current heading.
-		TargetHeading = FGBFI::getHeading();
+		// VOR radials need to have that vor's offset added in
+		tgt_radial += current_radiostack->get_nav1_magvar();
 	    }
+
+	    // determine the heading adjustment needed.
+	    double adjustment = 
+		current_radiostack->get_nav1_heading_needle_deflection()
+		* (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM);
+	    if ( adjustment < -30.0 ) { adjustment = -30.0; }
+	    if ( adjustment >  30.0 ) { adjustment =  30.0; }
+
+	    // 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 );
 	    // cout << "target course (true) = " << TargetHeading << endl;
 	} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
@@ -473,9 +499,9 @@ int FGAutopilot::run() {
 		    set_HeadingMode( FG_HEADING_WAYPOINT );
 		} else {
 		    // end of the line
-		    heading_mode = FG_HEADING_LOCK;
+		    heading_mode = FG_TRUE_HEADING_LOCK;
 		    // use current heading
-		    TargetHeading = FGBFI::getHeading();
+		    TargetHeading = heading_node->getDoubleValue();
 		}
 	    }
 	    MakeTargetHeadingStr( TargetHeading );
@@ -484,86 +510,100 @@ int FGAutopilot::run() {
 	    MakeTargetWPStr( wp_distance );
 	}
 
-	double RelHeading;
-	double TargetRoll;
-	double RelRoll;
-	double AileronSet;
+	if ( heading_mode == FG_TC_HEADING_LOCK ) {
+	    // drive the turn coordinator to zero
+	    double turn = FGSteam::get_TC_std();
+	    // cout << "turn rate = " << turn << endl;
+	    double AileronSet = -turn / 2.0;
+	    if ( AileronSet < -1.0 ) { AileronSet = -1.0; }
+	    if ( AileronSet >  1.0 ) { AileronSet =  1.0; }
+	    globals->get_controls()->set_aileron( AileronSet );
+	    globals->get_controls()->set_rudder( AileronSet / 4.0 );
+	} else {
+	    // steer towards the target heading
+
+	    double RelHeading;
+	    double TargetRoll;
+	    double RelRoll;
+	    double AileronSet;
 
-	RelHeading = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
-	// figure out how far off we are from desired heading
+	    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.
-	FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
+	    // Now it is time to deterime how far we should be rolled.
+	    SG_LOG( SG_AUTOPILOT, SG_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;
+	    // 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 {
-		TargetRoll = MaxRoll;
-	    }
-	} else {
-	    // We have to calculate the Target roll
+		// 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
     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 = (FGBFI::getAltitude() 
-			- current_radiostack->get_nav1_elev()) * FEET_TO_METER;
-	    double current_angle = atan2( y, x ) * RAD_TO_DEG;
+	    double y = (altitude_node->getDoubleValue()
+			- current_radiostack->get_nav1_elev()) * SG_FEET_TO_METER;
+	    double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 	    // cout << "current angle = " << current_angle << endl;
 
 	    double target_angle = current_radiostack->get_nav1_target_gs();
@@ -579,23 +619,24 @@ int FGAutopilot::run() {
 	    // convert to meter/min
 	    // cout << "raw ground speed = " << cur_fdm_state->get_V_ground_speed() << endl;
 	    double horiz_vel = cur_fdm_state->get_V_ground_speed()
-		* FEET_TO_METER * 60.0;
+		* SG_FEET_TO_METER * 60.0;
 	    // cout << "Horizontal vel = " << horiz_vel << endl;
-	    TargetClimbRate = -sin( des_angle * DEG_TO_RAD ) * horiz_vel;
-	    // cout << "TargetClimbRate = " << TargetClimbRate << endl;
+	    climb_rate = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) * horiz_vel;
+	    // cout << "climb_rate = " << climb_rate << endl;
 	    /* climb_error_accum += gs_diff * 2.0; */
-	    /* TargetClimbRate = gs_diff * 200.0 + climb_error_accum; */
+	    /* 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;
+	    climb_rate =
+		( TargetAGL - altitude_agl_node->getDoubleValue()
+                  * SG_FEET_TO_METER ) * 16.0;
 	    // cout << "target agl = " << TargetAGL 
 	    //      << "  current agl = " << fgAPget_agl() 
-	    //      << "  target climb rate = " << TargetClimbRate 
+	    //      << "  target climb rate = " << climb_rate 
 	    //      << endl;
 	} else {
 	    // just try to zero out rate of climb ...
-	    TargetClimbRate = 0.0;
+	    climb_rate = 0.0;
 	}
 
 	speed = get_speed();
@@ -604,30 +645,36 @@ int FGAutopilot::run() {
 	    max_climb = 0.0;
 	} else if ( speed < best_climb ) {
 	    max_climb = ((best_climb - min_climb) - (best_climb - speed)) 
-		* ideal_climb_rate 
+		* fabs(TargetClimbRate) 
 		/ (best_climb - min_climb);
 	} else {			
-	    max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
+	    max_climb = ( speed - best_climb ) * 10.0 + fabs(TargetClimbRate);
 	}
 
 	// 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) ) {
+	    climb_rate = fabs(TargetClimbRate);
 	}
 
-	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(TargetDecentRate) ) {
+	    climb_rate = -fabs(TargetDecentRate);
 	}
 
-	error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate;
-	// cout << "climb rate = " << fgAPget_climb() 
-	//      << "  error = " << error << endl;
+	// cout << "Target climb rate = " << TargetClimbRate << endl;
+	// cout << "given our speed, modified desired climb rate = "
+	//      << climb_rate * SG_METER_TO_FEET 
+	//      << " fpm" << endl;
+        // cout << "Current climb rate = "
+        //      << vertical_speed_node->getDoubleValue() * 60 << " fpm" << 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
@@ -654,7 +701,7 @@ int FGAutopilot::run() {
 	    total_adj = -1.0;
 	}
 
-	controls.set_elevator( total_adj );
+	globals->get_controls()->set_elevator_trim( total_adj );
     }
 
     // auto throttle
@@ -693,7 +740,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
@@ -725,16 +773,18 @@ 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
+    // Ok, we are done
+    SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run( returns )" );
+
     return 0;
 }
 
@@ -744,21 +794,26 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 
     if ( heading_mode == FG_DG_HEADING_LOCK ) {
 	// set heading hold to current heading (as read from DG)
-	DGTargetHeading = FGSteam::get_DG_deg();
-    } else if ( heading_mode == FG_HEADING_LOCK ) {
+	// ... 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 ) {
 	if ( globals->get_route()->size() ) {
 	    double course, distance;
 
-	    old_lat = FGBFI::getLatitude();
-	    old_lon = FGBFI::getLongitude();
+	    old_lat = latitude_node->getDoubleValue();
+	    old_lon = longitude_node->getDoubleValue();
 
 	    waypoint = globals->get_route()->get_first();
-	    waypoint.CourseAndDistance( FGBFI::getLongitude(),
-					FGBFI::getLatitude(),
-					FGBFI::getLatitude() * FEET_TO_METER,
+	    waypoint.CourseAndDistance( longitude_node->getDoubleValue(),
+					latitude_node->getDoubleValue(),
+					altitude_node->getDoubleValue()
+                                        * SG_FEET_TO_METER,
 					&course, &distance );
 	    TargetHeading = course;
 	    TargetDistance = distance;
@@ -770,17 +825,16 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 		TargetAltitude = waypoint.get_target_alt();
 		altitude_mode = FG_ALTITUDE_LOCK;
 		set_AltitudeEnabled( true );
-		MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
+		MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 	    }
 
-	    FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( "
+	    SG_LOG( SG_COCKPIT, SG_INFO, " set_HeadingMode: ( "
 		    << get_TargetLatitude()  << " "
 		    << get_TargetLongitude() << " ) "
 		    );
 	} else {
 	    // no more way points, default to heading lock.
-	    heading_mode = FG_HEADING_LOCK;
-	    TargetHeading = FGBFI::getHeading();
+	    heading_mode = FG_TC_HEADING_LOCK;
 	}
     }
 
@@ -795,47 +849,48 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
     alt_error_accum = 0.0;
 
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
-	// lock at current altitude
-	TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
+	if ( TargetAltitude < altitude_agl_node->getDoubleValue()
+             * SG_FEET_TO_METER ) {
+	}
 
 	if ( fgGetString("/sim/startup/units") == "feet" ) {
-	    MakeTargetAltitudeStr( TargetAltitude * METER_TO_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;
+	TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER;
 
 	if ( fgGetString("/sim/startup/units") == "feet" ) {
-	    MakeTargetAltitudeStr( TargetAGL * METER_TO_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():" );
+    SG_LOG( SG_COCKPIT, SG_INFO, " set_AltitudeMode():" );
 }
 
 
 #if 0
 static inline double get_aoa( void ) {
-    return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
+    return( cur_fdm_state->get_Gamma_vert_rad() * SGD_RADIANS_TO_DEGREES );
 }
 
 static inline double fgAPget_latitude( void ) {
-    return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
+    return( cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES );
 }
 
 static inline double fgAPget_longitude( void ) {
-    return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
+    return( cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES );
 }
 
 static inline double fgAPget_roll( void ) {
-    return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
+    return( cur_fdm_state->get_Phi() * SGD_RADIANS_TO_DEGREES );
 }
 
 static inline double get_pitch( void ) {
@@ -843,16 +898,16 @@ static inline double get_pitch( void ) {
 }
 
 double fgAPget_heading( void ) {
-    return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
+    return( cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES );
 }
 
 static inline double fgAPget_altitude( void ) {
-    return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
+    return( cur_fdm_state->get_Altitude() * SG_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 );
+    return( cur_fdm_state->get_Climb_Rate() * SG_FEET_TO_METER * 60 );
 }
 
 static inline double get_sideslip( void ) {
@@ -862,8 +917,8 @@ static inline double get_sideslip( void ) {
 static inline double fgAPget_agl( void ) {
     double agl;
 
-    agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
-	- scenery.cur_elev;
+    agl = cur_fdm_state->get_Altitude() * SG_FEET_TO_METER
+	- scenery.get_cur_elev();
 
     return( agl );
 }
@@ -876,11 +931,11 @@ void FGAutopilot::AltitudeSet( double new_altitude ) {
     // cout << "new altitude = " << new_altitude << endl;
 
     if ( fgGetString("/sim/startup/units") == "feet" ) {
-	target_alt = new_altitude * FEET_TO_METER;
+	target_alt = new_altitude * SG_FEET_TO_METER;
     }
 
-    if( target_alt < scenery.cur_elev ) {
-	target_alt = scenery.cur_elev;
+    if( target_alt < scenery.get_cur_elev() ) {
+	target_alt = scenery.get_cur_elev();
     }
 
     TargetAltitude = target_alt;
@@ -889,7 +944,7 @@ void FGAutopilot::AltitudeSet( double new_altitude ) {
     // cout << "TargetAltitude = " << TargetAltitude << endl;
 
     if ( fgGetString("/sim/startup/units") == "feet" ) {
-	target_alt *= METER_TO_FEET;
+	target_alt *= SG_METER_TO_FEET;
     }
     // ApAltitudeDialogInput->setValue((float)target_alt);
     MakeTargetAltitudeStr( target_alt );
@@ -903,8 +958,8 @@ void FGAutopilot::AltitudeAdjust( double inc )
     double target_alt, target_agl;
 
     if ( fgGetString("/sim/startup/units") == "feet" ) {
-	target_alt = TargetAltitude * METER_TO_FEET;
-	target_agl = TargetAGL * METER_TO_FEET;
+	target_alt = TargetAltitude * SG_METER_TO_FEET;
+	target_agl = TargetAGL * SG_METER_TO_FEET;
     } else {
 	target_alt = TargetAltitude;
 	target_agl = TargetAGL;
@@ -914,30 +969,30 @@ void FGAutopilot::AltitudeAdjust( double inc )
     // cout << "target_agl / inc = " << target_agl / inc << endl;
     // cout << "(int)(target_agl / inc) = " << (int)(target_agl / inc) << endl;
 
-    if ( fabs((int)(target_alt / inc) * inc - target_alt) < FG_EPSILON ) {
+    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) < FG_EPSILON ) {
+    if ( fabs((int)(target_agl / inc) * inc - target_agl) < SG_EPSILON ) {
 	target_agl += inc;
     } else {
 	target_agl = ( int ) ( target_agl / inc ) * inc + inc;
     }
 
     if ( fgGetString("/sim/startup/units") == "feet" ) {
-	target_alt *= FEET_TO_METER;
-	target_agl *= FEET_TO_METER;
+	target_alt *= SG_FEET_TO_METER;
+	target_agl *= SG_FEET_TO_METER;
     }
 
     TargetAltitude = target_alt;
     TargetAGL = target_agl;
 	
     if ( fgGetString("/sim/startup/units") == "feet" )
-	target_alt *= METER_TO_FEET;
+	target_alt *= SG_METER_TO_FEET;
     if ( fgGetString("/sim/startup/units") == "feet" )
-	target_agl *= METER_TO_FEET;
+	target_agl *= SG_METER_TO_FEET;
 
     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 	MakeTargetAltitudeStr( target_alt );
@@ -950,7 +1005,8 @@ void FGAutopilot::AltitudeAdjust( double inc )
 
 
 void FGAutopilot::HeadingAdjust( double inc ) {
-    if ( heading_mode != FG_DG_HEADING_LOCK && heading_mode != FG_HEADING_LOCK )
+    if ( heading_mode != FG_DG_HEADING_LOCK
+	 && heading_mode != FG_TRUE_HEADING_LOCK )
     {
 	heading_mode = FG_DG_HEADING_LOCK;
     }
@@ -968,13 +1024,19 @@ void FGAutopilot::HeadingAdjust( double inc ) {
 
 
 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();
 }
 
@@ -989,11 +1051,11 @@ void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
     auto_throttle = value;
 
     if ( auto_throttle == true ) {
-	TargetSpeed = FGBFI::getAirspeed();
+        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 );
 }