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
  37 #include <Scenery/scenery.hxx>
  38
  39 // #ifdef NEEDNAMESPACESTD
  40 // using namespace std;
  41 // #endif
  42
  43 #include "autopilot.hxx"
  44
  45 #include <Include/fg_constants.h>
  46 #include <Debug/fg_debug.h>
  47
  48
  49 // static list < double > alt_error_queue;
  50
  51
  52 // The below routines were copied right from hud.c ( I hate reinventing
  53 // the wheel more than necessary)
  54
  55 // The following routines obtain information concerntin the aircraft's
  56 // current state and return it to calling instrument display routines.
  57 // They should eventually be member functions of the aircraft.
  58 //
  59
  60 static double get_throttleval( void )
  61 {
  62         fgCONTROLS *pcontrols;
  63
  64   pcontrols = current_aircraft.controls;
  65   return pcontrols->throttle[0];     // Hack limiting to one engine
  66 }
  67
  68 static double get_aileronval( void )
  69 {
  70         fgCONTROLS *pcontrols;
  71
  72   pcontrols = current_aircraft.controls;
  73   return pcontrols->aileron;
  74 }
  75
  76 static double get_elevatorval( void )
  77 {
  78         fgCONTROLS *pcontrols;
  79
  80   pcontrols = current_aircraft.controls;
  81   return pcontrols->elevator;
  82 }
  83
  84 static double get_elev_trimval( void )
  85 {
  86         fgCONTROLS *pcontrols;
  87
  88   pcontrols = current_aircraft.controls;
  89   return pcontrols->elevator_trim;
  90 }
  91
  92 static double get_rudderval( void )
  93 {
  94         fgCONTROLS *pcontrols;
  95
  96   pcontrols = current_aircraft.controls;
  97   return pcontrols->rudder;
  98 }
  99
 100 static double get_speed( void )
 101 {
 102         fgFLIGHT *f;
 103
 104         f = current_aircraft.flight;
 105         return( FG_V_equiv_kts );    // Make an explicit function call.
 106 }
 107
 108 static double get_aoa( void )
 109 {
 110         fgFLIGHT *f;
 111
 112         f = current_aircraft.flight;
 113         return( FG_Gamma_vert_rad * RAD_TO_DEG );
 114 }
 115
 116 static double fgAPget_roll( void )
 117 {
 118         fgFLIGHT *f;
 119
 120         f = current_aircraft.flight;
 121         return( FG_Phi * RAD_TO_DEG );
 122 }
 123
 124 static double get_pitch( void )
 125 {
 126         fgFLIGHT *f;
 127
 128         f = current_aircraft.flight;
 129         return( FG_Theta );
 130 }
 131
 132 double fgAPget_heading( void )
 133 {
 134         fgFLIGHT *f;
 135
 136         f = current_aircraft.flight;
 137         return( FG_Psi * RAD_TO_DEG );
 138 }
 139
 140 static double fgAPget_altitude( void )
 141 {
 142         fgFLIGHT *f;
 143
 144         f = current_aircraft.flight;
 145
 146         return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
 147 }
 148
 149 static double fgAPget_climb( void )
 150 {
 151         fgFLIGHT *f;
 152
 153         f = current_aircraft.flight;
 154
 155         // return in meters per minute
 156         return( FG_Climb_Rate * FEET_TO_METER * 60 );
 157 }
 158
 159 static double get_sideslip( void )
 160 {
 161         fgFLIGHT *f;
 162
 163         f = current_aircraft.flight;
 164
 165         return( FG_Beta );
 166 }
 167
 168 static double fgAPget_agl( void )
 169 {
 170         fgFLIGHT *f;
 171         double agl;
 172
 173         f = current_aircraft.flight;
 174         agl = FG_Altitude * FEET_TO_METER - scenery.cur_elev;
 175
 176         return( agl );
 177 }
 178
 179 // End of copied section.  ( thanks for the wheel :-)
 180
 181 // Local Prototype section
 182
 183 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
 184 double NormalizeDegrees( double Input);
 185
 186 // End Local ProtoTypes
 187
 188 fgAPDataPtr APDataGlobal;       // global variable holding the AP info
 189                                 // I want this gone.  Data should be in aircraft structure
 190
 191
 192
 193 void fgAPInit( fgAIRCRAFT *current_aircraft )
 194 {
 195         fgAPDataPtr APData ;
 196
 197         fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
 198
 199         APData  = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
 200
 201         if (APData == NULL) // I couldn't get the mem.  Dying
 202                 fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
 203
 204         APData->heading_hold = 0 ;     // turn the heading hold off
 205         APData->altitude_hold = 0 ;    // turn the altitude hold off
 206
 207         APData->TargetHeading = 0.0;    // default direction, due north
 208         APData->TargetAltitude = 3000;  // default altitude in meters
 209         APData->alt_error_accum = 0.0;
 210
 211         // These eventually need to be read from current_aircaft somehow.
 212
 213         APData->MaxRoll = 7;            // the maximum roll, in Deg
 214         APData->RollOut = 30;           // the deg from heading to start rolling out at, in Deg
 215         APData->MaxAileron= .1;         // how far can I move the aleron from center.
 216         APData->RollOutSmooth = 10;     // Smoothing distance for alerion control
 217
 218         //Remove at a later date
 219         APDataGlobal = APData;
 220
 221 };
 222
 223 int fgAPRun( void )
 224 {
 225     // Remove the following lines when the calling funcitons start
 226     // passing in the data pointer
 227
 228     fgAPDataPtr APData;
 229
 230     APData = APDataGlobal;
 231     // end section
 232
 233     // heading hold enabled?
 234     if ( APData->heading_hold == 1 ) {
 235         double RelHeading;
 236         double TargetRoll;
 237         double RelRoll;
 238         double AileronSet;
 239
 240         RelHeading =
 241             NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
 242         // figure out how far off we are from desired heading
 243
 244         // Now it is time to deterime how far we should be rolled.
 245         fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
 246
 247
 248         // Check if we are further from heading than the roll out point
 249         if ( fabs(RelHeading) > APData->RollOut ) {
 250             // set Target Roll to Max in desired direction
 251             if (RelHeading < 0 ) {
 252                 TargetRoll = 0-APData->MaxRoll;
 253             } else {
 254                 TargetRoll = APData->MaxRoll;
 255             }
 256         } else {
 257             // We have to calculate the Target roll
 258
 259             // This calculation engine thinks that the Target roll
 260             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 261             // -MaxRoll) I hope this works well.  If I get ambitious
 262             // some day this might become a fancier curve or
 263             // something.
 264
 265             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
 266                                             -APData->MaxRoll, APData->RollOut,
 267                                             APData->MaxRoll );
 268         }
 269
 270         // Target Roll has now been Found.
 271
 272         // Compare Target roll to Current Roll, Generate Rel Roll
 273
 274         fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
 275
 276         RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
 277
 278         // Check if we are further from heading than the roll out smooth point
 279         if ( fabs(RelRoll) > APData->RollOutSmooth ) {
 280             // set Target Roll to Max in desired direction
 281             if (RelRoll < 0 ) {
 282                 AileronSet = 0-APData->MaxAileron;
 283             } else {
 284                 AileronSet = APData->MaxAileron;
 285             }
 286         } else {
 287             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
 288                                             -APData->MaxAileron,
 289                                             APData->RollOutSmooth,
 290                                             APData->MaxAileron );
 291         }
 292
 293         fgAileronSet(AileronSet);
 294         fgRudderSet(0.0);
 295     }
 296
 297     // altitude hold or terrain follow enabled?
 298     if ( (APData->altitude_hold == 1) || (APData->terrain_follow == 1) ) {
 299         double speed, max_climb, error;
 300         double prop_error, int_error;
 301         double prop_adj, int_adj, total_adj;
 302
 303         if (APData->altitude_hold == 1) {
 304             // normal altitude hold
 305             APData->TargetClimbRate =
 306                 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
 307         } else if (APData->terrain_follow == 1) {
 308             // brain dead ground hugging with no look ahead
 309             APData->TargetClimbRate =
 310                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
 311         } else {
 312             // just try to zero out rate of climb ...
 313             APData->TargetClimbRate = 0.0;
 314         }
 315
 316         speed = get_speed();
 317
 318         if ( speed < 90.0 ) {
 319             max_climb = 0.0;
 320         } else if ( speed < 100.0 ) {
 321             max_climb = (speed - 90.0) * 20;
 322         } else {
 323             max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
 324         }
 325
 326         if ( APData->TargetClimbRate > max_climb ) {
 327             APData->TargetClimbRate = max_climb;
 328         }
 329
 330         if ( APData->TargetClimbRate < -400.0 ) {
 331             APData->TargetClimbRate = -400.0;
 332         }
 333
 334         error = fgAPget_climb() - APData->TargetClimbRate;
 335
 336         // accumulate the error under the curve ... this really should
 337         // be *= delta t
 338         APData->alt_error_accum += error;
 339
 340         // calculate integral error, and adjustment amount
 341         int_error = APData->alt_error_accum;
 342         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 343         int_adj = int_error / 8000.0;
 344
 345         // caclulate proportional error
 346         prop_error = error;
 347         prop_adj = prop_error / 2000.0;
 348
 349         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 350         if ( total_adj >  0.6 ) { total_adj =  0.6; }
 351         if ( total_adj < -0.2 ) { total_adj = -0.2; }
 352
 353         fgElevSet( total_adj );
 354     }
 355
 356     // auto throttle enabled?
 357     if ( APData->auto_throttle == 1 ) {
 358         double error;
 359         double prop_error, int_error;
 360         double prop_adj, int_adj, total_adj;
 361
 362         error = APData->TargetSpeed - get_speed();
 363
 364         // accumulate the error under the curve ... this really should
 365         // be *= delta t
 366         APData->speed_error_accum += error;
 367         if ( APData->speed_error_accum > 2000.0 ) {
 368             APData->speed_error_accum = 2000.0;
 369         }
 370         if ( APData->speed_error_accum < -2000.0 ) {
 371             APData->speed_error_accum = -2000.0;
 372         }
 373
 374         // calculate integral error, and adjustment amount
 375         int_error = APData->speed_error_accum;
 376
 377         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 378         int_adj = int_error / 200.0;
 379
 380         // caclulate proportional error
 381         prop_error = error;
 382         prop_adj = 0.5 + prop_error / 50.0;
 383
 384         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 385         if ( total_adj > 1.0 ) { total_adj = 1.0; }
 386         if ( total_adj < 0.0 ) { total_adj = 0.0; }
 387
 388         fgThrottleSet( 0, total_adj );
 389     }
 390
 391      /*
 392     if (APData->Mode == 2) // Glide slope hold
 393     {
 394         double RelSlope;
 395         double RelElevator;
 396
 397         // First, calculate Relative slope and normalize it
 398         RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
 399
 400         // Now calculate the elevator offset from current angle
 401         if ( abs(RelSlope) > APData->SlopeSmooth )
 402         {
 403             if ( RelSlope < 0 )         //  set RelElevator to max in the correct direction
 404                 RelElevator = -APData->MaxElevator;
 405             else
 406                 RelElevator = APData->MaxElevator;
 407         }
 408
 409         else
 410             RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
 411
 412         // set the elevator
 413         fgElevMove(RelElevator);
 414
 415     }
 416     */
 417
 418     // Ok, we are done
 419     return 0;
 420
 421 }
 422
 423 /*
 424 void fgAPSetMode( int mode)
 425 {
 426     //Remove the following line when the calling funcitons start passing in the data pointer
 427     fgAPDataPtr APData;
 428
 429     APData = APDataGlobal;
 430     // end section
 431
 432     fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
 433
 434     APData->Mode = mode;  // set the new mode
 435 }
 436 */
 437
 438 void fgAPToggleHeading( void )
 439 {
 440     // Remove at a later date
 441     fgAPDataPtr APData;
 442
 443     APData = APDataGlobal;
 444     // end section
 445
 446     if ( APData->heading_hold ) {
 447         // turn off heading hold
 448         APData->heading_hold = 0;
 449     } else {
 450         // turn on heading hold, lock at current heading
 451         APData->heading_hold = 1;
 452         APData->TargetHeading = fgAPget_heading();
 453     }
 454
 455     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
 456               APData->heading_hold,
 457               APData->TargetHeading);
 458 }
 459
 460
 461 void fgAPToggleAltitude( void )
 462 {
 463     // Remove at a later date
 464     fgAPDataPtr APData;
 465
 466     APData = APDataGlobal;
 467     // end section
 468
 469     if ( APData->altitude_hold ) {
 470         // turn off altitude hold
 471         APData->altitude_hold = 0;
 472     } else {
 473         // turn on altitude hold, lock at current altitude
 474         APData->altitude_hold = 1;
 475         APData->terrain_follow = 0;
 476         APData->TargetAltitude = fgAPget_altitude();
 477         APData->alt_error_accum = 0.0;
 478         // alt_error_queue.erase( alt_error_queue.begin(),
 479         //                        alt_error_queue.end() );
 480     }
 481
 482     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
 483               APData->altitude_hold,
 484               APData->TargetAltitude);
 485 }
 486
 487
 488 void fgAPToggleAutoThrottle ( void )
 489 {
 490     // Remove at a later date
 491     fgAPDataPtr APData;
 492
 493     APData = APDataGlobal;
 494     // end section
 495
 496     if ( APData->auto_throttle ) {
 497         // turn off altitude hold
 498         APData->auto_throttle = 0;
 499     } else {
 500         // turn on terrain follow, lock at current agl
 501         APData->auto_throttle = 1;
 502         APData->TargetSpeed = get_speed();
 503         APData->speed_error_accum = 0.0;
 504     }
 505
 506     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: (%d) %.2f\n",
 507               APData->auto_throttle,
 508               APData->TargetSpeed);
 509 }
 510
 511 void fgAPToggleTerrainFollow( void )
 512 {
 513     // Remove at a later date
 514     fgAPDataPtr APData;
 515
 516     APData = APDataGlobal;
 517     // end section
 518
 519     if ( APData->terrain_follow ) {
 520         // turn off altitude hold
 521         APData->terrain_follow = 0;
 522     } else {
 523         // turn on terrain follow, lock at current agl
 524         APData->terrain_follow = 1;
 525         APData->altitude_hold = 0;
 526         APData->TargetAGL = fgAPget_agl();
 527         APData->alt_error_accum = 0.0;
 528     }
 529
 530     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: (%d) %.2f\n",
 531               APData->terrain_follow,
 532               APData->TargetAGL);
 533 }
 534
 535 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
 536 {
 537     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 538     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 539
 540     double m, b, y;             // the constants to find in y=mx+b
 541
 542     m=(y2-y1)/(x2-x1);  // calculate the m
 543
 544     b= y1- m * x1;              // calculate the b
 545
 546     y = m * x + b;              // the final calculation
 547
 548     return (y);
 549
 550 };
 551
 552 double NormalizeDegrees(double Input)
 553 {
 554     // normalize the input to the range (-180,180]
 555     // Input should not be greater than -360 to 360.  Current rules send the output to an undefined state.
 556     if (Input > 180)
 557         Input -= 360;
 558     if (Input <= -180)
 559         Input += 360;
 560
 561     return (Input);
 562 };