Autopilot/autopilot.cxx

   1 /**************************************************************************
   2  * autopilot.cxx -- autopilot subsystem
   3  *
   4  * Written by Jeff Goeke-Smith, started April 1998.
   5  *
   6  * Copyright (C) 1998  Jeff Goeke-Smith, jgoeke@voyager.net
   7  *
   8  * This program is free software; you can redistribute it and/or
   9  * modify it under the terms of the GNU General Public License as
  10  * published by the Free Software Foundation; either version 2 of the
  11  * License, or (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful, but
  14  * WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  * General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  21  *
  22  *
  23  *
  24  **************************************************************************/
  25
  26
  27 #ifdef HAVE_CONFIG_H
  28 #  include <config.h>
  29 #endif
  30
  31 #include <assert.h>
  32 #include <stdlib.h>
  33
  34 #include <list>
  35 #include <Include/fg_stl_config.h>
  36 #include <Scenery/scenery.hxx>
  37
  38 #ifdef NEEDNAMESPACESTD
  39 using namespace std;
  40 #endif
  41
  42 #include "autopilot.hxx"
  43
  44 #include <Include/fg_constants.h>
  45 #include <Debug/fg_debug.h>
  46
  47
  48 static list < double > alt_error_queue;
  49
  50
  51 // The below routines were copied right from hud.c ( I hate reinventing
  52 // the wheel more than necessary)
  53
  54 // The following routines obtain information concerntin the aircraft's
  55 // current state and return it to calling instrument display routines.
  56 // They should eventually be member functions of the aircraft.
  57 //
  58
  59 static double get_throttleval( void )
  60 {
  61         fgCONTROLS *pcontrols;
  62
  63   pcontrols = current_aircraft.controls;
  64   return pcontrols->throttle[0];     // Hack limiting to one engine
  65 }
  66
  67 static double get_aileronval( void )
  68 {
  69         fgCONTROLS *pcontrols;
  70
  71   pcontrols = current_aircraft.controls;
  72   return pcontrols->aileron;
  73 }
  74
  75 static double get_elevatorval( void )
  76 {
  77         fgCONTROLS *pcontrols;
  78
  79   pcontrols = current_aircraft.controls;
  80   return pcontrols->elevator;
  81 }
  82
  83 static double get_elev_trimval( void )
  84 {
  85         fgCONTROLS *pcontrols;
  86
  87   pcontrols = current_aircraft.controls;
  88   return pcontrols->elevator_trim;
  89 }
  90
  91 static double get_rudderval( void )
  92 {
  93         fgCONTROLS *pcontrols;
  94
  95   pcontrols = current_aircraft.controls;
  96   return pcontrols->rudder;
  97 }
  98
  99 static double get_speed( void )
 100 {
 101         fgFLIGHT *f;
 102
 103         f = current_aircraft.flight;
 104         return( FG_V_equiv_kts );    // Make an explicit function call.
 105 }
 106
 107 static double get_aoa( void )
 108 {
 109         fgFLIGHT *f;
 110
 111         f = current_aircraft.flight;
 112         return( FG_Gamma_vert_rad * RAD_TO_DEG );
 113 }
 114
 115 static double fgAPget_roll( void )
 116 {
 117         fgFLIGHT *f;
 118
 119         f = current_aircraft.flight;
 120         return( FG_Phi * RAD_TO_DEG );
 121 }
 122
 123 static double get_pitch( void )
 124 {
 125         fgFLIGHT *f;
 126
 127         f = current_aircraft.flight;
 128         return( FG_Theta );
 129 }
 130
 131 double fgAPget_heading( void )
 132 {
 133         fgFLIGHT *f;
 134
 135         f = current_aircraft.flight;
 136         return( FG_Psi * RAD_TO_DEG );
 137 }
 138
 139 static double fgAPget_altitude( void )
 140 {
 141         fgFLIGHT *f;
 142
 143         f = current_aircraft.flight;
 144
 145         return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
 146 }
 147
 148 static double fgAPget_climb( void )
 149 {
 150         fgFLIGHT *f;
 151
 152         f = current_aircraft.flight;
 153
 154         // return in meters per minute
 155         return( FG_Climb_Rate * FEET_TO_METER * 60 );
 156 }
 157
 158 static double get_sideslip( void )
 159 {
 160         fgFLIGHT *f;
 161
 162         f = current_aircraft.flight;
 163
 164         return( FG_Beta );
 165 }
 166
 167 static double fgAPget_agl( void )
 168 {
 169         fgFLIGHT *f;
 170         double agl;
 171
 172         f = current_aircraft.flight;
 173         agl = FG_Altitude * FEET_TO_METER - scenery.cur_elev;
 174
 175         return( agl );
 176 }
 177
 178 // End of copied section.  ( thanks for the wheel :-)
 179
 180 // Local Prototype section
 181
 182 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
 183 double NormalizeDegrees( double Input);
 184
 185 // End Local ProtoTypes
 186
 187 fgAPDataPtr APDataGlobal;       // global variable holding the AP info
 188                                 // I want this gone.  Data should be in aircraft structure
 189
 190
 191
 192 void fgAPInit( fgAIRCRAFT *current_aircraft )
 193 {
 194         fgAPDataPtr APData ;
 195
 196         fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
 197
 198         APData  = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
 199
 200         if (APData == NULL) // I couldn't get the mem.  Dying
 201                 fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
 202
 203         APData->heading_hold = 0 ;     // turn the heading hold off
 204         APData->altitude_hold = 0 ;    // turn the altitude hold off
 205
 206         APData->TargetHeading = 0.0;    // default direction, due north
 207         APData->TargetAltitude = 3000;  // default altitude in meters
 208         APData->alt_error_accum = 0.0;
 209
 210         // These eventually need to be read from current_aircaft somehow.
 211
 212         APData->MaxRoll = 7;            // the maximum roll, in Deg
 213         APData->RollOut = 30;           // the deg from heading to start rolling out at, in Deg
 214         APData->MaxAileron= .1;         // how far can I move the aleron from center.
 215         APData->RollOutSmooth = 10;     // Smoothing distance for alerion control
 216
 217         //Remove at a later date
 218         APDataGlobal = APData;
 219
 220 };
 221
 222 int fgAPRun( void )
 223 {
 224     // Remove the following lines when the calling funcitons start
 225     // passing in the data pointer
 226
 227     fgAPDataPtr APData;
 228
 229     APData = APDataGlobal;
 230     // end section
 231
 232     // heading hold enabled?
 233     if ( APData->heading_hold == 1 ) {
 234         double RelHeading;
 235         double TargetRoll;
 236         double RelRoll;
 237         double AileronSet;
 238
 239         RelHeading =
 240             NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
 241         // figure out how far off we are from desired heading
 242
 243         // Now it is time to deterime how far we should be rolled.
 244         fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
 245
 246
 247         // Check if we are further from heading than the roll out point
 248         if ( fabs(RelHeading) > APData->RollOut ) {
 249             // set Target Roll to Max in desired direction
 250             if (RelHeading < 0 ) {
 251                 TargetRoll = 0-APData->MaxRoll;
 252             } else {
 253                 TargetRoll = APData->MaxRoll;
 254             }
 255         } else {
 256             // We have to calculate the Target roll
 257
 258             // This calculation engine thinks that the Target roll
 259             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 260             // -MaxRoll) I hope this works well.  If I get ambitious
 261             // some day this might become a fancier curve or
 262             // something.
 263
 264             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
 265                                             -APData->MaxRoll, APData->RollOut,
 266                                             APData->MaxRoll );
 267         }
 268
 269         // Target Roll has now been Found.
 270
 271         // Compare Target roll to Current Roll, Generate Rel Roll
 272
 273         fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
 274
 275         RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
 276
 277         // Check if we are further from heading than the roll out smooth point
 278         if ( fabs(RelRoll) > APData->RollOutSmooth ) {
 279             // set Target Roll to Max in desired direction
 280             if (RelRoll < 0 ) {
 281                 AileronSet = 0-APData->MaxAileron;
 282             } else {
 283                 AileronSet = APData->MaxAileron;
 284             }
 285         } else {
 286             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
 287                                             -APData->MaxAileron,
 288                                             APData->RollOutSmooth,
 289                                             APData->MaxAileron );
 290         }
 291
 292         fgAileronSet(AileronSet);
 293         fgRudderSet(0.0);
 294     }
 295
 296     // altitude hold enabled?
 297     if ( APData->altitude_hold == 1 ) {
 298         double error, size;
 299         double prop_error, int_error;
 300         double prop_adj, int_adj, total_adj;
 301
 302         // normal altitude hold
 303         // APData->TargetClimbRate =
 304         //    (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
 305
 306         // brain dead ground hugging with no look ahead
 307         APData->TargetClimbRate =
 308             ( 500 - fgAPget_agl() ) * 8.0;
 309
 310         if ( APData->TargetClimbRate > 200.0 ) {
 311             APData->TargetClimbRate = 200.0;
 312         }
 313         if ( APData->TargetClimbRate < -200.0 ) {
 314             APData->TargetClimbRate = -200.0;
 315         }
 316
 317         error = fgAPget_climb() - APData->TargetClimbRate;
 318
 319         // push current error onto queue and add into accumulator
 320         alt_error_queue.push_back(error);
 321         APData->alt_error_accum += error;
 322
 323         // if queue size larger than 60 ... pop front and subtract
 324         // from accumulator
 325         size = alt_error_queue.size();
 326         if ( size > 300 ) {
 327             APData->alt_error_accum -= alt_error_queue.front();
 328             alt_error_queue.pop_front();
 329             size--;
 330         }
 331
 332         // calculate integral error, and adjustment amount
 333         int_error = APData->alt_error_accum / 10;
 334         printf("error = %.2f  int_error = %.2f\n", error, int_error);
 335         int_adj = int_error / 2000.0;
 336
 337         // caclulate proportional error
 338         prop_error = error;
 339         prop_adj = prop_error / 1000.0;
 340
 341         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 342         if ( total_adj >  0.5 ) { total_adj =  0.5; }
 343         if ( total_adj < -0.5 ) { total_adj = -0.5; }
 344
 345         fgElevSet( total_adj );
 346     }
 347
 348     /*
 349     if (APData->Mode == 2) // Glide slope hold
 350     {
 351         double RelSlope;
 352         double RelElevator;
 353
 354         // First, calculate Relative slope and normalize it
 355         RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
 356
 357         // Now calculate the elevator offset from current angle
 358         if ( abs(RelSlope) > APData->SlopeSmooth )
 359         {
 360             if ( RelSlope < 0 )         //  set RelElevator to max in the correct direction
 361                 RelElevator = -APData->MaxElevator;
 362             else
 363                 RelElevator = APData->MaxElevator;
 364         }
 365
 366         else
 367             RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
 368
 369         // set the elevator
 370         fgElevMove(RelElevator);
 371
 372     }
 373     */
 374
 375     // Ok, we are done
 376     return 0;
 377
 378 }
 379
 380 /*
 381 void fgAPSetMode( int mode)
 382 {
 383     //Remove the following line when the calling funcitons start passing in the data pointer
 384     fgAPDataPtr APData;
 385
 386     APData = APDataGlobal;
 387     // end section
 388
 389     fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
 390
 391     APData->Mode = mode;  // set the new mode
 392 }
 393 */
 394
 395 void fgAPToggleHeading( void )
 396 {
 397     // Remove at a later date
 398     fgAPDataPtr APData;
 399
 400     APData = APDataGlobal;
 401     // end section
 402
 403     if ( APData->heading_hold ) {
 404         // turn off heading hold
 405         APData->heading_hold = 0;
 406     } else {
 407         // turn on heading hold, lock at current heading
 408         APData->heading_hold = 1;
 409         APData->TargetHeading = fgAPget_heading();
 410     }
 411
 412     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
 413               APData->heading_hold,
 414               APData->TargetHeading);
 415 }
 416
 417
 418 void fgAPToggleAltitude( void )
 419 {
 420     // Remove at a later date
 421     fgAPDataPtr APData;
 422
 423     APData = APDataGlobal;
 424     // end section
 425
 426     if ( APData->altitude_hold ) {
 427         // turn off altitude hold
 428         APData->altitude_hold = 0;
 429     } else {
 430         // turn on altitude hold, lock at current altitude
 431         APData->altitude_hold = 1;
 432         APData->TargetAltitude = fgAPget_altitude();
 433         APData->alt_error_accum = 0.0;
 434         alt_error_queue.erase( alt_error_queue.begin(), alt_error_queue.end() );
 435     }
 436
 437     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
 438               APData->altitude_hold,
 439               APData->TargetAltitude);
 440 }
 441
 442
 443 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
 444 {
 445     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 446     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 447
 448     double m, b, y;             // the constants to find in y=mx+b
 449
 450     m=(y2-y1)/(x2-x1);  // calculate the m
 451
 452     b= y1- m * x1;              // calculate the b
 453
 454     y = m * x + b;              // the final calculation
 455
 456     return (y);
 457
 458 };
 459
 460 double NormalizeDegrees(double Input)
 461 {
 462     // normalize the input to the range (-180,180]
 463     // Input should not be greater than -360 to 360.  Current rules send the output to an undefined state.
 464     if (Input > 180)
 465         Input -= 360;
 466     if (Input <= -180)
 467         Input += 360;
 468
 469     return (Input);
 470 };