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