#include <assert.h>
#include <stdlib.h>
-#include <list>
-#include <Include/fg_stl_config.h>
-
-#ifdef NEEDNAMESPACESTD
-using namespace std;
-#endif
+#include <Scenery/scenery.hxx>
#include "autopilot.hxx"
#include <Include/fg_constants.h>
-#include <Debug/fg_debug.h>
-
-
-static list < double > alt_error_queue;
+#include <Debug/logstream.hxx>
// The below routines were copied right from hud.c ( I hate reinventing
// 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 )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
- return( FG_V_equiv_kts ); // Make an explicit function call.
+ return( current_aircraft.fdm_state->get_V_equiv_kts() );
}
static double get_aoa( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
- return( FG_Gamma_vert_rad * RAD_TO_DEG );
+ return( current_aircraft.fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
}
static double fgAPget_roll( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
- return( FG_Phi * RAD_TO_DEG );
+ return( current_aircraft.fdm_state->get_Phi() * RAD_TO_DEG );
}
static double get_pitch( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
- return( FG_Theta );
+ return( current_aircraft.fdm_state->get_Theta() );
}
double fgAPget_heading( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
- return( FG_Psi * RAD_TO_DEG );
+ return( current_aircraft.fdm_state->get_Psi() * RAD_TO_DEG );
}
static double fgAPget_altitude( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
-
- return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
+ return( current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER );
}
static double fgAPget_climb( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
-
- // return in meters per minute
- return( FG_Climb_Rate * FEET_TO_METER * 60 );
+ // return in meters per minute
+ return( current_aircraft.fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
}
static double get_sideslip( void )
{
- fgFLIGHT *f;
-
- f = current_aircraft.flight;
-
- return( FG_Beta );
+ return( current_aircraft.fdm_state->get_Beta() );
+}
+
+static double fgAPget_agl( void )
+{
+ double agl;
+
+ agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER
+ - scenery.cur_elev;
+
+ return( agl );
}
// End of copied section. ( thanks for the wheel :-)
{
fgAPDataPtr APData ;
- fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
+ FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
- if (APData == NULL) // I couldn't get the mem. Dying
- fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
+ if (APData == NULL) {
+ // I couldn't get the mem. Dying
+ FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
+ exit(-1);
+ }
APData->heading_hold = 0 ; // turn the heading hold off
APData->altitude_hold = 0 ; // turn the altitude hold off
// figure out how far off we are from desired heading
// Now it is time to deterime how far we should be rolled.
- fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
+ FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
// Check if we are further from heading than the roll out point
// Compare Target roll to Current Roll, Generate Rel Roll
- fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
+ FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
APData->MaxAileron );
}
- fgAileronSet(AileronSet);
- fgRudderSet(0.0);
+ controls.set_aileron( AileronSet );
+ controls.set_rudder( 0.0 );
}
- // altitude hold enabled?
- if ( APData->altitude_hold == 1 ) {
- double error, size;
+ // altitude hold or terrain follow enabled?
+ if ( (APData->altitude_hold == 1) || (APData->terrain_follow == 1) ) {
+ double speed, max_climb, error;
double prop_error, int_error;
- double prop_adj, int_adj;
+ double prop_adj, int_adj, total_adj;
+
+ if (APData->altitude_hold == 1) {
+ // normal altitude hold
+ APData->TargetClimbRate =
+ (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
+ } else if (APData->terrain_follow == 1) {
+ // brain dead ground hugging with no look ahead
+ APData->TargetClimbRate =
+ ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
+ } else {
+ // just try to zero out rate of climb ...
+ APData->TargetClimbRate = 0.0;
+ }
- error = fgAPget_climb();
+ speed = get_speed();
- // push current error onto queue and add into accumulator
- alt_error_queue.push_back(error);
- APData->alt_error_accum += error;
+ if ( speed < 90.0 ) {
+ max_climb = 0.0;
+ } else if ( speed < 100.0 ) {
+ max_climb = (speed - 90.0) * 20;
+ } else {
+ max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
+ }
+
+ if ( APData->TargetClimbRate > max_climb ) {
+ APData->TargetClimbRate = max_climb;
+ }
- // if queue size larger than 20 ... pop front and subtract
- // from accumulator
- size = alt_error_queue.size();
- if ( size > 20 ) {
- APData->alt_error_accum -= alt_error_queue.front();
- alt_error_queue.pop_front();
- size--;
+ if ( APData->TargetClimbRate < -400.0 ) {
+ APData->TargetClimbRate = -400.0;
}
+ error = fgAPget_climb() - APData->TargetClimbRate;
+
+ // accumulate the error under the curve ... this really should
+ // be *= delta t
+ APData->alt_error_accum += error;
+
// calculate integral error, and adjustment amount
- int_error = APData->alt_error_accum / size;
- printf("int_error = %.2f\n", int_error);
- int_adj = int_error / 2000.0;
+ int_error = APData->alt_error_accum;
+ // printf("error = %.2f int_error = %.2f\n", error, int_error);
+ int_adj = int_error / 8000.0;
// caclulate proportional error
- prop_error = fgAPget_climb();
+ prop_error = error;
prop_adj = prop_error / 2000.0;
- fgElevSet( int_adj );
+ total_adj = 0.9 * prop_adj + 0.1 * int_adj;
+ if ( total_adj > 0.6 ) { total_adj = 0.6; }
+ if ( total_adj < -0.2 ) { total_adj = -0.2; }
+
+ 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::ALL_ENGINES, total_adj );
+ }
+
+ /*
if (APData->Mode == 2) // Glide slope hold
{
double RelSlope;
APData->TargetHeading = fgAPget_heading();
}
- fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
- APData->heading_hold,
- APData->TargetHeading);
+ FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
+ << APData->heading_hold << ") " << APData->TargetHeading );
}
} else {
// turn on altitude hold, lock at current altitude
APData->altitude_hold = 1;
+ APData->terrain_follow = 0;
APData->TargetAltitude = fgAPget_altitude();
+ APData->alt_error_accum = 0.0;
+ // alt_error_queue.erase( alt_error_queue.begin(),
+ // alt_error_queue.end() );
}
- fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
- APData->altitude_hold,
- APData->TargetAltitude);
+ FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
+ << APData->altitude_hold << ") " << APData->TargetAltitude );
}
+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;
+ }
+
+ FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
+ << APData->auto_throttle << ") " << APData->TargetSpeed );
+}
+
+void fgAPToggleTerrainFollow( void )
+{
+ // Remove at a later date
+ fgAPDataPtr APData;
+
+ APData = APDataGlobal;
+ // end section
+
+ if ( APData->terrain_follow ) {
+ // turn off altitude hold
+ APData->terrain_follow = 0;
+ } else {
+ // turn on terrain follow, lock at current agl
+ APData->terrain_follow = 1;
+ APData->altitude_hold = 0;
+ APData->TargetAGL = fgAPget_agl();
+ APData->alt_error_accum = 0.0;
+ }
+
+ FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
+ << APData->terrain_follow << ") " << APData->TargetAGL );
+}
+
double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
{
// This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
projects / flightgear.git / blobdiff