Turned "struct fgCONTROLS" into a class, with inlined accessor functions.

[flightgear.git] / Autopilot / autopilot.cxx

diff --git a/Autopilot/autopilot.cxx b/Autopilot/autopilot.cxx
index e9c3b9c88edef37532cb9c40ceaf2204a482661f..663a5e45b2eb5e3a6de945ca8f32ddf232b28d09 100644 (file)
--- a/Autopilot/autopilot.cxx
+++ b/Autopilot/autopilot.cxx
@@ -57,45 +57,6 @@
 // They should eventually be member functions of the aircraft.
 //
 
-static double get_throttleval( void )
-{
-       fgCONTROLS *pcontrols;
-
-  pcontrols = current_aircraft.controls;
-  return pcontrols->throttle[0];     // Hack limiting to one engine
-}
-
-static double get_aileronval( void )
-{
-       fgCONTROLS *pcontrols;
-
-  pcontrols = current_aircraft.controls;
-  return pcontrols->aileron;
-}
-
-static double get_elevatorval( void )
-{
-       fgCONTROLS *pcontrols;
-
-  pcontrols = current_aircraft.controls;
-  return pcontrols->elevator;
-}
-
-static double get_elev_trimval( void )
-{
-       fgCONTROLS *pcontrols;
-
-  pcontrols = current_aircraft.controls;
-  return pcontrols->elevator_trim;
-}
-
-static double get_rudderval( void )
-{
-       fgCONTROLS *pcontrols;
-
-  pcontrols = current_aircraft.controls;
-  return pcontrols->rudder;
-}
 
 static double get_speed( void )
 {
@@ -290,8 +251,8 @@ int fgAPRun( void )
                                            APData->MaxAileron );
        }
                
-       fgAileronSet(AileronSet);
-       fgRudderSet(0.0);
+       controls.set_aileron( AileronSet );
+       controls.set_rudder( 0.0 );
     }
 
     // altitude hold or terrain follow enabled?
@@ -333,19 +294,10 @@ int fgAPRun( void )
 
        error = fgAPget_climb() - APData->TargetClimbRate;
 
-       // push current error onto queue and add into accumulator
-       // alt_error_queue.push_back(error);
+       // accumulate the error under the curve ... this really should
+       // be *= delta t
        APData->alt_error_accum += error;
 
-       // if queue size larger than 60 ... pop front and subtract
-       // from accumulator
-       // size = alt_error_queue.size();
-       // if ( size > 300 ) {
-           // APData->alt_error_accum -= alt_error_queue.front();
-           // alt_error_queue.pop_front();
-           // size--;
-       // }
-
        // calculate integral error, and adjustment amount
        int_error = APData->alt_error_accum;
        // printf("error = %.2f  int_error = %.2f\n", error, int_error);
@@ -359,10 +311,45 @@ int fgAPRun( void )
        if ( total_adj >  0.6 ) { total_adj =  0.6; }
        if ( total_adj < -0.2 ) { total_adj = -0.2; }
 
-       fgElevSet( total_adj );
+       controls.set_elevator( total_adj );
+    }
+
+    // auto throttle enabled?
+    if ( APData->auto_throttle == 1 ) {
+       double error;
+       double prop_error, int_error;
+       double prop_adj, int_adj, total_adj;
+
+       error = APData->TargetSpeed - get_speed();
+
+       // accumulate the error under the curve ... this really should
+       // be *= delta t
+       APData->speed_error_accum += error;
+       if ( APData->speed_error_accum > 2000.0 ) {
+           APData->speed_error_accum = 2000.0;
+       }
+       if ( APData->speed_error_accum < -2000.0 ) {
+           APData->speed_error_accum = -2000.0;
+       }
+
+       // calculate integral error, and adjustment amount
+       int_error = APData->speed_error_accum;
+
+       // printf("error = %.2f  int_error = %.2f\n", error, int_error);
+       int_adj = int_error / 200.0;
+       
+       // caclulate proportional error
+       prop_error = error;
+       prop_adj = 0.5 + prop_error / 50.0;
+
+       total_adj = 0.9 * prop_adj + 0.1 * int_adj;
+       if ( total_adj > 1.0 ) { total_adj = 1.0; }
+       if ( total_adj < 0.0 ) { total_adj = 0.0; }
+
+       controls.set_throttle( fgCONTROLS::FG_ALL_ENGINES, total_adj );
     }
 
-    /*
+     /*
     if (APData->Mode == 2) // Glide slope hold
     {
        double RelSlope;
@@ -459,6 +446,29 @@ void fgAPToggleAltitude( void )
 }
                 
 
+void fgAPToggleAutoThrottle ( void )
+{
+    // Remove at a later date
+    fgAPDataPtr APData;
+
+    APData = APDataGlobal;
+    // end section
+
+    if ( APData->auto_throttle ) {
+       // turn off altitude hold
+       APData->auto_throttle = 0;
+    } else {
+       // turn on terrain follow, lock at current agl
+       APData->auto_throttle = 1;
+       APData->TargetSpeed = get_speed();
+       APData->speed_error_accum = 0.0;
+    }
+
+    fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: (%d) %.2f\n",
+             APData->auto_throttle,
+             APData->TargetSpeed);
+}
+
 void fgAPToggleTerrainFollow( void )
 {
     // Remove at a later date