/************************************************************************** * 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 #endif #include #include #include #include "autopilot.hxx" #include #include // 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 ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_V_equiv_kts() ); // Make an explicit function call. } static double get_aoa( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Gamma_vert_rad() * RAD_TO_DEG ); } static double fgAPget_roll( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Phi() * RAD_TO_DEG ); } static double get_pitch( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Theta() ); } double fgAPget_heading( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Psi() * RAD_TO_DEG ); } static double fgAPget_altitude( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Altitude() * FEET_TO_METER /* -rough_elev */ ); } static double fgAPget_climb( void ) { fgFLIGHT *f; f = current_aircraft.flight; // return in meters per minute return( f->get_Climb_Rate() * FEET_TO_METER * 60 ); } static double get_sideslip( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( f->get_Beta() ); } static double fgAPget_agl( void ) { fgFLIGHT *f; double agl; f = current_aircraft.flight; 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::FG_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); };