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