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