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 #ifdef NEEDNAMESPACESTD
  38 using namespace std;
  39 #endif
  40
  41 #include "autopilot.hxx"
  42
  43 #include <Include/fg_constants.h>
  44 #include <Debug/fg_debug.h>
  45
  46
  47 static list < double > alt_error_queue;
  48
  49
  50 // The below routines were copied right from hud.c ( I hate reinventing
  51 // the wheel more than necessary)
  52
  53 // The following routines obtain information concerntin the aircraft's
  54 // current state and return it to calling instrument display routines.
  55 // They should eventually be member functions of the aircraft.
  56 //
  57
  58 static double get_throttleval( void )
  59 {
  60         fgCONTROLS *pcontrols;
  61
  62   pcontrols = current_aircraft.controls;
  63   return pcontrols->throttle[0];     // Hack limiting to one engine
  64 }
  65
  66 static double get_aileronval( void )
  67 {
  68         fgCONTROLS *pcontrols;
  69
  70   pcontrols = current_aircraft.controls;
  71   return pcontrols->aileron;
  72 }
  73
  74 static double get_elevatorval( void )
  75 {
  76         fgCONTROLS *pcontrols;
  77
  78   pcontrols = current_aircraft.controls;
  79   return pcontrols->elevator;
  80 }
  81
  82 static double get_elev_trimval( void )
  83 {
  84         fgCONTROLS *pcontrols;
  85
  86   pcontrols = current_aircraft.controls;
  87   return pcontrols->elevator_trim;
  88 }
  89
  90 static double get_rudderval( void )
  91 {
  92         fgCONTROLS *pcontrols;
  93
  94   pcontrols = current_aircraft.controls;
  95   return pcontrols->rudder;
  96 }
  97
  98 static double get_speed( void )
  99 {
 100         fgFLIGHT *f;
 101
 102         f = current_aircraft.flight;
 103         return( FG_V_equiv_kts );    // Make an explicit function call.
 104 }
 105
 106 static double get_aoa( void )
 107 {
 108         fgFLIGHT *f;
 109
 110         f = current_aircraft.flight;
 111         return( FG_Gamma_vert_rad * RAD_TO_DEG );
 112 }
 113
 114 static double fgAPget_roll( void )
 115 {
 116         fgFLIGHT *f;
 117
 118         f = current_aircraft.flight;
 119         return( FG_Phi * RAD_TO_DEG );
 120 }
 121
 122 static double get_pitch( void )
 123 {
 124         fgFLIGHT *f;
 125
 126         f = current_aircraft.flight;
 127         return( FG_Theta );
 128 }
 129
 130 double fgAPget_heading( void )
 131 {
 132         fgFLIGHT *f;
 133
 134         f = current_aircraft.flight;
 135         return( FG_Psi * RAD_TO_DEG );
 136 }
 137
 138 static double fgAPget_altitude( void )
 139 {
 140         fgFLIGHT *f;
 141
 142         f = current_aircraft.flight;
 143
 144         return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
 145 }
 146
 147 static double fgAPget_climb( void )
 148 {
 149         fgFLIGHT *f;
 150
 151         f = current_aircraft.flight;
 152
 153         // return in meters per minute
 154         return( FG_Climb_Rate * FEET_TO_METER * 60 );
 155 }
 156
 157 static double get_sideslip( void )
 158 {
 159         fgFLIGHT *f;
 160
 161         f = current_aircraft.flight;
 162
 163         return( FG_Beta );
 164 }
 165
 166 // End of copied section.  ( thanks for the wheel :-)
 167
 168 // Local Prototype section
 169
 170 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
 171 double NormalizeDegrees( double Input);
 172
 173 // End Local ProtoTypes
 174
 175 fgAPDataPtr APDataGlobal;       // global variable holding the AP info
 176                                 // I want this gone.  Data should be in aircraft structure
 177
 178
 179
 180 void fgAPInit( fgAIRCRAFT *current_aircraft )
 181 {
 182         fgAPDataPtr APData ;
 183
 184         fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
 185
 186         APData  = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
 187
 188         if (APData == NULL) // I couldn't get the mem.  Dying
 189                 fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
 190
 191         APData->heading_hold = 0 ;     // turn the heading hold off
 192         APData->altitude_hold = 0 ;    // turn the altitude hold off
 193
 194         APData->TargetHeading = 0.0;    // default direction, due north
 195         APData->TargetAltitude = 3000;  // default altitude in meters
 196         APData->alt_error_accum = 0.0;
 197
 198         // These eventually need to be read from current_aircaft somehow.
 199
 200         APData->MaxRoll = 7;            // the maximum roll, in Deg
 201         APData->RollOut = 30;           // the deg from heading to start rolling out at, in Deg
 202         APData->MaxAileron= .1;         // how far can I move the aleron from center.
 203         APData->RollOutSmooth = 10;     // Smoothing distance for alerion control
 204
 205         //Remove at a later date
 206         APDataGlobal = APData;
 207
 208 };
 209
 210 int fgAPRun( void )
 211 {
 212     // Remove the following lines when the calling funcitons start
 213     // passing in the data pointer
 214
 215     fgAPDataPtr APData;
 216
 217     APData = APDataGlobal;
 218     // end section
 219
 220     // heading hold enabled?
 221     if ( APData->heading_hold == 1 ) {
 222         double RelHeading;
 223         double TargetRoll;
 224         double RelRoll;
 225         double AileronSet;
 226
 227         RelHeading =
 228             NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
 229         // figure out how far off we are from desired heading
 230
 231         // Now it is time to deterime how far we should be rolled.
 232         fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
 233
 234
 235         // Check if we are further from heading than the roll out point
 236         if ( fabs(RelHeading) > APData->RollOut ) {
 237             // set Target Roll to Max in desired direction
 238             if (RelHeading < 0 ) {
 239                 TargetRoll = 0-APData->MaxRoll;
 240             } else {
 241                 TargetRoll = APData->MaxRoll;
 242             }
 243         } else {
 244             // We have to calculate the Target roll
 245
 246             // This calculation engine thinks that the Target roll
 247             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 248             // -MaxRoll) I hope this works well.  If I get ambitious
 249             // some day this might become a fancier curve or
 250             // something.
 251
 252             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
 253                                             -APData->MaxRoll, APData->RollOut,
 254                                             APData->MaxRoll );
 255         }
 256
 257         // Target Roll has now been Found.
 258
 259         // Compare Target roll to Current Roll, Generate Rel Roll
 260
 261         fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
 262
 263         RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
 264
 265         // Check if we are further from heading than the roll out smooth point
 266         if ( fabs(RelRoll) > APData->RollOutSmooth ) {
 267             // set Target Roll to Max in desired direction
 268             if (RelRoll < 0 ) {
 269                 AileronSet = 0-APData->MaxAileron;
 270             } else {
 271                 AileronSet = APData->MaxAileron;
 272             }
 273         } else {
 274             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
 275                                             -APData->MaxAileron,
 276                                             APData->RollOutSmooth,
 277                                             APData->MaxAileron );
 278         }
 279
 280         fgAileronSet(AileronSet);
 281         fgRudderSet(0.0);
 282     }
 283
 284     // altitude hold enabled?
 285     if ( APData->altitude_hold == 1 ) {
 286         double error, size;
 287         double prop_error, int_error;
 288         double prop_adj, int_adj;
 289
 290         error = fgAPget_climb();
 291
 292         // push current error onto queue and add into accumulator
 293         alt_error_queue.push_back(error);
 294         APData->alt_error_accum += error;
 295
 296         // if queue size larger than 20 ... pop front and subtract
 297         // from accumulator
 298         size = alt_error_queue.size();
 299         if ( size > 20 ) {
 300             APData->alt_error_accum -= alt_error_queue.front();
 301             alt_error_queue.pop_front();
 302             size--;
 303         }
 304
 305         // calculate integral error, and adjustment amount
 306         int_error = APData->alt_error_accum / size;
 307         printf("int_error = %.2f\n", int_error);
 308         int_adj = int_error / 2000.0;
 309
 310         // caclulate proportional error
 311         prop_error = fgAPget_climb();
 312         prop_adj = prop_error / 2000.0;
 313
 314         fgElevSet( int_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     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
 382               APData->heading_hold,
 383               APData->TargetHeading);
 384 }
 385
 386
 387 void fgAPToggleAltitude( void )
 388 {
 389     // Remove at a later date
 390     fgAPDataPtr APData;
 391
 392     APData = APDataGlobal;
 393     // end section
 394
 395     if ( APData->altitude_hold ) {
 396         // turn off altitude hold
 397         APData->altitude_hold = 0;
 398     } else {
 399         // turn on altitude hold, lock at current altitude
 400         APData->altitude_hold = 1;
 401         APData->TargetAltitude = fgAPget_altitude();
 402     }
 403
 404     fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
 405               APData->altitude_hold,
 406               APData->TargetAltitude);
 407 }
 408
 409
 410 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
 411 {
 412     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 413     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 414
 415     double m, b, y;             // the constants to find in y=mx+b
 416
 417     m=(y2-y1)/(x2-x1);  // calculate the m
 418
 419     b= y1- m * x1;              // calculate the b
 420
 421     y = m * x + b;              // the final calculation
 422
 423     return (y);
 424
 425 };
 426
 427 double NormalizeDegrees(double Input)
 428 {
 429     // normalize the input to the range (-180,180]
 430     // Input should not be greater than -360 to 360.  Current rules send the output to an undefined state.
 431     if (Input > 180)
 432         Input -= 360;
 433     if (Input <= -180)
 434         Input += 360;
 435
 436     return (Input);
 437 };