src/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
  23
  24 #ifdef HAVE_CONFIG_H
  25 #  include <config.h>
  26 #endif
  27
  28 #include <assert.h>
  29 #include <stdlib.h>
  30
  31 #include <Scenery/scenery.hxx>
  32
  33 #include "autopilot.hxx"
  34
  35 #include <Include/fg_constants.h>
  36 #include <Debug/logstream.hxx>
  37 #include <Main/options.hxx>
  38 #include <GUI/gui.h>
  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     return( current_aircraft.fdm_state->get_V_equiv_kts() );
  52 }
  53
  54 static double get_aoa( void )
  55 {
  56     return( current_aircraft.fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
  57 }
  58
  59 static double fgAPget_roll( void )
  60 {
  61     return( current_aircraft.fdm_state->get_Phi() * RAD_TO_DEG );
  62 }
  63
  64 static double get_pitch( void )
  65 {
  66     return( current_aircraft.fdm_state->get_Theta() );
  67 }
  68
  69 double fgAPget_heading( void )
  70 {
  71     return( current_aircraft.fdm_state->get_Psi() * RAD_TO_DEG );
  72 }
  73
  74 static double fgAPget_altitude( void )
  75 {
  76     return( current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER );
  77 }
  78
  79 static double fgAPget_climb( void )
  80 {
  81     // return in meters per minute
  82     return( current_aircraft.fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
  83 }
  84
  85 static double get_sideslip( void )
  86 {
  87     return( current_aircraft.fdm_state->get_Beta() );
  88 }
  89
  90 static double fgAPget_agl( void )
  91 {
  92     double agl;
  93
  94     agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER
  95         - scenery.cur_elev;
  96
  97     return( agl );
  98 }
  99
 100 // End of copied section.  ( thanks for the wheel :-)
 101
 102 // Local Prototype section
 103
 104 double LinearExtrapolate( double x, double x1, double y1, double x2, double y2);
 105 double NormalizeDegrees( double Input);
 106
 107 // End Local ProtoTypes
 108
 109 fgAPDataPtr APDataGlobal;       // global variable holding the AP info
 110 // I want this gone.  Data should be in aircraft structure
 111
 112
 113 bool fgAPHeadingEnabled( void )
 114 {
 115     fgAPDataPtr APData;
 116
 117     APData = APDataGlobal;
 118
 119     // heading hold enabled?
 120     return APData->heading_hold;
 121 }
 122
 123 bool fgAPAltitudeEnabled( void )
 124 {
 125     fgAPDataPtr APData;
 126
 127     APData = APDataGlobal;
 128
 129     // altitude hold or terrain follow enabled?
 130     return APData->altitude_hold;
 131 }
 132
 133 bool fgAPTerrainFollowEnabled( void )
 134 {
 135     fgAPDataPtr APData;
 136
 137     APData = APDataGlobal;
 138
 139     // altitude hold or terrain follow enabled?
 140     return APData->terrain_follow ;
 141 }
 142
 143 bool fgAPAutoThrottleEnabled( void )
 144 {
 145     fgAPDataPtr APData;
 146
 147     APData = APDataGlobal;
 148
 149     // autothrottle enabled?
 150     return APData->auto_throttle;
 151 }
 152
 153 void fgAPAltitudeAdjust( double inc )
 154 {
 155     // Remove at a later date
 156     fgAPDataPtr APData = APDataGlobal;
 157     // end section
 158
 159     double target_alt, target_agl;
 160
 161     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 162         target_alt = APData->TargetAltitude * METER_TO_FEET;
 163         target_agl = APData->TargetAGL * METER_TO_FEET;
 164     } else {
 165         target_alt = APData->TargetAltitude;
 166         target_agl = APData->TargetAGL;
 167     }
 168
 169     target_alt = (int)(target_alt / inc) * inc + inc;
 170     target_agl = (int)(target_agl / inc) * inc + inc;
 171
 172     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 173         target_alt *= FEET_TO_METER;
 174         target_agl *= FEET_TO_METER;
 175     }
 176
 177     APData->TargetAltitude = target_alt;
 178     APData->TargetAGL = target_agl;
 179 }
 180
 181 void fgAPAltitudeSet( double new_altitude ) {
 182     // Remove at a later date
 183     fgAPDataPtr APData = APDataGlobal;
 184     // end section
 185     double target_alt = new_altitude;
 186
 187     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 188         target_alt = new_altitude * FEET_TO_METER;
 189
 190     if( target_alt < scenery.cur_elev )
 191         target_alt = scenery.cur_elev;
 192
 193     APData->TargetAltitude = target_alt;
 194 }
 195
 196 void fgAPHeadingAdjust( double inc )
 197 {
 198     fgAPDataPtr APData;
 199     APData = APDataGlobal;
 200
 201     double target = (int)(APData->TargetHeading / inc) * inc + inc;
 202
 203     APData->TargetHeading = NormalizeDegrees(target);
 204 }
 205
 206 void fgAPHeadingSet( double new_heading ) {
 207     fgAPDataPtr APData = APDataGlobal;
 208
 209     new_heading = NormalizeDegrees( new_heading );
 210     APData->TargetHeading = new_heading;
 211 }
 212
 213 void fgAPAutoThrottleAdjust( double inc )
 214 {
 215     fgAPDataPtr APData;
 216     APData = APDataGlobal;
 217
 218     double target = (int)(APData->TargetSpeed / inc) * inc + inc;
 219
 220     APData->TargetSpeed = target;
 221 }
 222
 223 void fgAPReset(void)
 224 {
 225     fgAPDataPtr APData = APDataGlobal;
 226
 227     if( fgAPTerrainFollowEnabled() )
 228         fgAPToggleTerrainFollow( );
 229
 230     if( fgAPAltitudeEnabled() )
 231         fgAPToggleAltitude();
 232
 233     if( fgAPHeadingEnabled() )
 234         fgAPToggleHeading();
 235
 236     if( fgAPAutoThrottleEnabled() )
 237         fgAPToggleAutoThrottle();
 238
 239     APData->TargetHeading = 0.0;     // default direction, due north
 240     APData->TargetAltitude = 3000;   // default altitude in meters
 241     APData->alt_error_accum = 0.0;
 242 }
 243
 244 #define mySlider puSlider
 245
 246 /// These statics will eventually go into the class
 247 /// they are just here while I am experimenting -- NHV :-)
 248
 249 static double MaxRollAdjust;       // MaxRollAdjust       = 2 * APData->MaxRoll;
 250 static double RollOutAdjust;       // RollOutAdjust       = 2 * APData->RollOut;
 251 static double MaxAileronAdjust;    // MaxAileronAdjust    = 2 * APData->MaxAileron;
 252 static double RollOutSmoothAdjust; // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
 253
 254 static float MaxRollValue;         // 0.1 -> 1.0
 255 static float RollOutValue;
 256 static float MaxAileronValue;
 257 static float RollOutSmoothValue;
 258
 259 static float TmpMaxRollValue;      // for cancel operation
 260 static float TmpRollOutValue;
 261 static float TmpMaxAileronValue;
 262 static float TmpRollOutSmoothValue;
 263
 264 static puDialogBox     *APAdjustDialog;
 265 static puFrame         *APAdjustFrame;
 266 static puText          *APAdjustDialogMessage;
 267 static puFont          APAdjustLegendFont;
 268 static puFont          APAdjustLabelFont;
 269
 270 static puOneShot       *APAdjustOkButton;
 271 static puOneShot       *APAdjustResetButton;
 272 static puOneShot       *APAdjustCancelButton;
 273
 274 //static puButton        *APAdjustDragButton;
 275
 276 static puText          *APAdjustMaxRollTitle;
 277 static puText          *APAdjustRollOutTitle;
 278 static puText          *APAdjustMaxAileronTitle;
 279 static puText          *APAdjustRollOutSmoothTitle;
 280
 281 static puText          *APAdjustMaxAileronText;
 282 static puText          *APAdjustMaxRollText;
 283 static puText          *APAdjustRollOutText;
 284 static puText          *APAdjustRollOutSmoothText;
 285
 286 static mySlider        *APAdjustHS0;
 287 static mySlider        *APAdjustHS1;
 288 static mySlider        *APAdjustHS2;
 289 static mySlider        *APAdjustHS3;
 290
 291 static char SliderText[4][8];
 292
 293 // THIS NEEDS IMPROVEMENT !!!!!!!!!!!!!
 294 static int scan_number(char *s, double *new_value)
 295 {
 296     int ret = 0;
 297     char WordBuf[64];
 298     char *cptr = s;
 299     char *WordBufPtr = WordBuf;
 300
 301     if (*cptr == '+')
 302         cptr++;
 303     if (*cptr == '-') {
 304         *WordBufPtr++ = *cptr++;
 305     }
 306     while (isdigit(*cptr) ) {
 307         *WordBufPtr++ = *cptr++;
 308         ret = 1;
 309     }
 310     if (*cptr == '.')
 311         *WordBufPtr++ = *cptr++;  // put the '.' into the string
 312     while (isdigit(*cptr)) {
 313         *WordBufPtr++ = *cptr++;
 314         ret = 1;
 315     }
 316     if( ret == 1 ) {
 317         *WordBufPtr = '\0';
 318         sscanf(WordBuf, "%lf", new_value);
 319     }
 320
 321     return(ret);
 322 } // scan_number
 323
 324
 325 ///////// AutoPilot New Heading Dialog
 326
 327 static puDialogBox     *ApHeadingDialog;
 328 static puFrame         *ApHeadingDialogFrame;
 329 static puText          *ApHeadingDialogMessage;
 330 static puInput         *ApHeadingDialogInput;
 331 static puOneShot       *ApHeadingDialogOkButton;
 332 static puOneShot       *ApHeadingDialogCancelButton;
 333
 334 void ApHeadingDialog_Cancel(puObject *)
 335 {
 336     ApHeadingDialogInput->rejectInput();
 337     FG_POP_PUI_DIALOG( ApHeadingDialog );
 338 }
 339
 340 void ApHeadingDialog_OK (puObject *me)
 341 {
 342     int error = 0;
 343     char *c;
 344     string s;
 345     ApHeadingDialogInput -> getValue( &c );
 346
 347     if( strlen(c) ) {
 348         double NewHeading;
 349         if( scan_number( c, &NewHeading ) )
 350             {
 351                 if(!fgAPHeadingEnabled())
 352                     fgAPToggleHeading();
 353                 fgAPHeadingSet( NewHeading );
 354             } else {
 355                 error = 1;
 356                 s = c;
 357                 s += " is not a valid number.";
 358             }
 359     }
 360     ApHeadingDialog_Cancel(me);
 361     if( error )  mkDialog(s.c_str());
 362 }
 363
 364 void NewHeading(puObject *cb)
 365 {
 366     //  string ApHeadingLabel( "Enter New Heading" );
 367     //  ApHeadingDialogMessage  -> setLabel(ApHeadingLabel.c_str());
 368     ApHeadingDialogInput    -> acceptInput();
 369     FG_PUSH_PUI_DIALOG( ApHeadingDialog );
 370 }
 371
 372 void NewHeadingInit(void)
 373 {
 374     //  printf("NewHeadingInit\n");
 375     char NewHeadingLabel[] = "Enter New Heading";
 376     char *s;
 377
 378     float heading = fgAPget_heading();
 379     int len = 260/2 -
 380         (puGetStringWidth( puGetDefaultLabelFont(), NewHeadingLabel ) /2 );
 381
 382     ApHeadingDialog = new puDialogBox (150, 50);
 383     {
 384         ApHeadingDialogFrame   = new puFrame (0, 0, 260, 150);
 385
 386         ApHeadingDialogMessage = new puText   (len, 110);
 387         ApHeadingDialogMessage    -> setDefaultValue (NewHeadingLabel);
 388         ApHeadingDialogMessage    -> getDefaultValue (&s);
 389         ApHeadingDialogMessage    -> setLabel        (s);
 390
 391         ApHeadingDialogInput   = new puInput  ( 50, 70, 210, 100 );
 392         ApHeadingDialogInput   ->    setValue ( heading );
 393
 394         ApHeadingDialogOkButton     =  new puOneShot         (50, 10, 110, 50);
 395         ApHeadingDialogOkButton     ->     setLegend         (gui_msg_OK);
 396         ApHeadingDialogOkButton     ->     makeReturnDefault (TRUE);
 397         ApHeadingDialogOkButton     ->     setCallback       (ApHeadingDialog_OK);
 398
 399         ApHeadingDialogCancelButton =  new puOneShot         (140, 10, 210, 50);
 400         ApHeadingDialogCancelButton ->     setLegend         (gui_msg_CANCEL);
 401         ApHeadingDialogCancelButton ->     setCallback       (ApHeadingDialog_Cancel);
 402
 403     }
 404     FG_FINALIZE_PUI_DIALOG( ApHeadingDialog );
 405 }
 406
 407 ///////// AutoPilot New Altitude Dialog
 408
 409 static puDialogBox     *ApAltitudeDialog = 0;
 410 static puFrame         *ApAltitudeDialogFrame = 0;
 411 static puText          *ApAltitudeDialogMessage = 0;
 412 static puInput         *ApAltitudeDialogInput = 0;
 413
 414 static puOneShot       *ApAltitudeDialogOkButton = 0;
 415 static puOneShot       *ApAltitudeDialogCancelButton = 0;
 416
 417 void ApAltitudeDialog_Cancel(puObject *)
 418 {
 419     ApAltitudeDialogInput -> rejectInput();
 420     FG_POP_PUI_DIALOG( ApAltitudeDialog );
 421 }
 422
 423 void ApAltitudeDialog_OK (puObject *me)
 424 {
 425     int error = 0;
 426     string s;
 427     char *c;
 428     ApAltitudeDialogInput->getValue( &c );
 429
 430     if( strlen( c ) ) {
 431         double NewAltitude;
 432         if( scan_number( c, &NewAltitude) )
 433             {
 434                 if(!(fgAPAltitudeEnabled()))
 435                     fgAPToggleAltitude();
 436                 fgAPAltitudeSet( NewAltitude );
 437             } else {
 438                 error = 1;
 439                 s = c;
 440                 s += " is not a valid number.";
 441             }
 442     }
 443     ApAltitudeDialog_Cancel(me);
 444     if( error )  mkDialog(s.c_str());
 445 }
 446
 447 void NewAltitude(puObject *cb)
 448 {
 449     ApAltitudeDialogInput -> acceptInput();
 450     FG_PUSH_PUI_DIALOG( ApAltitudeDialog );
 451 }
 452
 453 void NewAltitudeInit(void)
 454 {
 455     //  printf("NewAltitudeInit\n");
 456     char NewAltitudeLabel[] = "Enter New Altitude";
 457     char *s;
 458
 459     float alt = current_aircraft.fdm_state->get_Altitude();
 460     int len = 260/2 -
 461         (puGetStringWidth( puGetDefaultLabelFont(), NewAltitudeLabel )/2);
 462
 463     ApAltitudeDialog = new puDialogBox (150, 50);
 464     {
 465         ApAltitudeDialogFrame   = new puFrame  (0, 0, 260, 150);
 466         ApAltitudeDialogMessage = new puText   (len, 110);
 467         ApAltitudeDialogMessage    -> setDefaultValue (NewAltitudeLabel);
 468         ApAltitudeDialogMessage    -> getDefaultValue (&s);
 469         ApAltitudeDialogMessage    -> setLabel (s);
 470
 471         ApAltitudeDialogInput   = new puInput  ( 50, 70, 210, 100 );
 472         ApAltitudeDialogInput      -> setValue ( alt );
 473         // Uncomment the next line to have input active on startup
 474         // ApAltitudeDialogInput   ->    acceptInput       ( );
 475         // cursor at begining or end of line ?
 476         //len = strlen(s);
 477         //      len = 0;
 478         //      ApAltitudeDialogInput   ->    setCursor         ( len );
 479         //      ApAltitudeDialogInput   ->    setSelectRegion   ( 5, 9 );
 480
 481         ApAltitudeDialogOkButton     =  new puOneShot         (50, 10, 110, 50);
 482         ApAltitudeDialogOkButton     ->     setLegend         (gui_msg_OK);
 483         ApAltitudeDialogOkButton     ->     makeReturnDefault (TRUE);
 484         ApAltitudeDialogOkButton     ->     setCallback       (ApAltitudeDialog_OK);
 485
 486         ApAltitudeDialogCancelButton =  new puOneShot         (140, 10, 210, 50);
 487         ApAltitudeDialogCancelButton ->     setLegend         (gui_msg_CANCEL);
 488         ApAltitudeDialogCancelButton ->     setCallback       (ApAltitudeDialog_Cancel);
 489
 490     }
 491     FG_FINALIZE_PUI_DIALOG( ApAltitudeDialog );
 492 }
 493
 494 /////// simple AutoPilot GAIN / LIMITS ADJUSTER
 495
 496
 497 #define fgAP_CLAMP(val,min,max) \
 498 ( (val) = (val) > (max) ? (max) : (val) < (min) ? (min) : (val) )
 499
 500     static void maxroll_adj(puObject *hs)
 501 {
 502     float val ;
 503     fgAPDataPtr APData;
 504     APData = APDataGlobal;
 505
 506     hs -> getValue ( &val ) ;
 507     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 508     //    printf ( "maxroll_adj( %p ) %f %f\n", hs, val, MaxRollAdjust * val ) ;
 509     APData->MaxRoll = MaxRollAdjust * val;
 510     sprintf(SliderText[0],"%05.2f", APData->MaxRoll );
 511     APAdjustMaxRollText -> setLabel ( SliderText[0] ) ;
 512 }
 513
 514 static void rollout_adj(puObject *hs)
 515 {
 516     float val ;
 517     fgAPDataPtr APData;
 518     APData = APDataGlobal;
 519
 520     hs -> getValue ( &val ) ;
 521     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 522     //    printf ( "rollout_adj( %p ) %f %f\n", hs, val, RollOutAdjust * val ) ;
 523     APData->RollOut = RollOutAdjust * val;
 524     sprintf(SliderText[1],"%05.2f", APData->RollOut );
 525     APAdjustRollOutText -> setLabel ( SliderText[1] );
 526 }
 527
 528 static void maxaileron_adj( puObject *hs )
 529 {
 530     float val ;
 531     fgAPDataPtr APData;
 532     APData = APDataGlobal;
 533
 534     hs -> getValue ( &val ) ;
 535     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 536     //    printf ( "maxaileron_adj( %p ) %f %f\n", hs, val, MaxAileronAdjust * val ) ;
 537     APData->MaxAileron = MaxAileronAdjust * val;
 538     sprintf(SliderText[3],"%05.2f", APData->MaxAileron );
 539     APAdjustMaxAileronText -> setLabel ( SliderText[3] );
 540 }
 541
 542 static void rolloutsmooth_adj( puObject *hs )
 543 {
 544     float val ;
 545     fgAPDataPtr APData;
 546     APData = APDataGlobal;
 547
 548     hs -> getValue ( &val ) ;
 549     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 550     //    printf ( "rolloutsmooth_adj( %p ) %f %f\n", hs, val, RollOutSmoothAdjust * val ) ;
 551     APData->RollOutSmooth = RollOutSmoothAdjust * val;
 552     sprintf(SliderText[2],"%5.2f", APData->RollOutSmooth );
 553     APAdjustRollOutSmoothText -> setLabel ( SliderText[2] );
 554
 555 }
 556
 557 static void goAwayAPAdjust (puObject *)
 558 {
 559     FG_POP_PUI_DIALOG( APAdjustDialog );
 560 }
 561
 562 void cancelAPAdjust(puObject *self)
 563 {
 564     fgAPDataPtr APData  = APDataGlobal;
 565
 566     APData->MaxRoll       = TmpMaxRollValue;
 567     APData->RollOut       = TmpRollOutValue;
 568     APData->MaxAileron    = TmpMaxAileronValue;
 569     APData->RollOutSmooth = TmpRollOutSmoothValue;
 570
 571     goAwayAPAdjust(self);
 572 }
 573
 574 void resetAPAdjust(puObject *self)
 575 {
 576     fgAPDataPtr APData  = APDataGlobal;
 577
 578     APData->MaxRoll       = MaxRollAdjust / 2;
 579     APData->RollOut       = RollOutAdjust / 2;
 580     APData->MaxAileron    = MaxAileronAdjust / 2;
 581     APData->RollOutSmooth = RollOutSmoothAdjust / 2;
 582
 583     FG_POP_PUI_DIALOG( APAdjustDialog );
 584
 585     fgAPAdjust( self );
 586 }
 587
 588 void fgAPAdjust( puObject * )
 589 {
 590     fgAPDataPtr APData  = APDataGlobal;
 591
 592     TmpMaxRollValue        = APData->MaxRoll;
 593     TmpRollOutValue        = APData->RollOut;
 594     TmpMaxAileronValue     = APData->MaxAileron;
 595     TmpRollOutSmoothValue  = APData->RollOutSmooth;
 596
 597     MaxRollValue        = APData->MaxRoll       / MaxRollAdjust;
 598     RollOutValue        = APData->RollOut       / RollOutAdjust;
 599     MaxAileronValue     = APData->MaxAileron    / MaxAileronAdjust;
 600     RollOutSmoothValue  = APData->RollOutSmooth / RollOutSmoothAdjust;
 601
 602     APAdjustHS0 -> setValue ( MaxRollValue ) ;
 603     APAdjustHS1 -> setValue ( RollOutValue ) ;
 604     APAdjustHS2 -> setValue ( RollOutSmoothValue ) ;
 605     APAdjustHS3 -> setValue ( MaxAileronValue ) ;
 606
 607     FG_PUSH_PUI_DIALOG( APAdjustDialog );
 608 }
 609
 610
 611 // Done once at system initialization
 612 // Done once at system initialization
 613 void fgAPAdjustInit( void ) {
 614
 615     //  printf("fgAPAdjustInit\n");
 616 #define HORIZONTAL  FALSE
 617
 618     int DialogX = 40;
 619     int DialogY = 100;
 620     int DialogWidth = 230;
 621
 622     char Label[] =  "AutoPilot Adjust";
 623     char *s;
 624
 625     fgAPDataPtr APData = APDataGlobal;
 626
 627     int labelX = (DialogWidth / 2) -
 628         (puGetStringWidth( puGetDefaultLabelFont(), Label ) / 2);
 629     labelX -= 30;  // KLUDGEY
 630
 631     int nSliders = 4;
 632     int slider_x = 10;
 633     int slider_y = 55;
 634     int slider_width = 210;
 635     int slider_title_x = 15;
 636     int slider_value_x = 160;
 637     float slider_delta = 0.1f;
 638
 639     TmpMaxRollValue       = APData-> MaxRoll;
 640     TmpRollOutValue       = APData-> RollOut;
 641     TmpMaxAileronValue    = APData-> MaxAileron;
 642     TmpRollOutSmoothValue = APData-> RollOutSmooth;
 643
 644     MaxRollValue       = APData-> MaxRoll / MaxRollAdjust;
 645     RollOutValue       = APData-> RollOut / RollOutAdjust;
 646     MaxAileronValue    = APData-> MaxAileron / MaxAileronAdjust;
 647     RollOutSmoothValue = APData-> RollOutSmooth / RollOutSmoothAdjust;
 648
 649     puGetDefaultFonts (  &APAdjustLegendFont,  &APAdjustLabelFont );
 650     APAdjustDialog = new puDialogBox ( DialogX, DialogY ); {
 651         int horiz_slider_height = puGetStringHeight (APAdjustLabelFont) +
 652             puGetStringDescender (APAdjustLabelFont) +
 653             PUSTR_TGAP + PUSTR_BGAP + 5;
 654
 655         APAdjustFrame = new puFrame ( 0, 0,
 656                                       DialogWidth, 85 + nSliders * horiz_slider_height );
 657
 658         APAdjustDialogMessage = new puText ( labelX, 52 + nSliders * horiz_slider_height );
 659         APAdjustDialogMessage -> setDefaultValue ( Label );
 660         APAdjustDialogMessage -> getDefaultValue ( &s );
 661         APAdjustDialogMessage -> setLabel        ( s );
 662
 663         APAdjustHS0 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 664         APAdjustHS0-> setDelta ( slider_delta ) ;
 665         APAdjustHS0-> setValue ( MaxRollValue ) ;
 666         APAdjustHS0-> setCBMode ( PUSLIDER_DELTA ) ;
 667         APAdjustHS0-> setCallback ( maxroll_adj ) ;
 668
 669         sprintf( SliderText[ 0 ], "%05.2f", APData->MaxRoll );
 670         APAdjustMaxRollTitle = new puText ( slider_title_x, slider_y ) ;
 671         APAdjustMaxRollTitle-> setDefaultValue ( "MaxRoll" ) ;
 672         APAdjustMaxRollTitle-> getDefaultValue ( &s ) ;
 673         APAdjustMaxRollTitle-> setLabel ( s ) ;
 674         APAdjustMaxRollText = new puText ( slider_value_x, slider_y ) ;
 675         APAdjustMaxRollText-> setLabel ( SliderText[ 0 ] ) ;
 676
 677         slider_y += horiz_slider_height;
 678
 679         APAdjustHS1 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 680         APAdjustHS1-> setDelta ( slider_delta ) ;
 681         APAdjustHS1-> setValue ( RollOutValue ) ;
 682         APAdjustHS1-> setCBMode ( PUSLIDER_DELTA ) ;
 683         APAdjustHS1-> setCallback ( rollout_adj ) ;
 684
 685         sprintf( SliderText[ 1 ], "%05.2f", APData->RollOut );
 686         APAdjustRollOutTitle = new puText ( slider_title_x, slider_y ) ;
 687         APAdjustRollOutTitle-> setDefaultValue ( "AdjustRollOut" ) ;
 688         APAdjustRollOutTitle-> getDefaultValue ( &s ) ;
 689         APAdjustRollOutTitle-> setLabel ( s ) ;
 690         APAdjustRollOutText = new puText ( slider_value_x, slider_y ) ;
 691         APAdjustRollOutText-> setLabel ( SliderText[ 1 ] );
 692
 693         slider_y += horiz_slider_height;
 694
 695         APAdjustHS2 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 696         APAdjustHS2-> setDelta ( slider_delta ) ;
 697         APAdjustHS2-> setValue ( RollOutSmoothValue ) ;
 698         APAdjustHS2-> setCBMode ( PUSLIDER_DELTA ) ;
 699         APAdjustHS2-> setCallback ( rolloutsmooth_adj ) ;
 700
 701         sprintf( SliderText[ 2 ], "%5.2f", APData->RollOutSmooth );
 702         APAdjustRollOutSmoothTitle = new puText ( slider_title_x, slider_y ) ;
 703         APAdjustRollOutSmoothTitle-> setDefaultValue ( "RollOutSmooth" ) ;
 704         APAdjustRollOutSmoothTitle-> getDefaultValue ( &s ) ;
 705         APAdjustRollOutSmoothTitle-> setLabel ( s ) ;
 706         APAdjustRollOutSmoothText = new puText ( slider_value_x, slider_y ) ;
 707         APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] );
 708
 709         slider_y += horiz_slider_height;
 710
 711         APAdjustHS3 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 712         APAdjustHS3-> setDelta ( slider_delta ) ;
 713         APAdjustHS3-> setValue ( MaxAileronValue ) ;
 714         APAdjustHS3-> setCBMode ( PUSLIDER_DELTA ) ;
 715         APAdjustHS3-> setCallback ( maxaileron_adj ) ;
 716
 717         sprintf( SliderText[ 3 ], "%05.2f", APData->MaxAileron );
 718         APAdjustMaxAileronTitle = new puText ( slider_title_x, slider_y ) ;
 719         APAdjustMaxAileronTitle-> setDefaultValue ( "MaxAileron" ) ;
 720         APAdjustMaxAileronTitle-> getDefaultValue ( &s ) ;
 721         APAdjustMaxAileronTitle-> setLabel ( s ) ;
 722         APAdjustMaxAileronText = new puText ( slider_value_x, slider_y ) ;
 723         APAdjustMaxAileronText-> setLabel ( SliderText[ 3 ] );
 724
 725         APAdjustOkButton = new puOneShot ( 10, 10, 60, 50 );
 726         APAdjustOkButton-> setLegend ( gui_msg_OK );
 727         APAdjustOkButton-> makeReturnDefault ( TRUE );
 728         APAdjustOkButton-> setCallback ( goAwayAPAdjust );
 729
 730         APAdjustCancelButton = new puOneShot ( 70, 10, 150, 50 );
 731         APAdjustCancelButton-> setLegend ( gui_msg_CANCEL );
 732         APAdjustCancelButton-> setCallback ( cancelAPAdjust );
 733
 734         APAdjustResetButton = new puOneShot ( 160, 10, 220, 50 );
 735         APAdjustResetButton-> setLegend ( gui_msg_RESET );
 736         APAdjustResetButton-> setCallback ( resetAPAdjust );
 737     }
 738     FG_FINALIZE_PUI_DIALOG( APAdjustDialog );
 739
 740 #undef HORIZONTAL
 741 }
 742
 743 void fgAPInit( fgAIRCRAFT *current_aircraft )
 744 {
 745     fgAPDataPtr APData ;
 746
 747     FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
 748
 749     APData  = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
 750
 751     if (APData == NULL) {
 752         // I couldn't get the mem.  Dying
 753         FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
 754         exit(-1);
 755     }
 756
 757     APData->heading_hold = false ;     // turn the heading hold off
 758     APData->altitude_hold = false ;    // turn the altitude hold off
 759
 760     APData->TargetHeading = 0.0;    // default direction, due north
 761     APData->TargetAltitude = 3000;  // default altitude in meters
 762     APData->alt_error_accum = 0.0;
 763
 764     // These eventually need to be read from current_aircaft somehow.
 765
 766 #if 0
 767     // Original values
 768     // the maximum roll, in Deg
 769     APData->MaxRoll = 7;
 770     // the deg from heading to start rolling out at, in Deg
 771     APData->RollOut = 30;
 772     // how far can I move the aleron from center.
 773     APData->MaxAileron= .1;
 774     // Smoothing distance for alerion control
 775     APData->RollOutSmooth = 10;
 776 #endif
 777
 778     // the maximum roll, in Deg
 779     APData->MaxRoll = 20;
 780
 781     // the deg from heading to start rolling out at, in Deg
 782     APData->RollOut = 20;
 783
 784     // how far can I move the aleron from center.
 785     APData->MaxAileron= .2;
 786
 787     // Smoothing distance for alerion control
 788     APData->RollOutSmooth = 10;
 789
 790     //Remove at a later date
 791     APDataGlobal = APData;
 792
 793 #if !defined( USING_SLIDER_CLASS )
 794     MaxRollAdjust       = 2 * APData->MaxRoll;
 795     RollOutAdjust       = 2 * APData->RollOut;
 796     MaxAileronAdjust    = 2 * APData->MaxAileron;
 797     RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
 798 #endif // !defined( USING_SLIDER_CLASS )
 799
 800     fgAPAdjustInit( ) ;
 801     NewHeadingInit();
 802     NewAltitudeInit();
 803 };
 804
 805 int fgAPRun( void )
 806 {
 807     // Remove the following lines when the calling funcitons start
 808     // passing in the data pointer
 809
 810     fgAPDataPtr APData;
 811
 812     APData = APDataGlobal;
 813     // end section
 814
 815     // heading hold enabled?
 816     if ( APData->heading_hold == true ) {
 817         double RelHeading;
 818         double TargetRoll;
 819         double RelRoll;
 820         double AileronSet;
 821
 822         RelHeading =
 823             NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
 824         // figure out how far off we are from desired heading
 825
 826         // Now it is time to deterime how far we should be rolled.
 827         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
 828
 829
 830         // Check if we are further from heading than the roll out point
 831         if ( fabs(RelHeading) > APData->RollOut ) {
 832             // set Target Roll to Max in desired direction
 833             if (RelHeading < 0 ) {
 834                 TargetRoll = 0-APData->MaxRoll;
 835             } else {
 836                 TargetRoll = APData->MaxRoll;
 837             }
 838         } else {
 839             // We have to calculate the Target roll
 840
 841             // This calculation engine thinks that the Target roll
 842             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 843             // -MaxRoll) I hope this works well.  If I get ambitious
 844             // some day this might become a fancier curve or
 845             // something.
 846
 847             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
 848                                             -APData->MaxRoll, APData->RollOut,
 849                                             APData->MaxRoll );
 850         }
 851
 852         // Target Roll has now been Found.
 853
 854         // Compare Target roll to Current Roll, Generate Rel Roll
 855
 856         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
 857
 858         RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
 859
 860         // Check if we are further from heading than the roll out smooth point
 861         if ( fabs(RelRoll) > APData->RollOutSmooth ) {
 862             // set Target Roll to Max in desired direction
 863             if (RelRoll < 0 ) {
 864                 AileronSet = 0-APData->MaxAileron;
 865             } else {
 866                 AileronSet = APData->MaxAileron;
 867             }
 868         } else {
 869             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
 870                                             -APData->MaxAileron,
 871                                             APData->RollOutSmooth,
 872                                             APData->MaxAileron );
 873         }
 874
 875         controls.set_aileron( AileronSet );
 876         controls.set_rudder( 0.0 );
 877     }
 878
 879     // altitude hold or terrain follow enabled?
 880     if ( APData->altitude_hold || APData->terrain_follow ) {
 881         double speed, max_climb, error;
 882         double prop_error, int_error;
 883         double prop_adj, int_adj, total_adj;
 884
 885         if ( APData->altitude_hold ) {
 886             // normal altitude hold
 887             APData->TargetClimbRate =
 888                 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
 889         } else if ( APData->terrain_follow ) {
 890             // brain dead ground hugging with no look ahead
 891             APData->TargetClimbRate =
 892                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
 893         } else {
 894             // just try to zero out rate of climb ...
 895             APData->TargetClimbRate = 0.0;
 896         }
 897
 898         speed = get_speed();
 899
 900         if ( speed < 90.0 ) {
 901             max_climb = 0.0;
 902         } else if ( speed < 100.0 ) {
 903             max_climb = (speed - 90.0) * 20;
 904         } else {
 905             max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
 906         }
 907
 908         if ( APData->TargetClimbRate > max_climb ) {
 909             APData->TargetClimbRate = max_climb;
 910         }
 911
 912         if ( APData->TargetClimbRate < -400.0 ) {
 913             APData->TargetClimbRate = -400.0;
 914         }
 915
 916         error = fgAPget_climb() - APData->TargetClimbRate;
 917
 918         // accumulate the error under the curve ... this really should
 919         // be *= delta t
 920         APData->alt_error_accum += error;
 921
 922         // calculate integral error, and adjustment amount
 923         int_error = APData->alt_error_accum;
 924         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 925         int_adj = int_error / 8000.0;
 926
 927         // caclulate proportional error
 928         prop_error = error;
 929         prop_adj = prop_error / 2000.0;
 930
 931         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 932         if ( total_adj >  0.6 ) { total_adj =  0.6; }
 933         if ( total_adj < -0.2 ) { total_adj = -0.2; }
 934
 935         controls.set_elevator( total_adj );
 936     }
 937
 938     // auto throttle enabled?
 939     if ( APData->auto_throttle ) {
 940         double error;
 941         double prop_error, int_error;
 942         double prop_adj, int_adj, total_adj;
 943
 944         error = APData->TargetSpeed - get_speed();
 945
 946         // accumulate the error under the curve ... this really should
 947         // be *= delta t
 948         APData->speed_error_accum += error;
 949         if ( APData->speed_error_accum > 2000.0 ) {
 950             APData->speed_error_accum = 2000.0;
 951         }
 952         if ( APData->speed_error_accum < -2000.0 ) {
 953             APData->speed_error_accum = -2000.0;
 954         }
 955
 956         // calculate integral error, and adjustment amount
 957         int_error = APData->speed_error_accum;
 958
 959         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 960         int_adj = int_error / 200.0;
 961
 962         // caclulate proportional error
 963         prop_error = error;
 964         prop_adj = 0.5 + prop_error / 50.0;
 965
 966         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 967         if ( total_adj > 1.0 ) { total_adj = 1.0; }
 968         if ( total_adj < 0.0 ) { total_adj = 0.0; }
 969
 970         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
 971     }
 972
 973     /*
 974     if (APData->Mode == 2) // Glide slope hold
 975     {
 976     double RelSlope;
 977     double RelElevator;
 978
 979     // First, calculate Relative slope and normalize it
 980     RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
 981
 982     // Now calculate the elevator offset from current angle
 983     if ( abs(RelSlope) > APData->SlopeSmooth )
 984     {
 985         if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
 986         RelElevator = -APData->MaxElevator;
 987         else
 988         RelElevator = APData->MaxElevator;
 989     }
 990
 991     else
 992         RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
 993
 994     // set the elevator
 995     fgElevMove(RelElevator);
 996
 997     }
 998     */
 999
1000     // Ok, we are done
1001     return 0;
1002
1003 }
1004
1005 /*
1006 void fgAPSetMode( int mode)
1007 {
1008     //Remove the following line when the calling funcitons start passing in the data pointer
1009     fgAPDataPtr APData;
1010
1011     APData = APDataGlobal;
1012     // end section
1013
1014     fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
1015
1016     APData->Mode = mode;  // set the new mode
1017 }
1018 */
1019
1020 void fgAPToggleHeading( void )
1021 {
1022     // Remove at a later date
1023     fgAPDataPtr APData;
1024
1025     APData = APDataGlobal;
1026     // end section
1027
1028     if ( APData->heading_hold ) {
1029         // turn off heading hold
1030         APData->heading_hold = false;
1031     } else {
1032         // turn on heading hold, lock at current heading
1033         APData->heading_hold = true;
1034         APData->TargetHeading = fgAPget_heading();
1035     }
1036
1037     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
1038             << APData->heading_hold << ") " << APData->TargetHeading );
1039 }
1040
1041
1042 void fgAPToggleAltitude( void )
1043 {
1044     // Remove at a later date
1045     fgAPDataPtr APData;
1046
1047     APData = APDataGlobal;
1048     // end section
1049
1050     if ( APData->altitude_hold ) {
1051         // turn off altitude hold
1052         APData->altitude_hold = false;
1053     } else {
1054         // turn on altitude hold, lock at current altitude
1055         APData->altitude_hold = true;
1056         APData->terrain_follow = false;
1057         APData->TargetAltitude = fgAPget_altitude();
1058         APData->alt_error_accum = 0.0;
1059         // alt_error_queue.erase( alt_error_queue.begin(),
1060         //                        alt_error_queue.end() );
1061     }
1062
1063     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
1064             << APData->altitude_hold << ") " << APData->TargetAltitude );
1065 }
1066
1067
1068 void fgAPToggleAutoThrottle ( void )
1069 {
1070     // Remove at a later date
1071     fgAPDataPtr APData;
1072
1073     APData = APDataGlobal;
1074     // end section
1075
1076     if ( APData->auto_throttle ) {
1077         // turn off altitude hold
1078         APData->auto_throttle = false;
1079     } else {
1080         // turn on terrain follow, lock at current agl
1081         APData->auto_throttle = true;
1082         APData->TargetSpeed = get_speed();
1083         APData->speed_error_accum = 0.0;
1084     }
1085
1086     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1087             << APData->auto_throttle << ") " << APData->TargetSpeed );
1088 }
1089
1090 void fgAPToggleTerrainFollow( void )
1091 {
1092     // Remove at a later date
1093     fgAPDataPtr APData;
1094
1095     APData = APDataGlobal;
1096     // end section
1097
1098     if ( APData->terrain_follow ) {
1099         // turn off altitude hold
1100         APData->terrain_follow = false;
1101     } else {
1102         // turn on terrain follow, lock at current agl
1103         APData->terrain_follow = true;
1104         APData->altitude_hold = false;
1105         APData->TargetAGL = fgAPget_agl();
1106         APData->alt_error_accum = 0.0;
1107     }
1108
1109     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
1110             << APData->terrain_follow << ") " << APData->TargetAGL );
1111 }
1112
1113 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
1114 {
1115     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
1116     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
1117
1118     double m, b, y;     // the constants to find in y=mx+b
1119     m=(y2-y1)/(x2-x1);  // calculate the m
1120     b= y1- m * x1;  // calculate the b
1121     y = m * x + b;  // the final calculation
1122
1123     return y;
1124 };
1125
1126 double NormalizeDegrees(double Input)
1127 {
1128     // normalize the input to the range (-180,180]
1129     // Input should not be greater than -360 to 360.  Current rules send the output to an undefined state.
1130     if (Input > 180)
1131         Input -= 360;
1132     if (Input <= -180)
1133         Input += 360;
1134
1135     return (Input);
1136 };