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