Autopilot/autopilot.cxx

   1 // autopilot.cxx -- autopilot subsystem
   2 //
   3 // Written by Jeff Goeke-Smith, started April 1998.
   4 //
   5 // Copyright (C) 1998  Jeff Goeke-Smith, jgoeke@voyager.net
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22 // (Log is kept at end of this file)
  23
  24
  25 #ifdef HAVE_CONFIG_H
  26 #  include <config.h>
  27 #endif
  28
  29 #include <assert.h>
  30 #include <stdlib.h>
  31
  32 #include <Scenery/scenery.hxx>
  33
  34 #include "autopilot.hxx"
  35
  36 #include <Include/fg_constants.h>
  37 #include <Debug/logstream.hxx>
  38 #include <Main/options.hxx>
  39
  40
  41 // The below routines were copied right from hud.c ( I hate reinventing
  42 // the wheel more than necessary)
  43
  44 // The following routines obtain information concerntin the aircraft's
  45 // current state and return it to calling instrument display routines.
  46 // They should eventually be member functions of the aircraft.
  47 //
  48
  49
  50 static double get_speed( void )
  51 {
  52     return( current_aircraft.fdm_state->get_V_equiv_kts() );
  53 }
  54
  55 static double get_aoa( void )
  56 {
  57     return( current_aircraft.fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
  58 }
  59
  60 static double fgAPget_roll( void )
  61 {
  62     return( current_aircraft.fdm_state->get_Phi() * RAD_TO_DEG );
  63 }
  64
  65 static double get_pitch( void )
  66 {
  67     return( current_aircraft.fdm_state->get_Theta() );
  68 }
  69
  70 double fgAPget_heading( void )
  71 {
  72     return( current_aircraft.fdm_state->get_Psi() * RAD_TO_DEG );
  73 }
  74
  75 static double fgAPget_altitude( void )
  76 {
  77     return( current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER );
  78 }
  79
  80 static double fgAPget_climb( void )
  81 {
  82     // return in meters per minute
  83     return( current_aircraft.fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
  84 }
  85
  86 static double get_sideslip( void )
  87 {
  88     return( current_aircraft.fdm_state->get_Beta() );
  89 }
  90
  91 static double fgAPget_agl( void )
  92 {
  93         double agl;
  94
  95         agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER
  96             - scenery.cur_elev;
  97
  98         return( agl );
  99 }
 100
 101 // End of copied section.  ( thanks for the wheel :-)
 102
 103 // Local Prototype section
 104
 105 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
 106 double NormalizeDegrees( double Input);
 107
 108 // End Local ProtoTypes
 109
 110 fgAPDataPtr APDataGlobal;       // global variable holding the AP info
 111                                 // I want this gone.  Data should be in aircraft structure
 112
 113
 114 bool fgAPHeadingEnabled( void )
 115 {
 116     fgAPDataPtr APData;
 117
 118     APData = APDataGlobal;
 119
 120     // heading hold enabled?
 121     return APData->heading_hold;
 122 }
 123
 124 bool fgAPAltitudeEnabled( void )
 125 {
 126     fgAPDataPtr APData;
 127
 128     APData = APDataGlobal;
 129
 130     // altitude hold or terrain follow enabled?
 131     return APData->altitude_hold || APData->terrain_follow ;
 132 }
 133
 134 bool fgAPAutoThrottleEnabled( void )
 135 {
 136     fgAPDataPtr APData;
 137
 138     APData = APDataGlobal;
 139
 140     // autothrottle enabled?
 141     return APData->auto_throttle;
 142 }
 143
 144 void fgAPAltitudeAdjust( double inc )
 145 {
 146     // Remove at a later date
 147     fgAPDataPtr APData;
 148     APData = APDataGlobal;
 149     // end section
 150
 151     double target_alt, target_agl;
 152
 153     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 154         target_alt = APData->TargetAltitude * METER_TO_FEET;
 155         target_agl = APData->TargetAGL * METER_TO_FEET;
 156     } else {
 157         target_alt = APData->TargetAltitude;
 158         target_agl = APData->TargetAGL;
 159     }
 160
 161     target_alt = (int)(target_alt / inc) * inc + inc;
 162     target_agl = (int)(target_agl / inc) * inc + inc;
 163
 164     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 165         target_alt *= FEET_TO_METER;
 166         target_agl *= FEET_TO_METER;
 167     }
 168
 169     APData->TargetAltitude = target_alt;
 170     APData->TargetAGL = target_agl;
 171 }
 172
 173 void fgAPHeadingAdjust( double inc )
 174 {
 175     fgAPDataPtr APData;
 176     APData = APDataGlobal;
 177
 178     double target = (int)(APData->TargetHeading / inc) * inc + inc;
 179
 180     APData->TargetHeading = NormalizeDegrees(target);
 181 }
 182
 183 void fgAPAutoThrottleAdjust( double inc )
 184 {
 185     fgAPDataPtr APData;
 186     APData = APDataGlobal;
 187
 188     double target = (int)(APData->TargetSpeed / inc) * inc + inc;
 189
 190     APData->TargetSpeed = target;
 191 }
 192
 193 void fgAPInit( fgAIRCRAFT *current_aircraft )
 194 {
 195         fgAPDataPtr APData ;
 196
 197         FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
 198
 199         APData  = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
 200
 201         if (APData == NULL) {
 202             // I couldn't get the mem.  Dying
 203             FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
 204             exit(-1);
 205         }
 206
 207         APData->heading_hold = false ;     // turn the heading hold off
 208         APData->altitude_hold = false ;    // turn the altitude hold off
 209
 210         APData->TargetHeading = 0.0;    // default direction, due north
 211         APData->TargetAltitude = 3000;  // default altitude in meters
 212         APData->alt_error_accum = 0.0;
 213
 214         // These eventually need to be read from current_aircaft somehow.
 215
 216 #if 0
 217         // Original values
 218         // the maximum roll, in Deg
 219         APData->MaxRoll = 7;
 220         // the deg from heading to start rolling out at, in Deg
 221         APData->RollOut = 30;
 222         // how far can I move the aleron from center.
 223         APData->MaxAileron= .1;
 224         // Smoothing distance for alerion control
 225         APData->RollOutSmooth = 10;
 226 #endif
 227
 228         // the maximum roll, in Deg
 229         APData->MaxRoll = 20;
 230
 231         // the deg from heading to start rolling out at, in Deg
 232         APData->RollOut = 20;
 233
 234         // how far can I move the aleron from center.
 235         APData->MaxAileron= .2;
 236
 237         // Smoothing distance for alerion control
 238         APData->RollOutSmooth = 10;
 239
 240         //Remove at a later date
 241         APDataGlobal = APData;
 242
 243 };
 244
 245 int fgAPRun( void )
 246 {
 247     // Remove the following lines when the calling funcitons start
 248     // passing in the data pointer
 249
 250     fgAPDataPtr APData;
 251
 252     APData = APDataGlobal;
 253     // end section
 254
 255     // heading hold enabled?
 256     if ( APData->heading_hold == true ) {
 257         double RelHeading;
 258         double TargetRoll;
 259         double RelRoll;
 260         double AileronSet;
 261
 262         RelHeading =
 263             NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
 264         // figure out how far off we are from desired heading
 265
 266         // Now it is time to deterime how far we should be rolled.
 267         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
 268
 269
 270         // Check if we are further from heading than the roll out point
 271         if ( fabs(RelHeading) > APData->RollOut ) {
 272             // set Target Roll to Max in desired direction
 273             if (RelHeading < 0 ) {
 274                 TargetRoll = 0-APData->MaxRoll;
 275             } else {
 276                 TargetRoll = APData->MaxRoll;
 277             }
 278         } else {
 279             // We have to calculate the Target roll
 280
 281             // This calculation engine thinks that the Target roll
 282             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 283             // -MaxRoll) I hope this works well.  If I get ambitious
 284             // some day this might become a fancier curve or
 285             // something.
 286
 287             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
 288                                             -APData->MaxRoll, APData->RollOut,
 289                                             APData->MaxRoll );
 290         }
 291
 292         // Target Roll has now been Found.
 293
 294         // Compare Target roll to Current Roll, Generate Rel Roll
 295
 296         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
 297
 298         RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
 299
 300         // Check if we are further from heading than the roll out smooth point
 301         if ( fabs(RelRoll) > APData->RollOutSmooth ) {
 302             // set Target Roll to Max in desired direction
 303             if (RelRoll < 0 ) {
 304                 AileronSet = 0-APData->MaxAileron;
 305             } else {
 306                 AileronSet = APData->MaxAileron;
 307             }
 308         } else {
 309             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
 310                                             -APData->MaxAileron,
 311                                             APData->RollOutSmooth,
 312                                             APData->MaxAileron );
 313         }
 314
 315         controls.set_aileron( AileronSet );
 316         controls.set_rudder( 0.0 );
 317     }
 318
 319     // altitude hold or terrain follow enabled?
 320     if ( APData->altitude_hold || APData->terrain_follow ) {
 321         double speed, max_climb, error;
 322         double prop_error, int_error;
 323         double prop_adj, int_adj, total_adj;
 324
 325         if ( APData->altitude_hold ) {
 326             // normal altitude hold
 327             APData->TargetClimbRate =
 328                 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
 329         } else if ( APData->terrain_follow ) {
 330             // brain dead ground hugging with no look ahead
 331             APData->TargetClimbRate =
 332                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
 333         } else {
 334             // just try to zero out rate of climb ...
 335             APData->TargetClimbRate = 0.0;
 336         }
 337
 338         speed = get_speed();
 339
 340         if ( speed < 90.0 ) {
 341             max_climb = 0.0;
 342         } else if ( speed < 100.0 ) {
 343             max_climb = (speed - 90.0) * 20;
 344         } else {
 345             max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
 346         }
 347
 348         if ( APData->TargetClimbRate > max_climb ) {
 349             APData->TargetClimbRate = max_climb;
 350         }
 351
 352         if ( APData->TargetClimbRate < -400.0 ) {
 353             APData->TargetClimbRate = -400.0;
 354         }
 355
 356         error = fgAPget_climb() - APData->TargetClimbRate;
 357
 358         // accumulate the error under the curve ... this really should
 359         // be *= delta t
 360         APData->alt_error_accum += error;
 361
 362         // calculate integral error, and adjustment amount
 363         int_error = APData->alt_error_accum;
 364         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 365         int_adj = int_error / 8000.0;
 366
 367         // caclulate proportional error
 368         prop_error = error;
 369         prop_adj = prop_error / 2000.0;
 370
 371         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 372         if ( total_adj >  0.6 ) { total_adj =  0.6; }
 373         if ( total_adj < -0.2 ) { total_adj = -0.2; }
 374
 375         controls.set_elevator( total_adj );
 376     }
 377
 378     // auto throttle enabled?
 379     if ( APData->auto_throttle ) {
 380         double error;
 381         double prop_error, int_error;
 382         double prop_adj, int_adj, total_adj;
 383
 384         error = APData->TargetSpeed - get_speed();
 385
 386         // accumulate the error under the curve ... this really should
 387         // be *= delta t
 388         APData->speed_error_accum += error;
 389         if ( APData->speed_error_accum > 2000.0 ) {
 390             APData->speed_error_accum = 2000.0;
 391         }
 392         if ( APData->speed_error_accum < -2000.0 ) {
 393             APData->speed_error_accum = -2000.0;
 394         }
 395
 396         // calculate integral error, and adjustment amount
 397         int_error = APData->speed_error_accum;
 398
 399         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 400         int_adj = int_error / 200.0;
 401
 402         // caclulate proportional error
 403         prop_error = error;
 404         prop_adj = 0.5 + prop_error / 50.0;
 405
 406         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 407         if ( total_adj > 1.0 ) { total_adj = 1.0; }
 408         if ( total_adj < 0.0 ) { total_adj = 0.0; }
 409
 410         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
 411     }
 412
 413      /*
 414     if (APData->Mode == 2) // Glide slope hold
 415     {
 416         double RelSlope;
 417         double RelElevator;
 418
 419         // First, calculate Relative slope and normalize it
 420         RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
 421
 422         // Now calculate the elevator offset from current angle
 423         if ( abs(RelSlope) > APData->SlopeSmooth )
 424         {
 425             if ( RelSlope < 0 )         //  set RelElevator to max in the correct direction
 426                 RelElevator = -APData->MaxElevator;
 427             else
 428                 RelElevator = APData->MaxElevator;
 429         }
 430
 431         else
 432             RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
 433
 434         // set the elevator
 435         fgElevMove(RelElevator);
 436
 437     }
 438     */
 439
 440     // Ok, we are done
 441     return 0;
 442
 443 }
 444
 445 /*
 446 void fgAPSetMode( int mode)
 447 {
 448     //Remove the following line when the calling funcitons start passing in the data pointer
 449     fgAPDataPtr APData;
 450
 451     APData = APDataGlobal;
 452     // end section
 453
 454     fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
 455
 456     APData->Mode = mode;  // set the new mode
 457 }
 458 */
 459
 460 void fgAPToggleHeading( void )
 461 {
 462     // Remove at a later date
 463     fgAPDataPtr APData;
 464
 465     APData = APDataGlobal;
 466     // end section
 467
 468     if ( APData->heading_hold ) {
 469         // turn off heading hold
 470         APData->heading_hold = false;
 471     } else {
 472         // turn on heading hold, lock at current heading
 473         APData->heading_hold = true;
 474         APData->TargetHeading = fgAPget_heading();
 475     }
 476
 477     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
 478             << APData->heading_hold << ") " << APData->TargetHeading );
 479 }
 480
 481
 482 void fgAPToggleAltitude( void )
 483 {
 484     // Remove at a later date
 485     fgAPDataPtr APData;
 486
 487     APData = APDataGlobal;
 488     // end section
 489
 490     if ( APData->altitude_hold ) {
 491         // turn off altitude hold
 492         APData->altitude_hold = false;
 493     } else {
 494         // turn on altitude hold, lock at current altitude
 495         APData->altitude_hold = true;
 496         APData->terrain_follow = false;
 497         APData->TargetAltitude = fgAPget_altitude();
 498         APData->alt_error_accum = 0.0;
 499         // alt_error_queue.erase( alt_error_queue.begin(),
 500         //                        alt_error_queue.end() );
 501     }
 502
 503     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
 504             << APData->altitude_hold << ") " << APData->TargetAltitude );
 505 }
 506
 507
 508 void fgAPToggleAutoThrottle ( void )
 509 {
 510     // Remove at a later date
 511     fgAPDataPtr APData;
 512
 513     APData = APDataGlobal;
 514     // end section
 515
 516     if ( APData->auto_throttle ) {
 517         // turn off altitude hold
 518         APData->auto_throttle = false;
 519     } else {
 520         // turn on terrain follow, lock at current agl
 521         APData->auto_throttle = true;
 522         APData->TargetSpeed = get_speed();
 523         APData->speed_error_accum = 0.0;
 524     }
 525
 526     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
 527             << APData->auto_throttle << ") " << APData->TargetSpeed );
 528 }
 529
 530 void fgAPToggleTerrainFollow( void )
 531 {
 532     // Remove at a later date
 533     fgAPDataPtr APData;
 534
 535     APData = APDataGlobal;
 536     // end section
 537
 538     if ( APData->terrain_follow ) {
 539         // turn off altitude hold
 540         APData->terrain_follow = false;
 541     } else {
 542         // turn on terrain follow, lock at current agl
 543         APData->terrain_follow = true;
 544         APData->altitude_hold = false;
 545         APData->TargetAGL = fgAPget_agl();
 546         APData->alt_error_accum = 0.0;
 547     }
 548
 549     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
 550             << APData->terrain_follow << ") " << APData->TargetAGL );
 551 }
 552
 553 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
 554 {
 555     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 556     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 557
 558     double m, b, y;             // the constants to find in y=mx+b
 559
 560     m=(y2-y1)/(x2-x1);  // calculate the m
 561
 562     b= y1- m * x1;              // calculate the b
 563
 564     y = m * x + b;              // the final calculation
 565
 566     return (y);
 567
 568 };
 569
 570 double NormalizeDegrees(double Input)
 571 {
 572     // normalize the input to the range (-180,180]
 573     // Input should not be greater than -360 to 360.  Current rules send the output to an undefined state.
 574     if (Input > 180)
 575         Input -= 360;
 576     if (Input <= -180)
 577         Input += 360;
 578
 579     return (Input);
 580 };
 581
 582
 583 // $Log$
 584 // Revision 1.15  1999/02/12 23:22:35  curt
 585 // Allow auto-throttle adjustment while active.
 586 //
 587 // Revision 1.14  1999/02/12 22:17:14  curt
 588 // Changes contributed by Norman Vine to allow adjustment of the autopilot
 589 // while it is activated.
 590 //