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