/************************************************************************** * autopilot.c -- 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 "autopilot.h" #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_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. } static double get_aoa( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( FG_Gamma_vert_rad * RAD_TO_DEG ); } static double fgAPget_roll( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( FG_Phi * RAD_TO_DEG ); } static double get_pitch( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( FG_Theta ); } double fgAPget_heading( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( FG_Psi * RAD_TO_DEG ); } static double get_altitude( void ) { fgFLIGHT *f; // double rough_elev; f = current_aircraft.flight; // rough_elev = mesh_altitude(FG_Longitude * RAD_TO_ARCSEC, // FG_Latitude * RAD_TO_ARCSEC); return( FG_Altitude * FEET_TO_METER /* -rough_elev */ ); } static double get_sideslip( void ) { fgFLIGHT *f; f = current_aircraft.flight; return( FG_Beta ); } // 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 ; fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" ); 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"); APData->Mode = 0 ; // turn the AP off APData->Heading = 0.0; // default direction, due north // 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 if (APData->Mode == 0) // the autopilot is shut off return 0 ; if (APData->Mode == 1) // heading hold mode { double RelHeading; double TargetRoll; double RelRoll; double AileronSet; RelHeading = NormalizeDegrees( APData->Heading - fgAPget_heading()); // 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); if ( abs(RelHeading) > APData->RollOut ) // We are further from heading than the roll out point { if (RelHeading < 0 ) // set Target Roll to Max in desired direction 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 fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll); RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll()); if ( abs(RelRoll) > APData->RollOutSmooth ) // We are further from heading than the roll out smooth point { if (RelRoll < 0 ) // set Target Roll to Max in desired direction AileronSet = 0-APData->MaxAileron; else AileronSet = APData->MaxAileron; } else AileronSet = LinearExtrapolate(RelRoll,-APData->RollOutSmooth,-APData->MaxAileron,APData->RollOutSmooth,APData->MaxAileron); fgAileronSet(AileronSet); //Cool, it is done. return 0; } 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); } //every thing else failed. Not in a valid autopilot mode return -1; } 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 fgAPSetHeading( double Heading) { // Set the heading for the autopilot subsystem // Special Magic Number: // -1= Set Heading To Current Heading, Define equivilent AP_CURRENT_HEADING // Remove at a later date fgAPDataPtr APData; APData = APDataGlobal; // end section if (Heading == AP_CURRENT_HEADING) { APData->Heading = fgAPget_heading(); } else { APData->Heading = Heading; } fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading : %f\n", APData->Heading); } 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); };