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