fly/flightgear
1
0
Fork 0
Code Activity
flightgear/Autopilot/autopilot.cxx

512 lines
13 KiB
C++
Raw Blame History

/**************************************************************************
* autopilot.cxx -- autopilot subsystem
*
* Written by Jeff Goeke-Smith, started April 1998.
*
* Copyright (C) 1998 Jeff Goeke-Smith, jgoeke@voyager.net
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License as
* published by the Free Software Foundation; either version 2 of the
* License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*
*
*
**************************************************************************/
#ifdef HAVE_CONFIG_H
# include <config.h>
#endif
#include <assert.h>
#include <stdlib.h>
#include <Scenery/scenery.hxx>
#include "autopilot.hxx"
#include <Include/fg_constants.h>
#include <Debug/logstream.hxx>
// The below routines were copied right from hud.c ( I hate reinventing
// the wheel more than necessary)
// The following routines obtain information concerntin the aircraft's
// current state and return it to calling instrument display routines.
// They should eventually be member functions of the aircraft.
//
static double get_speed( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_V_equiv_kts() ); // Make an explicit function call.
}
static double get_aoa( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Gamma_vert_rad() * RAD_TO_DEG );
}
static double fgAPget_roll( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Phi() * RAD_TO_DEG );
}
static double get_pitch( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Theta() );
}
double fgAPget_heading( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Psi() * RAD_TO_DEG );
}
static double fgAPget_altitude( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Altitude() * FEET_TO_METER /* -rough_elev */ );
}
static double fgAPget_climb( void )
{
FGState *f;
f = current_aircraft.fdm_state;
// return in meters per minute
return( f->get_Climb_Rate() * FEET_TO_METER * 60 );
}
static double get_sideslip( void )
{
FGState *f;
f = current_aircraft.fdm_state;
return( f->get_Beta() );
}
static double fgAPget_agl( void )
{
FGState *f;
double agl;
f = current_aircraft.fdm_state;
agl = f->get_Altitude() * FEET_TO_METER - scenery.cur_elev;
return( agl );
}
// End of copied section. ( thanks for the wheel :-)
// Local Prototype section
double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
double NormalizeDegrees( double Input);
// End Local ProtoTypes
fgAPDataPtr APDataGlobal; // global variable holding the AP info
// I want this gone. Data should be in aircraft structure
void fgAPInit( fgAIRCRAFT *current_aircraft )
{
fgAPDataPtr APData ;
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
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
APData->TargetHeading = 0.0; // default direction, due north
APData->TargetAltitude = 3000; // default altitude in meters
APData->alt_error_accum = 0.0;
// These eventually need to be read from current_aircaft somehow.
APData->MaxRoll = 7; // the maximum roll, in Deg
APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg
APData->MaxAileron= .1; // how far can I move the aleron from center.
APData->RollOutSmooth = 10; // Smoothing distance for alerion control
//Remove at a later date
APDataGlobal = APData;
};
int fgAPRun( void )
{
// Remove the following lines when the calling funcitons start
// passing in the data pointer
fgAPDataPtr APData;
APData = APDataGlobal;
// end section
// heading hold enabled?
if ( APData->heading_hold == 1 ) {
double RelHeading;
double TargetRoll;
double RelRoll;
double AileronSet;
RelHeading =
NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
// 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) > APData->RollOut ) {
// set Target Roll to Max in desired direction
if (RelHeading < 0 ) {
TargetRoll = 0-APData->MaxRoll;
} else {
TargetRoll = APData->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.
TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
-APData->MaxRoll, APData->RollOut,
APData->MaxRoll );
}
// Target Roll has now been Found.
// Compare Target roll to Current Roll, Generate Rel Roll
FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
// Check if we are further from heading than the roll out smooth point
if ( fabs(RelRoll) > APData->RollOutSmooth ) {
// set Target Roll to Max in desired direction
if (RelRoll < 0 ) {
AileronSet = 0-APData->MaxAileron;
} else {
AileronSet = APData->MaxAileron;
}
} else {
AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
-APData->MaxAileron,
APData->RollOutSmooth,
APData->MaxAileron );
}
controls.set_aileron( AileronSet );
controls.set_rudder( 0.0 );
}
// 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, 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;
}
speed = get_speed();
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 ( 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;
// printf("error = %.2f int_error = %.2f\n", error, int_error);
int_adj = int_error / 8000.0;
// caclulate proportional error
prop_error = error;
prop_adj = prop_error / 2000.0;
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;
double RelElevator;
// First, calculate Relative slope and normalize it
RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
// Now calculate the elevator offset from current angle
if ( abs(RelSlope) > APData->SlopeSmooth )
{
if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
RelElevator = -APData->MaxElevator;
else
RelElevator = APData->MaxElevator;
}
else
RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
// set the elevator
fgElevMove(RelElevator);
}
*/
// Ok, we are done
return 0;
}
/*
void fgAPSetMode( int mode)
{
//Remove the following line when the calling funcitons start passing in the data pointer
fgAPDataPtr APData;
APData = APDataGlobal;
// end section
fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
APData->Mode = mode; // set the new mode
}
*/
void fgAPToggleHeading( void )
{
// Remove at a later date
fgAPDataPtr APData;
APData = APDataGlobal;
// end section
if ( APData->heading_hold ) {
// turn off heading hold
APData->heading_hold = 0;
} else {
// turn on heading hold, lock at current heading
APData->heading_hold = 1;
APData->TargetHeading = fgAPget_heading();
}
FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
<< APData->heading_hold << ") " << APData->TargetHeading );
}
void fgAPToggleAltitude( void )
{
// Remove at a later date
fgAPDataPtr APData;
APData = APDataGlobal;
// end section
if ( APData->altitude_hold ) {
// turn off altitude hold
APData->altitude_hold = 0;
} 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() );
}
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
//assert(x1 != x2); // Divide by zero error. Cold abort for now
double m, b, y; // the constants to find in y=mx+b
m=(y2-y1)/(x2-x1); // calculate the m
b= y1- m * x1; // calculate the b
y = m * x + b; // the final calculation
return (y);
};
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)
Input -= 360;
if (Input <= -180)
Input += 360;
return (Input);
};
View git blame Copy permalink