src/Autopilot/autopilot.cxx

   1 // autopilot.cxx -- autopilot subsystem
   2 //
   3 // Started by Jeff Goeke-Smith, started April 1998.
   4 //
   5 // Copyright (C) 1998  Jeff Goeke-Smith, jgoeke@voyager.net
   6 //
   7 // Heavy modifications and additions by Norman Vine and few by Curtis
   8 // Olson
   9 //
  10 // This program is free software; you can redistribute it and/or
  11 // modify it under the terms of the GNU General Public License as
  12 // published by the Free Software Foundation; either version 2 of the
  13 // License, or (at your option) any later version.
  14 //
  15 // This program is distributed in the hope that it will be useful, but
  16 // WITHOUT ANY WARRANTY; without even the implied warranty of
  17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  18 // General Public License for more details.
  19 //
  20 // You should have received a copy of the GNU General Public License
  21 // along with this program; if not, write to the Free Software
  22 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  23 //
  24 // $Id$
  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 #include <Airports/simple.hxx>
  41 #include <GUI/gui.h>
  42 #include <Main/fg_init.hxx>
  43 #include <Main/options.hxx>
  44 #include <Time/fg_time.hxx>
  45
  46
  47 #define mySlider puSlider
  48
  49 // Climb speed constants
  50 const double min_climb = 70.0;  // kts
  51 const double best_climb = 75.0; // kts
  52 const double ideal_climb_rate = 500.0; // fpm
  53
  54 /// These statics will eventually go into the class
  55 /// they are just here while I am experimenting -- NHV :-)
  56 // AutoPilot Gain Adjuster members
  57 static double MaxRollAdjust;        // MaxRollAdjust       = 2 * APData->MaxRoll;
  58 static double RollOutAdjust;        // RollOutAdjust       = 2 * APData->RollOut;
  59 static double MaxAileronAdjust;     // MaxAileronAdjust    = 2 * APData->MaxAileron;
  60 static double RollOutSmoothAdjust;  // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
  61
  62 static float MaxRollValue;          // 0.1 -> 1.0
  63 static float RollOutValue;
  64 static float MaxAileronValue;
  65 static float RollOutSmoothValue;
  66
  67 static float TmpMaxRollValue;       // for cancel operation
  68 static float TmpRollOutValue;
  69 static float TmpMaxAileronValue;
  70 static float TmpRollOutSmoothValue;
  71
  72 static puDialogBox *APAdjustDialog;
  73 static puFrame     *APAdjustFrame;
  74 static puText      *APAdjustDialogMessage;
  75 static puFont      APAdjustLegendFont;
  76 static puFont      APAdjustLabelFont;
  77
  78 static puOneShot *APAdjustOkButton;
  79 static puOneShot *APAdjustResetButton;
  80 static puOneShot *APAdjustCancelButton;
  81
  82 //static puButton        *APAdjustDragButton;
  83
  84 static puText *APAdjustMaxRollTitle;
  85 static puText *APAdjustRollOutTitle;
  86 static puText *APAdjustMaxAileronTitle;
  87 static puText *APAdjustRollOutSmoothTitle;
  88
  89 static puText *APAdjustMaxAileronText;
  90 static puText *APAdjustMaxRollText;
  91 static puText *APAdjustRollOutText;
  92 static puText *APAdjustRollOutSmoothText;
  93
  94 static mySlider *APAdjustHS0;
  95 static mySlider *APAdjustHS1;
  96 static mySlider *APAdjustHS2;
  97 static mySlider *APAdjustHS3;
  98
  99 static char SliderText[ 4 ][ 8 ];
 100
 101 ///////// AutoPilot New Heading Dialog
 102
 103 static puDialogBox     *ApHeadingDialog;
 104 static puFrame         *ApHeadingDialogFrame;
 105 static puText          *ApHeadingDialogMessage;
 106 static puInput         *ApHeadingDialogInput;
 107 static puOneShot       *ApHeadingDialogOkButton;
 108 static puOneShot       *ApHeadingDialogCancelButton;
 109
 110
 111 ///////// AutoPilot New Altitude Dialog
 112
 113 static puDialogBox     *ApAltitudeDialog = 0;
 114 static puFrame         *ApAltitudeDialogFrame = 0;
 115 static puText          *ApAltitudeDialogMessage = 0;
 116 static puInput         *ApAltitudeDialogInput = 0;
 117
 118 static puOneShot       *ApAltitudeDialogOkButton = 0;
 119 static puOneShot       *ApAltitudeDialogCancelButton = 0;
 120
 121
 122 /// The beginnings of Lock AutoPilot to target location :-)
 123 //  Needs cleaning up but works
 124 //  These statics should disapear when this is a class
 125 static puDialogBox     *TgtAptDialog = 0;
 126 static puFrame         *TgtAptDialogFrame = 0;
 127 static puText          *TgtAptDialogMessage = 0;
 128 static puInput         *TgtAptDialogInput = 0;
 129
 130 static char NewTgtAirportId[16];
 131 static char NewTgtAirportLabel[] = "Enter New TgtAirport ID";
 132
 133 static puOneShot       *TgtAptDialogOkButton = 0;
 134 static puOneShot       *TgtAptDialogCancelButton = 0;
 135 static puOneShot       *TgtAptDialogResetButton = 0;
 136
 137
 138 // global variable holding the AP info
 139 // I want this gone.  Data should be in aircraft structure
 140 fgAPDataPtr APDataGlobal;
 141
 142 // Local Prototype section
 143 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 );
 144 static double NormalizeDegrees( double Input );
 145 // End Local ProtoTypes
 146
 147 extern char *coord_format_lat(float);
 148 extern char *coord_format_lon(float);
 149
 150
 151 static inline double get_ground_speed( void ) {
 152     // starts in ft/s so we convert to kts
 153     double ft_s = cur_fdm_state->get_V_ground_speed()
 154         * current_options.get_speed_up();;
 155     double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
 156     return kts;
 157 }
 158
 159 // The below routines were copied right from hud.c ( I hate reinventing
 160 // the wheel more than necessary)
 161
 162 // The following routines obtain information concerntin the aircraft's
 163 // current state and return it to calling instrument display routines.
 164 // They should eventually be member functions of the aircraft.
 165 //
 166 static void get_control_values( void ) {
 167     fgAPDataPtr APData;
 168     APData = APDataGlobal;
 169     APData->old_aileron = controls.get_aileron();
 170     APData->old_elevator = controls.get_elevator();
 171     APData->old_elevator_trim = controls.get_elevator_trim();
 172     APData->old_rudder = controls.get_rudder();
 173 }
 174
 175 static void MakeTargetHeadingStr( fgAPDataPtr APData, double bearing ) {
 176     if( bearing < 0. )
 177         bearing += 360.;
 178     else if(bearing > 360. )
 179         bearing -= 360.;
 180     sprintf(APData->TargetHeadingStr, "APHeading  %6.1f", bearing);
 181 }
 182
 183 static inline void MakeTargetDistanceStr( fgAPDataPtr APData, double distance ) {
 184     double eta = distance*METER_TO_NM / get_ground_speed();
 185     if ( eta >= 100.0 ) { eta = 99.999; }
 186     int major, minor;
 187     if ( eta < (1.0/6.0) ) {
 188         // within 10 minutes, bump up to min/secs
 189         eta *= 60.0;
 190     }
 191     major = (int)eta;
 192     minor = (int)((eta - (int)eta) * 60.0);
 193     sprintf(APData->TargetDistanceStr, "APDistance %.2f NM  ETA %d:%02d",
 194             distance*METER_TO_NM, major, minor);
 195     // cout << "distance = " << distance*METER_TO_NM
 196     //      << "  gndsp = " << get_ground_speed()
 197     //      << "  time = " << eta
 198     //      << "  major = " << major
 199     //      << "  minor = " << minor
 200     //      << endl;
 201 }
 202
 203 static inline void MakeTargetAltitudeStr( fgAPDataPtr APData, double altitude ) {
 204     sprintf(APData->TargetAltitudeStr, "APAltitude  %6.0f", altitude);
 205 }
 206
 207 static inline void MakeTargetLatLonStr( fgAPDataPtr APData, double lat, double lon ) {
 208     float tmp;
 209     tmp = APData->TargetLatitude;
 210     sprintf( APData->TargetLatitudeStr , "%s", coord_format_lat(tmp) );
 211     tmp = APData->TargetLongitude;
 212     sprintf( APData->TargetLongitudeStr, "%s", coord_format_lon(tmp) );
 213
 214     sprintf(APData->TargetLatLonStr, "%s  %s",
 215             APData->TargetLatitudeStr,
 216             APData->TargetLongitudeStr );
 217 }
 218
 219 static inline double get_speed( void ) {
 220     return( cur_fdm_state->get_V_equiv_kts() );
 221 }
 222
 223 static inline double get_aoa( void ) {
 224     return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
 225 }
 226
 227 static inline double fgAPget_latitude( void ) {
 228     return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
 229 }
 230
 231 static inline double fgAPget_longitude( void ) {
 232     return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
 233 }
 234
 235 static inline double fgAPget_roll( void ) {
 236     return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
 237 }
 238
 239 static inline double get_pitch( void ) {
 240     return( cur_fdm_state->get_Theta() );
 241 }
 242
 243 double fgAPget_heading( void ) {
 244     return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
 245 }
 246
 247 static inline double fgAPget_altitude( void ) {
 248     return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
 249 }
 250
 251 static inline double fgAPget_climb( void ) {
 252     // return in meters per minute
 253     return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
 254 }
 255
 256 static inline double get_sideslip( void ) {
 257     return( cur_fdm_state->get_Beta() );
 258 }
 259
 260 static inline double fgAPget_agl( void ) {
 261     double agl;
 262
 263     agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
 264         - scenery.cur_elev;
 265
 266     return( agl );
 267 }
 268
 269 // End of copied section.  ( thanks for the wheel :-)
 270 double fgAPget_TargetLatitude( void ) {
 271     fgAPDataPtr APData = APDataGlobal;
 272     return APData->TargetLatitude;
 273 }
 274
 275 double fgAPget_TargetLongitude( void ) {
 276     fgAPDataPtr APData = APDataGlobal;
 277     return APData->TargetLongitude;
 278 }
 279
 280 double fgAPget_TargetHeading( void ) {
 281     fgAPDataPtr APData = APDataGlobal;
 282     return APData->TargetHeading;
 283 }
 284
 285 double fgAPget_TargetDistance( void ) {
 286     fgAPDataPtr APData = APDataGlobal;
 287     return APData->TargetDistance;
 288 }
 289
 290 double fgAPget_TargetAltitude( void ) {
 291     fgAPDataPtr APData = APDataGlobal;
 292     return APData->TargetAltitude;
 293 }
 294
 295 char *fgAPget_TargetLatitudeStr( void ) {
 296     fgAPDataPtr APData = APDataGlobal;
 297     return APData->TargetLatitudeStr;
 298 }
 299
 300 char *fgAPget_TargetLongitudeStr( void ) {
 301     fgAPDataPtr APData = APDataGlobal;
 302     return APData->TargetLongitudeStr;
 303 }
 304
 305 char *fgAPget_TargetDistanceStr( void ) {
 306     fgAPDataPtr APData = APDataGlobal;
 307     return APData->TargetDistanceStr;
 308 }
 309
 310 char *fgAPget_TargetHeadingStr( void ) {
 311     fgAPDataPtr APData = APDataGlobal;
 312     return APData->TargetHeadingStr;
 313 }
 314
 315 char *fgAPget_TargetAltitudeStr( void ) {
 316     fgAPDataPtr APData = APDataGlobal;
 317     return APData->TargetAltitudeStr;
 318 }
 319
 320 char *fgAPget_TargetLatLonStr( void ) {
 321     fgAPDataPtr APData = APDataGlobal;
 322     return APData->TargetLatLonStr;
 323 }
 324
 325 bool fgAPWayPointEnabled( void )
 326 {
 327     fgAPDataPtr APData;
 328
 329     APData = APDataGlobal;
 330
 331     // heading hold enabled?
 332     return APData->waypoint_hold;
 333 }
 334
 335
 336 bool fgAPHeadingEnabled( void )
 337 {
 338     fgAPDataPtr APData;
 339
 340     APData = APDataGlobal;
 341
 342     // heading hold enabled?
 343     return APData->heading_hold;
 344 }
 345
 346 bool fgAPAltitudeEnabled( void )
 347 {
 348     fgAPDataPtr APData;
 349
 350     APData = APDataGlobal;
 351
 352     // altitude hold or terrain follow enabled?
 353     return APData->altitude_hold;
 354 }
 355
 356 bool fgAPTerrainFollowEnabled( void )
 357 {
 358     fgAPDataPtr APData;
 359
 360     APData = APDataGlobal;
 361
 362     // altitude hold or terrain follow enabled?
 363     return APData->terrain_follow ;
 364 }
 365
 366 bool fgAPAutoThrottleEnabled( void )
 367 {
 368     fgAPDataPtr APData;
 369
 370     APData = APDataGlobal;
 371
 372     // autothrottle enabled?
 373     return APData->auto_throttle;
 374 }
 375
 376 void fgAPAltitudeAdjust( double inc )
 377 {
 378     // Remove at a later date
 379     fgAPDataPtr APData = APDataGlobal;
 380     // end section
 381
 382     double target_alt, target_agl;
 383
 384     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 385         target_alt = APData->TargetAltitude * METER_TO_FEET;
 386         target_agl = APData->TargetAGL * METER_TO_FEET;
 387     } else {
 388         target_alt = APData->TargetAltitude;
 389         target_agl = APData->TargetAGL;
 390     }
 391
 392     target_alt = ( int ) ( target_alt / inc ) * inc + inc;
 393     target_agl = ( int ) ( target_agl / inc ) * inc + inc;
 394
 395     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 396         target_alt *= FEET_TO_METER;
 397         target_agl *= FEET_TO_METER;
 398     }
 399
 400     APData->TargetAltitude = target_alt;
 401     APData->TargetAGL = target_agl;
 402
 403     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 404         target_alt *= METER_TO_FEET;
 405     ApAltitudeDialogInput->setValue((float)target_alt);
 406     MakeTargetAltitudeStr( APData, target_alt);
 407
 408     get_control_values();
 409 }
 410
 411 void fgAPAltitudeSet( double new_altitude ) {
 412     // Remove at a later date
 413     fgAPDataPtr APData = APDataGlobal;
 414     // end section
 415     double target_alt = new_altitude;
 416
 417     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 418         target_alt = new_altitude * FEET_TO_METER;
 419
 420     if( target_alt < scenery.cur_elev )
 421         target_alt = scenery.cur_elev;
 422
 423     APData->TargetAltitude = target_alt;
 424
 425     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 426         target_alt *= METER_TO_FEET;
 427     ApAltitudeDialogInput->setValue((float)target_alt);
 428     MakeTargetAltitudeStr( APData, target_alt);
 429
 430     get_control_values();
 431 }
 432
 433 void fgAPHeadingAdjust( double inc ) {
 434     fgAPDataPtr APData = APDataGlobal;
 435
 436     if ( APData->waypoint_hold )
 437         APData->waypoint_hold = false;
 438
 439     double target = ( int ) ( APData->TargetHeading / inc ) * inc + inc;
 440
 441     APData->TargetHeading = NormalizeDegrees( target );
 442     // following cast needed ambiguous plib
 443     ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
 444     MakeTargetHeadingStr( APData, APData->TargetHeading );
 445     get_control_values();
 446 }
 447
 448 void fgAPHeadingSet( double new_heading ) {
 449     fgAPDataPtr APData = APDataGlobal;
 450
 451     if ( APData->waypoint_hold )
 452         APData->waypoint_hold = false;
 453
 454     new_heading = NormalizeDegrees( new_heading );
 455     APData->TargetHeading = new_heading;
 456     // following cast needed ambiguous plib
 457     ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
 458     MakeTargetHeadingStr( APData, APData->TargetHeading );
 459     get_control_values();
 460 }
 461
 462 void fgAPAutoThrottleAdjust( double inc ) {
 463     fgAPDataPtr APData = APDataGlobal;
 464
 465     double target = ( int ) ( APData->TargetSpeed / inc ) * inc + inc;
 466
 467     APData->TargetSpeed = target;
 468 }
 469
 470 // THIS NEEDS IMPROVEMENT !!!!!!!!!!!!!
 471 static int scan_number(char *s, double *new_value)
 472 {
 473     int ret = 0;
 474     char WordBuf[64];
 475     char *cptr = s;
 476     char *WordBufPtr = WordBuf;
 477
 478     if (*cptr == '+')
 479         cptr++;
 480     if (*cptr == '-') {
 481         *WordBufPtr++ = *cptr++;
 482     }
 483     while (isdigit(*cptr) ) {
 484         *WordBufPtr++ = *cptr++;
 485         ret = 1;
 486     }
 487     if (*cptr == '.')
 488         *WordBufPtr++ = *cptr++;  // put the '.' into the string
 489     while (isdigit(*cptr)) {
 490         *WordBufPtr++ = *cptr++;
 491         ret = 1;
 492     }
 493     if( ret == 1 ) {
 494         *WordBufPtr = '\0';
 495         sscanf(WordBuf, "%lf", new_value);
 496     }
 497
 498     return(ret);
 499 } // scan_number
 500
 501
 502 void ApHeadingDialog_Cancel(puObject *)
 503 {
 504     ApHeadingDialogInput->rejectInput();
 505     FG_POP_PUI_DIALOG( ApHeadingDialog );
 506 }
 507
 508 void ApHeadingDialog_OK (puObject *me)
 509 {
 510     int error = 0;
 511     char *c;
 512     string s;
 513     ApHeadingDialogInput -> getValue( &c );
 514
 515     if( strlen(c) ) {
 516         double NewHeading;
 517         if( scan_number( c, &NewHeading ) )
 518             {
 519                 if(!fgAPHeadingEnabled())
 520                     fgAPToggleHeading();
 521                 fgAPHeadingSet( NewHeading );
 522             } else {
 523                 error = 1;
 524                 s = c;
 525                 s += " is not a valid number.";
 526             }
 527     }
 528     ApHeadingDialog_Cancel(me);
 529     if( error )  mkDialog(s.c_str());
 530 }
 531
 532 void NewHeading(puObject *cb)
 533 {
 534     //  string ApHeadingLabel( "Enter New Heading" );
 535     //  ApHeadingDialogMessage  -> setLabel(ApHeadingLabel.c_str());
 536     ApHeadingDialogInput    -> acceptInput();
 537     FG_PUSH_PUI_DIALOG( ApHeadingDialog );
 538 }
 539
 540 void NewHeadingInit(void)
 541 {
 542     //  printf("NewHeadingInit\n");
 543     char NewHeadingLabel[] = "Enter New Heading";
 544     char *s;
 545
 546     float heading = fgAPget_heading();
 547     int len = 260/2 -
 548         (puGetStringWidth( puGetDefaultLabelFont(), NewHeadingLabel ) /2 );
 549
 550     ApHeadingDialog = new puDialogBox (150, 50);
 551     {
 552         ApHeadingDialogFrame   = new puFrame (0, 0, 260, 150);
 553
 554         ApHeadingDialogMessage = new puText   (len, 110);
 555         ApHeadingDialogMessage    -> setDefaultValue (NewHeadingLabel);
 556         ApHeadingDialogMessage    -> getDefaultValue (&s);
 557         ApHeadingDialogMessage    -> setLabel        (s);
 558
 559         ApHeadingDialogInput   = new puInput  ( 50, 70, 210, 100 );
 560         ApHeadingDialogInput   ->    setValue ( heading );
 561
 562         ApHeadingDialogOkButton     =  new puOneShot         (50, 10, 110, 50);
 563         ApHeadingDialogOkButton     ->     setLegend         (gui_msg_OK);
 564         ApHeadingDialogOkButton     ->     makeReturnDefault (TRUE);
 565         ApHeadingDialogOkButton     ->     setCallback       (ApHeadingDialog_OK);
 566
 567         ApHeadingDialogCancelButton =  new puOneShot         (140, 10, 210, 50);
 568         ApHeadingDialogCancelButton ->     setLegend         (gui_msg_CANCEL);
 569         ApHeadingDialogCancelButton ->     setCallback       (ApHeadingDialog_Cancel);
 570
 571     }
 572     FG_FINALIZE_PUI_DIALOG( ApHeadingDialog );
 573 }
 574
 575 void ApAltitudeDialog_Cancel(puObject *)
 576 {
 577     ApAltitudeDialogInput -> rejectInput();
 578     FG_POP_PUI_DIALOG( ApAltitudeDialog );
 579 }
 580
 581 void ApAltitudeDialog_OK (puObject *me)
 582 {
 583     int error = 0;
 584     string s;
 585     char *c;
 586     ApAltitudeDialogInput->getValue( &c );
 587
 588     if( strlen( c ) ) {
 589         double NewAltitude;
 590         if( scan_number( c, &NewAltitude) )
 591             {
 592                 if(!(fgAPAltitudeEnabled()))
 593                     fgAPToggleAltitude();
 594                 fgAPAltitudeSet( NewAltitude );
 595             } else {
 596                 error = 1;
 597                 s = c;
 598                 s += " is not a valid number.";
 599             }
 600     }
 601     ApAltitudeDialog_Cancel(me);
 602     if( error )  mkDialog(s.c_str());
 603     get_control_values();
 604 }
 605
 606 void NewAltitude(puObject *cb)
 607 {
 608     ApAltitudeDialogInput -> acceptInput();
 609     FG_PUSH_PUI_DIALOG( ApAltitudeDialog );
 610 }
 611
 612 void NewAltitudeInit(void)
 613 {
 614     //  printf("NewAltitudeInit\n");
 615     char NewAltitudeLabel[] = "Enter New Altitude";
 616     char *s;
 617
 618     float alt = cur_fdm_state->get_Altitude();
 619
 620     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_METERS) {
 621         alt *= FEET_TO_METER;
 622     }
 623
 624     int len = 260/2 -
 625         (puGetStringWidth( puGetDefaultLabelFont(), NewAltitudeLabel )/2);
 626
 627     //  ApAltitudeDialog = new puDialogBox (150, 50);
 628     ApAltitudeDialog = new puDialogBox (150, 200);
 629     {
 630         ApAltitudeDialogFrame   = new puFrame  (0, 0, 260, 150);
 631         ApAltitudeDialogMessage = new puText   (len, 110);
 632         ApAltitudeDialogMessage    -> setDefaultValue (NewAltitudeLabel);
 633         ApAltitudeDialogMessage    -> getDefaultValue (&s);
 634         ApAltitudeDialogMessage    -> setLabel (s);
 635
 636         ApAltitudeDialogInput   = new puInput  ( 50, 70, 210, 100 );
 637         ApAltitudeDialogInput      -> setValue ( alt );
 638         // Uncomment the next line to have input active on startup
 639         // ApAltitudeDialogInput   ->    acceptInput       ( );
 640         // cursor at begining or end of line ?
 641         //len = strlen(s);
 642         //              len = 0;
 643         //              ApAltitudeDialogInput   ->    setCursor         ( len );
 644         //              ApAltitudeDialogInput   ->    setSelectRegion   ( 5, 9 );
 645
 646         ApAltitudeDialogOkButton     =  new puOneShot         (50, 10, 110, 50);
 647         ApAltitudeDialogOkButton     ->     setLegend         (gui_msg_OK);
 648         ApAltitudeDialogOkButton     ->     makeReturnDefault (TRUE);
 649         ApAltitudeDialogOkButton     ->     setCallback       (ApAltitudeDialog_OK);
 650
 651         ApAltitudeDialogCancelButton =  new puOneShot         (140, 10, 210, 50);
 652         ApAltitudeDialogCancelButton ->     setLegend         (gui_msg_CANCEL);
 653         ApAltitudeDialogCancelButton ->     setCallback       (ApAltitudeDialog_Cancel);
 654
 655     }
 656     FG_FINALIZE_PUI_DIALOG( ApAltitudeDialog );
 657 }
 658
 659 /////// simple AutoPilot GAIN / LIMITS ADJUSTER
 660
 661 #define fgAP_CLAMP(val,min,max) \
 662 ( (val) = (val) > (max) ? (max) : (val) < (min) ? (min) : (val) )
 663
 664     static void maxroll_adj( puObject *hs ) {
 665         float val ;
 666         fgAPDataPtr APData = APDataGlobal;
 667
 668         hs-> getValue ( &val ) ;
 669         fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 670         //    printf ( "maxroll_adj( %p ) %f %f\n", hs, val, MaxRollAdjust * val ) ;
 671         APData-> MaxRoll = MaxRollAdjust * val;
 672         sprintf( SliderText[ 0 ], "%05.2f", APData->MaxRoll );
 673         APAdjustMaxRollText -> setLabel ( SliderText[ 0 ] ) ;
 674 }
 675
 676 static void rollout_adj( puObject *hs ) {
 677     float val ;
 678     fgAPDataPtr APData = APDataGlobal;
 679
 680     hs-> getValue ( &val ) ;
 681     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 682     //    printf ( "rollout_adj( %p ) %f %f\n", hs, val, RollOutAdjust * val ) ;
 683     APData-> RollOut = RollOutAdjust * val;
 684     sprintf( SliderText[ 1 ], "%05.2f", APData->RollOut );
 685     APAdjustRollOutText -> setLabel ( SliderText[ 1 ] );
 686 }
 687
 688 static void maxaileron_adj( puObject *hs ) {
 689     float val ;
 690     fgAPDataPtr APData;
 691     APData = APDataGlobal;
 692
 693     hs-> getValue ( &val ) ;
 694     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 695     //    printf ( "maxaileron_adj( %p ) %f %f\n", hs, val, MaxAileronAdjust * val ) ;
 696     APData-> MaxAileron = MaxAileronAdjust * val;
 697     sprintf( SliderText[ 3 ], "%05.2f", APData->MaxAileron );
 698     APAdjustMaxAileronText -> setLabel ( SliderText[ 3 ] );
 699 }
 700
 701 static void rolloutsmooth_adj( puObject *hs ) {
 702     float val ;
 703     fgAPDataPtr APData = APDataGlobal;
 704
 705     hs -> getValue ( &val ) ;
 706     fgAP_CLAMP ( val, 0.1, 1.0 ) ;
 707     //    printf ( "rolloutsmooth_adj( %p ) %f %f\n", hs, val, RollOutSmoothAdjust * val ) ;
 708     APData->RollOutSmooth = RollOutSmoothAdjust * val;
 709     sprintf( SliderText[ 2 ], "%5.2f", APData-> RollOutSmooth );
 710     APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] );
 711
 712 }
 713
 714 static void goAwayAPAdjust (puObject *)
 715 {
 716     FG_POP_PUI_DIALOG( APAdjustDialog );
 717 }
 718
 719 void cancelAPAdjust( puObject *self ) {
 720     fgAPDataPtr APData = APDataGlobal;
 721
 722     APData-> MaxRoll       = TmpMaxRollValue;
 723     APData-> RollOut       = TmpRollOutValue;
 724     APData-> MaxAileron    = TmpMaxAileronValue;
 725     APData-> RollOutSmooth = TmpRollOutSmoothValue;
 726
 727     goAwayAPAdjust(self);
 728 }
 729
 730 void resetAPAdjust( puObject *self ) {
 731     fgAPDataPtr APData = APDataGlobal;
 732
 733     APData-> MaxRoll       = MaxRollAdjust / 2;
 734     APData-> RollOut       = RollOutAdjust / 2;
 735     APData-> MaxAileron    = MaxAileronAdjust / 2;
 736     APData-> RollOutSmooth = RollOutSmoothAdjust / 2;
 737
 738     FG_POP_PUI_DIALOG( APAdjustDialog );
 739
 740     fgAPAdjust( self );
 741 }
 742
 743 void fgAPAdjust( puObject * ) {
 744     fgAPDataPtr APData = APDataGlobal;
 745
 746     TmpMaxRollValue       = APData-> MaxRoll;
 747     TmpRollOutValue       = APData-> RollOut;
 748     TmpMaxAileronValue    = APData-> MaxAileron;
 749     TmpRollOutSmoothValue = APData-> RollOutSmooth;
 750
 751     MaxRollValue       = APData-> MaxRoll / MaxRollAdjust;
 752     RollOutValue       = APData-> RollOut / RollOutAdjust;
 753     MaxAileronValue    = APData-> MaxAileron / MaxAileronAdjust;
 754     RollOutSmoothValue = APData-> RollOutSmooth / RollOutSmoothAdjust;
 755
 756     APAdjustHS0-> setValue ( MaxRollValue ) ;
 757     APAdjustHS1-> setValue ( RollOutValue ) ;
 758     APAdjustHS2-> setValue ( RollOutSmoothValue ) ;
 759     APAdjustHS3-> setValue ( MaxAileronValue ) ;
 760
 761     FG_PUSH_PUI_DIALOG( APAdjustDialog );
 762 }
 763
 764 // Done once at system initialization
 765 void fgAPAdjustInit( void ) {
 766
 767     //  printf("fgAPAdjustInit\n");
 768 #define HORIZONTAL  FALSE
 769
 770     int DialogX = 40;
 771     int DialogY = 100;
 772     int DialogWidth = 230;
 773
 774     char Label[] =  "AutoPilot Adjust";
 775     char *s;
 776
 777     fgAPDataPtr APData = APDataGlobal;
 778
 779     int labelX = (DialogWidth / 2) -
 780         (puGetStringWidth( puGetDefaultLabelFont(), Label ) / 2);
 781     labelX -= 30;  // KLUDGEY
 782
 783     int nSliders = 4;
 784     int slider_x = 10;
 785     int slider_y = 55;
 786     int slider_width = 210;
 787     int slider_title_x = 15;
 788     int slider_value_x = 160;
 789     float slider_delta = 0.1f;
 790
 791     TmpMaxRollValue       = APData-> MaxRoll;
 792     TmpRollOutValue       = APData-> RollOut;
 793     TmpMaxAileronValue    = APData-> MaxAileron;
 794     TmpRollOutSmoothValue = APData-> RollOutSmooth;
 795
 796     MaxRollValue       = APData-> MaxRoll / MaxRollAdjust;
 797     RollOutValue       = APData-> RollOut / RollOutAdjust;
 798     MaxAileronValue    = APData-> MaxAileron / MaxAileronAdjust;
 799     RollOutSmoothValue = APData-> RollOutSmooth / RollOutSmoothAdjust;
 800
 801     puGetDefaultFonts (  &APAdjustLegendFont,  &APAdjustLabelFont );
 802     APAdjustDialog = new puDialogBox ( DialogX, DialogY ); {
 803         int horiz_slider_height = puGetStringHeight (APAdjustLabelFont) +
 804             puGetStringDescender (APAdjustLabelFont) +
 805             PUSTR_TGAP + PUSTR_BGAP + 5;
 806
 807         APAdjustFrame = new puFrame ( 0, 0,
 808                                       DialogWidth, 85 + nSliders * horiz_slider_height );
 809
 810         APAdjustDialogMessage = new puText ( labelX, 52 + nSliders * horiz_slider_height );
 811         APAdjustDialogMessage -> setDefaultValue ( Label );
 812         APAdjustDialogMessage -> getDefaultValue ( &s );
 813         APAdjustDialogMessage -> setLabel        ( s );
 814
 815         APAdjustHS0 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 816         APAdjustHS0-> setDelta ( slider_delta ) ;
 817         APAdjustHS0-> setValue ( MaxRollValue ) ;
 818         APAdjustHS0-> setCBMode ( PUSLIDER_DELTA ) ;
 819         APAdjustHS0-> setCallback ( maxroll_adj ) ;
 820
 821         sprintf( SliderText[ 0 ], "%05.2f", APData->MaxRoll );
 822         APAdjustMaxRollTitle = new puText ( slider_title_x, slider_y ) ;
 823         APAdjustMaxRollTitle-> setDefaultValue ( "MaxRoll" ) ;
 824         APAdjustMaxRollTitle-> getDefaultValue ( &s ) ;
 825         APAdjustMaxRollTitle-> setLabel ( s ) ;
 826         APAdjustMaxRollText = new puText ( slider_value_x, slider_y ) ;
 827         APAdjustMaxRollText-> setLabel ( SliderText[ 0 ] ) ;
 828
 829         slider_y += horiz_slider_height;
 830
 831         APAdjustHS1 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 832         APAdjustHS1-> setDelta ( slider_delta ) ;
 833         APAdjustHS1-> setValue ( RollOutValue ) ;
 834         APAdjustHS1-> setCBMode ( PUSLIDER_DELTA ) ;
 835         APAdjustHS1-> setCallback ( rollout_adj ) ;
 836
 837         sprintf( SliderText[ 1 ], "%05.2f", APData->RollOut );
 838         APAdjustRollOutTitle = new puText ( slider_title_x, slider_y ) ;
 839         APAdjustRollOutTitle-> setDefaultValue ( "AdjustRollOut" ) ;
 840         APAdjustRollOutTitle-> getDefaultValue ( &s ) ;
 841         APAdjustRollOutTitle-> setLabel ( s ) ;
 842         APAdjustRollOutText = new puText ( slider_value_x, slider_y ) ;
 843         APAdjustRollOutText-> setLabel ( SliderText[ 1 ] );
 844
 845         slider_y += horiz_slider_height;
 846
 847         APAdjustHS2 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 848         APAdjustHS2-> setDelta ( slider_delta ) ;
 849         APAdjustHS2-> setValue ( RollOutSmoothValue ) ;
 850         APAdjustHS2-> setCBMode ( PUSLIDER_DELTA ) ;
 851         APAdjustHS2-> setCallback ( rolloutsmooth_adj ) ;
 852
 853         sprintf( SliderText[ 2 ], "%5.2f", APData->RollOutSmooth );
 854         APAdjustRollOutSmoothTitle = new puText ( slider_title_x, slider_y ) ;
 855         APAdjustRollOutSmoothTitle-> setDefaultValue ( "RollOutSmooth" ) ;
 856         APAdjustRollOutSmoothTitle-> getDefaultValue ( &s ) ;
 857         APAdjustRollOutSmoothTitle-> setLabel ( s ) ;
 858         APAdjustRollOutSmoothText = new puText ( slider_value_x, slider_y ) ;
 859         APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] );
 860
 861         slider_y += horiz_slider_height;
 862
 863         APAdjustHS3 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
 864         APAdjustHS3-> setDelta ( slider_delta ) ;
 865         APAdjustHS3-> setValue ( MaxAileronValue ) ;
 866         APAdjustHS3-> setCBMode ( PUSLIDER_DELTA ) ;
 867         APAdjustHS3-> setCallback ( maxaileron_adj ) ;
 868
 869         sprintf( SliderText[ 3 ], "%05.2f", APData->MaxAileron );
 870         APAdjustMaxAileronTitle = new puText ( slider_title_x, slider_y ) ;
 871         APAdjustMaxAileronTitle-> setDefaultValue ( "MaxAileron" ) ;
 872         APAdjustMaxAileronTitle-> getDefaultValue ( &s ) ;
 873         APAdjustMaxAileronTitle-> setLabel ( s ) ;
 874         APAdjustMaxAileronText = new puText ( slider_value_x, slider_y ) ;
 875         APAdjustMaxAileronText-> setLabel ( SliderText[ 3 ] );
 876
 877         APAdjustOkButton = new puOneShot ( 10, 10, 60, 50 );
 878         APAdjustOkButton-> setLegend ( gui_msg_OK );
 879         APAdjustOkButton-> makeReturnDefault ( TRUE );
 880         APAdjustOkButton-> setCallback ( goAwayAPAdjust );
 881
 882         APAdjustCancelButton = new puOneShot ( 70, 10, 150, 50 );
 883         APAdjustCancelButton-> setLegend ( gui_msg_CANCEL );
 884         APAdjustCancelButton-> setCallback ( cancelAPAdjust );
 885
 886         APAdjustResetButton = new puOneShot ( 160, 10, 220, 50 );
 887         APAdjustResetButton-> setLegend ( gui_msg_RESET );
 888         APAdjustResetButton-> setCallback ( resetAPAdjust );
 889     }
 890     FG_FINALIZE_PUI_DIALOG( APAdjustDialog );
 891
 892 #undef HORIZONTAL
 893 }
 894
 895 // Simple Dialog to input Target Airport
 896 void TgtAptDialog_Cancel(puObject *)
 897 {
 898     FG_POP_PUI_DIALOG( TgtAptDialog );
 899 }
 900
 901 void TgtAptDialog_OK (puObject *)
 902 {
 903     fgAPDataPtr APData;
 904     APData = APDataGlobal;
 905     string TgtAptId;
 906
 907     //    FGTime *t = FGTime::cur_time_params;
 908     //    int PauseMode = t->getPause();
 909     //    if(!PauseMode)
 910     //        t->togglePauseMode();
 911
 912     char *s;
 913     TgtAptDialogInput->getValue(&s);
 914     TgtAptId = s;
 915
 916     TgtAptDialog_Cancel( NULL );
 917
 918     if ( TgtAptId.length() ) {
 919         // set initial position from TgtAirport id
 920
 921         fgAIRPORTS airports;
 922         fgAIRPORT a;
 923
 924         FG_LOG( FG_GENERAL, FG_INFO,
 925                 "Attempting to set starting position from airport code "
 926                 << s );
 927
 928         airports.load("apt_simple");
 929         if ( airports.search( TgtAptId, &a ) )
 930             {
 931                 double course, reverse, distance;
 932                 //            fgAPset_tgt_airport_id( TgtAptId.c_str() );
 933                 current_options.set_airport_id( TgtAptId.c_str() );
 934                 sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
 935
 936                 APData->TargetLatitude = a.latitude; // * DEG_TO_RAD;
 937                 APData->TargetLongitude = a.longitude; // * DEG_TO_RAD;
 938                 MakeTargetLatLonStr( APData,
 939                                      APData->TargetLatitude,
 940                                      APData->TargetLongitude);
 941
 942                 APData->old_lat = fgAPget_latitude();
 943                 APData->old_lon = fgAPget_longitude();
 944
 945                 // need to test for iter
 946                 if( ! geo_inverse_wgs_84( fgAPget_altitude(),
 947                                           fgAPget_latitude(),
 948                                           fgAPget_longitude(),
 949                                           APData->TargetLatitude,
 950                                           APData->TargetLongitude,
 951                                           &course,
 952                                           &reverse,
 953                                           &distance ) ) {
 954                     APData->TargetHeading = course;
 955                     MakeTargetHeadingStr( APData, APData->TargetHeading );
 956                     APData->TargetDistance = distance;
 957                     MakeTargetDistanceStr( APData, distance );
 958                     // This changes the AutoPilot Heading
 959                     // following cast needed
 960                     ApHeadingDialogInput->
 961                         setValue((float)APData->TargetHeading);
 962                     // Force this !
 963                     APData->waypoint_hold = true ;
 964                     APData->heading_hold = true;
 965                 }
 966             } else {
 967                 TgtAptId  += " not in database.";
 968                 mkDialog(TgtAptId.c_str());
 969             }
 970     }
 971     get_control_values();
 972     //    if( PauseMode != t->getPause() )
 973     //        t->togglePauseMode();
 974 }
 975
 976 void TgtAptDialog_Reset(puObject *)
 977 {
 978     //  strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
 979     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 980     TgtAptDialogInput->setValue ( NewTgtAirportId );
 981     TgtAptDialogInput->setCursor( 0 ) ;
 982 }
 983
 984 void NewTgtAirport(puObject *cb)
 985 {
 986     //  strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
 987     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 988     TgtAptDialogInput->setValue( NewTgtAirportId );
 989
 990     FG_PUSH_PUI_DIALOG( TgtAptDialog );
 991 }
 992
 993 void NewTgtAirportInit(void)
 994 {
 995     FG_LOG( FG_AUTOPILOT, FG_INFO, " enter NewTgtAirportInit()" );
 996     //  fgAPset_tgt_airport_id( current_options.get_airport_id() );
 997     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 998     FG_LOG( FG_AUTOPILOT, FG_INFO, " NewTgtAirportId " << NewTgtAirportId );
 999     //  printf(" NewTgtAirportId %s\n", NewTgtAirportId);
1000     int len = 150 - puGetStringWidth( puGetDefaultLabelFont(),
1001                                       NewTgtAirportLabel ) / 2;
1002
1003     TgtAptDialog = new puDialogBox (150, 50);
1004     {
1005         TgtAptDialogFrame   = new puFrame           (0,0,350, 150);
1006         TgtAptDialogMessage = new puText            (len, 110);
1007         TgtAptDialogMessage ->    setLabel          (NewTgtAirportLabel);
1008
1009         TgtAptDialogInput   = new puInput           (50, 70, 300, 100);
1010         TgtAptDialogInput   ->    setValue          (NewTgtAirportId);
1011         TgtAptDialogInput   ->    acceptInput();
1012
1013         TgtAptDialogOkButton     =  new puOneShot   (50, 10, 110, 50);
1014         TgtAptDialogOkButton     ->     setLegend   (gui_msg_OK);
1015         TgtAptDialogOkButton     ->     setCallback (TgtAptDialog_OK);
1016         TgtAptDialogOkButton     ->     makeReturnDefault(TRUE);
1017
1018         TgtAptDialogCancelButton =  new puOneShot   (140, 10, 210, 50);
1019         TgtAptDialogCancelButton ->     setLegend   (gui_msg_CANCEL);
1020         TgtAptDialogCancelButton ->     setCallback (TgtAptDialog_Cancel);
1021
1022         TgtAptDialogResetButton  =  new puOneShot   (240, 10, 300, 50);
1023         TgtAptDialogResetButton  ->     setLegend   (gui_msg_RESET);
1024         TgtAptDialogResetButton  ->     setCallback (TgtAptDialog_Reset);
1025     }
1026     FG_FINALIZE_PUI_DIALOG( TgtAptDialog );
1027     printf("leave NewTgtAirportInit()");
1028 }
1029
1030
1031 // Finally actual guts of AutoPilot
1032 void fgAPInit( fgAIRCRAFT *current_aircraft ) {
1033     fgAPDataPtr APData;
1034
1035     FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
1036
1037     APData = ( fgAPDataPtr ) calloc( sizeof( fgAPData ), 1 );
1038
1039     if ( APData == NULL ) {
1040         // I couldn't get the mem.  Dying
1041         FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying." );
1042         exit( -1 );
1043     }
1044
1045     FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot allocated " );
1046
1047     APData->waypoint_hold = false ;     // turn the target hold off
1048     APData->heading_hold = false ;      // turn the heading hold off
1049     APData->altitude_hold = false ;     // turn the altitude hold off
1050
1051     // Initialize target location to startup location
1052     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting startup location" );
1053     APData->old_lat =
1054         APData->TargetLatitude = fgAPget_latitude();
1055     APData->old_lon =
1056         APData->TargetLongitude = fgAPget_longitude();
1057
1058     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting TargetLatitudeStr" );
1059     MakeTargetLatLonStr( APData, APData->TargetLatitude, APData->TargetLongitude);
1060
1061     APData->TargetHeading = 0.0;     // default direction, due north
1062     APData->TargetAltitude = 3000;   // default altitude in meters
1063     APData->alt_error_accum = 0.0;
1064
1065     MakeTargetAltitudeStr( APData, 3000.0);
1066     MakeTargetHeadingStr( APData, 0.0 );
1067
1068     // These eventually need to be read from current_aircaft somehow.
1069
1070 #if 0
1071     // Original values
1072     // the maximum roll, in Deg
1073     APData->MaxRoll = 7;
1074     // the deg from heading to start rolling out at, in Deg
1075     APData->RollOut = 30;
1076     // how far can I move the aleron from center.
1077     APData->MaxAileron = .1;
1078     // Smoothing distance for alerion control
1079     APData->RollOutSmooth = 10;
1080 #endif
1081
1082     // the maximum roll, in Deg
1083     APData->MaxRoll = 20;
1084
1085     // the deg from heading to start rolling out at, in Deg
1086     APData->RollOut = 20;
1087
1088     // how far can I move the aleron from center.
1089     APData->MaxAileron = .2;
1090
1091     // Smoothing distance for alerion control
1092     APData->RollOutSmooth = 10;
1093
1094     //Remove at a later date
1095     APDataGlobal = APData;
1096
1097     // Hardwired for now should be in options
1098     // 25% max control variablilty  0.5 / 2.0
1099     APData->disengage_threshold = 1.0;
1100
1101 #if !defined( USING_SLIDER_CLASS )
1102     MaxRollAdjust = 2 * APData->MaxRoll;
1103     RollOutAdjust = 2 * APData->RollOut;
1104     MaxAileronAdjust = 2 * APData->MaxAileron;
1105     RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
1106 #endif  // !defined( USING_SLIDER_CLASS )
1107
1108     get_control_values();
1109
1110     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " calling NewTgtAirportInit" );
1111     NewTgtAirportInit();
1112     fgAPAdjustInit() ;
1113     NewHeadingInit();
1114     NewAltitudeInit();
1115 };
1116
1117
1118 void fgAPReset( void ) {
1119     fgAPDataPtr APData = APDataGlobal;
1120
1121     if ( fgAPTerrainFollowEnabled() )
1122         fgAPToggleTerrainFollow( );
1123
1124     if ( fgAPAltitudeEnabled() )
1125         fgAPToggleAltitude();
1126
1127     if ( fgAPHeadingEnabled() )
1128         fgAPToggleHeading();
1129
1130     if ( fgAPAutoThrottleEnabled() )
1131         fgAPToggleAutoThrottle();
1132
1133     APData->TargetHeading = 0.0;     // default direction, due north
1134     MakeTargetHeadingStr( APData, APData->TargetHeading );
1135
1136     APData->TargetAltitude = 3000;   // default altitude in meters
1137     MakeTargetAltitudeStr( APData, 3000);
1138
1139     APData->alt_error_accum = 0.0;
1140
1141
1142     get_control_values();
1143
1144     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
1145
1146     APData->TargetLatitude = fgAPget_latitude();
1147     APData->TargetLongitude = fgAPget_longitude();
1148     MakeTargetLatLonStr( APData,
1149                          APData->TargetLatitude,
1150                          APData->TargetLongitude);
1151 }
1152
1153
1154 int fgAPRun( void ) {
1155     // Remove the following lines when the calling funcitons start
1156     // passing in the data pointer
1157
1158     fgAPDataPtr APData;
1159
1160     APData = APDataGlobal;
1161     // end section
1162
1163     // get control settings
1164     double aileron = controls.get_aileron();
1165     double elevator = controls.get_elevator();
1166     double elevator_trim = controls.get_elevator_trim();
1167     double rudder = controls.get_rudder();
1168
1169     double lat = fgAPget_latitude();
1170     double lon = fgAPget_longitude();
1171
1172 #ifdef FG_FORCE_AUTO_DISENGAGE
1173     // see if somebody else has changed them
1174     if( fabs(aileron - APData->old_aileron) > APData->disengage_threshold ||
1175         fabs(elevator - APData->old_elevator) > APData->disengage_threshold ||
1176         fabs(elevator_trim - APData->old_elevator_trim) >
1177         APData->disengage_threshold ||
1178         fabs(rudder - APData->old_rudder) > APData->disengage_threshold )
1179     {
1180         // if controls changed externally turn autopilot off
1181         APData->waypoint_hold = false ;     // turn the target hold off
1182         APData->heading_hold = false ;      // turn the heading hold off
1183         APData->altitude_hold = false ;     // turn the altitude hold off
1184         APData->terrain_follow = false;     // turn the terrain_follow hold off
1185         // APData->auto_throttle = false;      // turn the auto_throttle off
1186
1187         // stash this runs control settings
1188         APData->old_aileron = aileron;
1189         APData->old_elevator = elevator;
1190         APData->old_elevator_trim = elevator_trim;
1191         APData->old_rudder = rudder;
1192
1193         return 0;
1194     }
1195 #endif
1196
1197     // waypoint hold enabled?
1198     if ( APData->waypoint_hold == true )
1199         {
1200             double wp_course, wp_reverse, wp_distance;
1201
1202 #ifdef DO_fgAP_CORRECTED_COURSE
1203             // compute course made good
1204             // this needs lots of special casing before use
1205             double course, reverse, distance, corrected_course;
1206             // need to test for iter
1207             geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1208                                 APData->old_lat,
1209                                 APData->old_lon,
1210                                 lat,
1211                                 lon,
1212                                 &course,
1213                                 &reverse,
1214                                 &distance );
1215 #endif // DO_fgAP_CORRECTED_COURSE
1216
1217             // compute course to way_point
1218             // need to test for iter
1219             if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1220                                       lat,
1221                                       lon,
1222                                       APData->TargetLatitude,
1223                                       APData->TargetLongitude,
1224                                       &wp_course,
1225                                       &wp_reverse,
1226                                       &wp_distance ) ) {
1227
1228 #ifdef DO_fgAP_CORRECTED_COURSE
1229                 corrected_course = course - wp_course;
1230                 if( fabs(corrected_course) > 0.1 )
1231                     printf("fgAP: course %f  wp_course %f  %f  %f\n",
1232                            course, wp_course, fabs(corrected_course), distance );
1233 #endif // DO_fgAP_CORRECTED_COURSE
1234
1235                 if ( wp_distance > 100 ) {
1236                     // corrected_course = course - wp_course;
1237                     APData->TargetHeading = NormalizeDegrees(wp_course);
1238                 } else {
1239                     printf("APData->distance(%f) to close\n", wp_distance);
1240                     // Real Close -- set heading hold to current heading
1241                     // and Ring the arival bell !!
1242                     NewTgtAirport(NULL);
1243                     APData->waypoint_hold = false;
1244                     // use previous
1245                     APData->TargetHeading = fgAPget_heading();
1246                 }
1247                 MakeTargetHeadingStr( APData, APData->TargetHeading );
1248                 // Force this just in case
1249                 APData->TargetDistance = wp_distance;
1250                 MakeTargetDistanceStr( APData, wp_distance );
1251                 // This changes the AutoPilot Heading Read Out
1252                 // following cast needed
1253                 ApHeadingDialogInput   ->    setValue ((float)APData->TargetHeading );
1254             }
1255             APData->heading_hold = true;
1256         }
1257
1258     // heading hold enabled?
1259     if ( APData->heading_hold == true ) {
1260         double RelHeading;
1261         double TargetRoll;
1262         double RelRoll;
1263         double AileronSet;
1264
1265         RelHeading =
1266             NormalizeDegrees( APData->TargetHeading - fgAPget_heading() );
1267         // figure out how far off we are from desired heading
1268
1269         // Now it is time to deterime how far we should be rolled.
1270         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
1271
1272
1273         // Check if we are further from heading than the roll out point
1274         if ( fabs( RelHeading ) > APData->RollOut ) {
1275             // set Target Roll to Max in desired direction
1276             if ( RelHeading < 0 ) {
1277                 TargetRoll = 0 - APData->MaxRoll;
1278             } else {
1279                 TargetRoll = APData->MaxRoll;
1280             }
1281         } else {
1282             // We have to calculate the Target roll
1283
1284             // This calculation engine thinks that the Target roll
1285             // should be a line from (RollOut,MaxRoll) to (-RollOut,
1286             // -MaxRoll) I hope this works well.  If I get ambitious
1287             // some day this might become a fancier curve or
1288             // something.
1289
1290             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
1291                                             -APData->MaxRoll, APData->RollOut,
1292                                             APData->MaxRoll );
1293         }
1294
1295         // Target Roll has now been Found.
1296
1297         // Compare Target roll to Current Roll, Generate Rel Roll
1298
1299         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
1300
1301         RelRoll = NormalizeDegrees( TargetRoll - fgAPget_roll() );
1302
1303         // Check if we are further from heading than the roll out smooth point
1304         if ( fabs( RelRoll ) > APData->RollOutSmooth ) {
1305             // set Target Roll to Max in desired direction
1306             if ( RelRoll < 0 ) {
1307                 AileronSet = 0 - APData->MaxAileron;
1308             } else {
1309                 AileronSet = APData->MaxAileron;
1310             }
1311         } else {
1312             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
1313                                             -APData->MaxAileron,
1314                                             APData->RollOutSmooth,
1315                                             APData->MaxAileron );
1316         }
1317
1318         controls.set_aileron( AileronSet );
1319         controls.set_rudder( AileronSet / 2.0 );
1320         // controls.set_rudder( 0.0 );
1321     }
1322
1323     // altitude hold or terrain follow enabled?
1324     if ( APData->altitude_hold || APData->terrain_follow ) {
1325         double speed, max_climb, error;
1326         double prop_error, int_error;
1327         double prop_adj, int_adj, total_adj;
1328
1329         if ( APData->altitude_hold ) {
1330             // normal altitude hold
1331             APData->TargetClimbRate =
1332                 ( APData->TargetAltitude - fgAPget_altitude() ) * 8.0;
1333         } else if ( APData->terrain_follow ) {
1334             // brain dead ground hugging with no look ahead
1335             APData->TargetClimbRate =
1336                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
1337             // cout << "target agl = " << APData->TargetAGL
1338             //      << "  current agl = " << fgAPget_agl()
1339             //      << "  target climb rate = " << APData->TargetClimbRate
1340             //      << endl;
1341         } else {
1342             // just try to zero out rate of climb ...
1343             APData->TargetClimbRate = 0.0;
1344         }
1345
1346         speed = get_speed();
1347
1348         if ( speed < min_climb ) {
1349             max_climb = 0.0;
1350         } else if ( speed < best_climb ) {
1351             max_climb = ((best_climb - min_climb) - (best_climb - speed))
1352                 * ideal_climb_rate
1353                 / (best_climb - min_climb);
1354         } else {
1355             max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
1356         }
1357
1358         // this first one could be optional if we wanted to allow
1359         // better climb performance assuming we have the airspeed to
1360         // support it.
1361         if ( APData->TargetClimbRate > ideal_climb_rate ) {
1362             APData->TargetClimbRate = ideal_climb_rate;
1363         }
1364
1365         if ( APData->TargetClimbRate > max_climb ) {
1366             APData->TargetClimbRate = max_climb;
1367         }
1368
1369         if ( APData->TargetClimbRate < -ideal_climb_rate ) {
1370             APData->TargetClimbRate = -ideal_climb_rate;
1371         }
1372
1373         error = fgAPget_climb() - APData->TargetClimbRate;
1374         // cout << "climb rate = " << fgAPget_climb()
1375         //      << "  error = " << error << endl;
1376
1377         // accumulate the error under the curve ... this really should
1378         // be *= delta t
1379         APData->alt_error_accum += error;
1380
1381         // calculate integral error, and adjustment amount
1382         int_error = APData->alt_error_accum;
1383         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1384         int_adj = int_error / 8000.0;
1385
1386         // caclulate proportional error
1387         prop_error = error;
1388         prop_adj = prop_error / 2000.0;
1389
1390         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1391         // if ( total_adj > 0.6 ) {
1392         //     total_adj = 0.6;
1393         // } else if ( total_adj < -0.2 ) {
1394         //     total_adj = -0.2;
1395         // }
1396         if ( total_adj > 1.0 ) {
1397             total_adj = 1.0;
1398         } else if ( total_adj < -1.0 ) {
1399             total_adj = -1.0;
1400         }
1401
1402         controls.set_elevator( total_adj );
1403     }
1404
1405     // auto throttle enabled?
1406     if ( APData->auto_throttle ) {
1407         double error;
1408         double prop_error, int_error;
1409         double prop_adj, int_adj, total_adj;
1410
1411         error = APData->TargetSpeed - get_speed();
1412
1413         // accumulate the error under the curve ... this really should
1414         // be *= delta t
1415         APData->speed_error_accum += error;
1416         if ( APData->speed_error_accum > 2000.0 ) {
1417             APData->speed_error_accum = 2000.0;
1418         }
1419         else if ( APData->speed_error_accum < -2000.0 ) {
1420             APData->speed_error_accum = -2000.0;
1421         }
1422
1423         // calculate integral error, and adjustment amount
1424         int_error = APData->speed_error_accum;
1425
1426         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1427         int_adj = int_error / 200.0;
1428
1429         // caclulate proportional error
1430         prop_error = error;
1431         prop_adj = 0.5 + prop_error / 50.0;
1432
1433         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1434         if ( total_adj > 1.0 ) {
1435             total_adj = 1.0;
1436         }
1437         else if ( total_adj < 0.0 ) {
1438             total_adj = 0.0;
1439         }
1440
1441         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
1442     }
1443
1444 #ifdef THIS_CODE_IS_NOT_USED
1445     if (APData->Mode == 2) // Glide slope hold
1446         {
1447             double RelSlope;
1448             double RelElevator;
1449
1450             // First, calculate Relative slope and normalize it
1451             RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
1452
1453             // Now calculate the elevator offset from current angle
1454             if ( abs(RelSlope) > APData->SlopeSmooth )
1455                 {
1456                     if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
1457                         RelElevator = -APData->MaxElevator;
1458                     else
1459                         RelElevator = APData->MaxElevator;
1460                 }
1461
1462             else
1463                 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
1464
1465             // set the elevator
1466             fgElevMove(RelElevator);
1467
1468         }
1469 #endif // THIS_CODE_IS_NOT_USED
1470
1471     // stash this runs control settings
1472     //  get_control_values();
1473     APData->old_aileron = controls.get_aileron();
1474     APData->old_elevator = controls.get_elevator();
1475     APData->old_elevator_trim = controls.get_elevator_trim();
1476     APData->old_rudder = controls.get_rudder();
1477
1478     // for cross track error
1479     APData->old_lat = lat;
1480     APData->old_lon = lon;
1481
1482         // Ok, we are done
1483     return 0;
1484 }
1485
1486 /*
1487 void fgAPSetMode( int mode)
1488 {
1489 //Remove the following line when the calling funcitons start passing in the data pointer
1490 fgAPDataPtr APData;
1491
1492 APData = APDataGlobal;
1493 // end section
1494
1495 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
1496
1497 APData->Mode = mode;  // set the new mode
1498 }
1499 */
1500 #if 0
1501 void fgAPset_tgt_airport_id( const string id ) {
1502     FG_LOG( FG_AUTOPILOT, FG_INFO, "entering  fgAPset_tgt_airport_id " << id );
1503     fgAPDataPtr APData;
1504     APData = APDataGlobal;
1505     APData->tgt_airport_id = id;
1506     FG_LOG( FG_AUTOPILOT, FG_INFO, "leaving  fgAPset_tgt_airport_id "
1507             << APData->tgt_airport_id );
1508 };
1509
1510 string fgAPget_tgt_airport_id( void ) {
1511     fgAPDataPtr APData = APDataGlobal;
1512     return APData->tgt_airport_id;
1513 };
1514 #endif
1515
1516 void fgAPToggleHeading( void ) {
1517     // Remove at a later date
1518     fgAPDataPtr APData;
1519
1520     APData = APDataGlobal;
1521     // end section
1522
1523     if ( APData->heading_hold || APData->waypoint_hold ) {
1524         // turn off heading hold
1525         APData->heading_hold = false;
1526         APData->waypoint_hold = false;
1527     } else {
1528         // turn on heading hold, lock at current heading
1529         APData->heading_hold = true;
1530         APData->TargetHeading = fgAPget_heading();
1531         MakeTargetHeadingStr( APData, APData->TargetHeading );
1532         ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1533     }
1534
1535     get_control_values();
1536     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
1537             << APData->heading_hold << ") " << APData->TargetHeading );
1538 }
1539
1540 void fgAPToggleWayPoint( void ) {
1541     // Remove at a later date
1542     fgAPDataPtr APData;
1543
1544     APData = APDataGlobal;
1545     // end section
1546
1547     if ( APData->waypoint_hold ) {
1548         // turn off location hold
1549         APData->waypoint_hold = false;
1550         // set heading hold to current heading
1551         //              APData->heading_hold = true;
1552         APData->TargetHeading = fgAPget_heading();
1553     } else {
1554         double course, reverse, distance;
1555         // turn on location hold
1556         // turn on heading hold
1557         APData->old_lat = fgAPget_latitude();
1558         APData->old_lon = fgAPget_longitude();
1559
1560                         // need to test for iter
1561         if(!geo_inverse_wgs_84( fgAPget_altitude(),
1562                                 fgAPget_latitude(),
1563                                 fgAPget_longitude(),
1564                                 APData->TargetLatitude,
1565                                 APData->TargetLongitude,
1566                                 &course,
1567                                 &reverse,
1568                                 &distance ) ) {
1569             APData->TargetHeading = course;
1570             APData->TargetDistance = distance;
1571             MakeTargetDistanceStr( APData, distance );
1572         }
1573         // Force this !
1574         APData->waypoint_hold = true;
1575         APData->heading_hold = true;
1576     }
1577
1578     // This changes the AutoPilot Heading
1579     // following cast needed
1580     ApHeadingDialogInput->setValue ((float)APData->TargetHeading );
1581     MakeTargetHeadingStr( APData, APData->TargetHeading );
1582
1583     get_control_values();
1584
1585     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
1586             << APData->waypoint_hold   << " "
1587             << APData->TargetLatitude  << " "
1588             << APData->TargetLongitude << " ) "
1589             );
1590 }
1591
1592
1593 void fgAPToggleAltitude( void ) {
1594     // Remove at a later date
1595     fgAPDataPtr APData;
1596
1597     APData = APDataGlobal;
1598     // end section
1599
1600     if ( APData->altitude_hold ) {
1601         // turn off altitude hold
1602         APData->altitude_hold = false;
1603     } else {
1604         // turn on altitude hold, lock at current altitude
1605         APData->altitude_hold = true;
1606         APData->terrain_follow = false;
1607         APData->TargetAltitude = fgAPget_altitude();
1608         APData->alt_error_accum = 0.0;
1609         // alt_error_queue.erase( alt_error_queue.begin(),
1610         //                        alt_error_queue.end() );
1611         float target_alt = APData->TargetAltitude;
1612         if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
1613             target_alt *= METER_TO_FEET;
1614
1615         ApAltitudeDialogInput->setValue(target_alt);
1616         MakeTargetAltitudeStr( APData, target_alt);
1617     }
1618
1619     get_control_values();
1620     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
1621             << APData->altitude_hold << ") " << APData->TargetAltitude );
1622 }
1623
1624
1625 void fgAPToggleAutoThrottle ( void ) {
1626     // Remove at a later date
1627     fgAPDataPtr APData;
1628
1629     APData = APDataGlobal;
1630     // end section
1631
1632     if ( APData->auto_throttle ) {
1633         // turn off altitude hold
1634         APData->auto_throttle = false;
1635     } else {
1636         // turn on terrain follow, lock at current agl
1637         APData->auto_throttle = true;
1638         APData->TargetSpeed = get_speed();
1639         APData->speed_error_accum = 0.0;
1640     }
1641
1642     get_control_values();
1643     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1644             << APData->auto_throttle << ") " << APData->TargetSpeed );
1645 }
1646
1647 void fgAPToggleTerrainFollow( void ) {
1648     // Remove at a later date
1649     fgAPDataPtr APData;
1650
1651     APData = APDataGlobal;
1652     // end section
1653
1654     if ( APData->terrain_follow ) {
1655         // turn off altitude hold
1656         APData->terrain_follow = false;
1657     } else {
1658         // turn on terrain follow, lock at current agl
1659         APData->terrain_follow = true;
1660         APData->altitude_hold = false;
1661         APData->TargetAGL = fgAPget_agl();
1662         APData->alt_error_accum = 0.0;
1663     }
1664     get_control_values();
1665
1666     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
1667             << APData->terrain_follow << ") " << APData->TargetAGL );
1668 }
1669
1670 static double NormalizeDegrees( double Input ) {
1671     // normalize the input to the range (-180,180]
1672     // Input should not be greater than -360 to 360.
1673     // Current rules send the output to an undefined state.
1674     if ( Input > 180 )
1675         while(Input > 180 )
1676             Input -= 360;
1677     else if ( Input <= -180 )
1678         while ( Input <= -180 )
1679             Input += 360;
1680     return ( Input );
1681 };
1682
1683 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
1684     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
1685     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
1686
1687         // Could be
1688         // static double y = 0.0;
1689         // double dx = x2 -x1;
1690         // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
1691         // {
1692
1693     double m, b, y;          // the constants to find in y=mx+b
1694     // double m, b;
1695
1696     m = ( y2 - y1 ) / ( x2 - x1 );   // calculate the m
1697
1698     b = y1 - m * x1;       // calculate the b
1699
1700     y = m * x + b;       // the final calculation
1701
1702     // }
1703
1704     return ( y );
1705
1706 };