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->setValue ((float)APData->TargetHeading );
 956                     // Force this !
 957                     APData->waypoint_hold = true ;
 958                     APData->heading_hold = true;
 959                 }
 960             } else {
 961                 TgtAptId  += " not in database.";
 962                 mkDialog(TgtAptId.c_str());
 963             }
 964     }
 965     get_control_values();
 966     //    if( PauseMode != t->getPause() )
 967     //        t->togglePauseMode();
 968 }
 969
 970 void TgtAptDialog_Reset(puObject *)
 971 {
 972     //  strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
 973     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 974     TgtAptDialogInput->setValue ( NewTgtAirportId );
 975     TgtAptDialogInput->setCursor( 0 ) ;
 976 }
 977
 978 void NewTgtAirport(puObject *cb)
 979 {
 980     //  strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
 981     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 982     TgtAptDialogInput->setValue( NewTgtAirportId );
 983
 984     FG_PUSH_PUI_DIALOG( TgtAptDialog );
 985 }
 986
 987 void NewTgtAirportInit(void)
 988 {
 989     FG_LOG( FG_AUTOPILOT, FG_INFO, " enter NewTgtAirportInit()" );
 990     //  fgAPset_tgt_airport_id( current_options.get_airport_id() );
 991     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 992     FG_LOG( FG_AUTOPILOT, FG_INFO, " NewTgtAirportId " << NewTgtAirportId );
 993     //  printf(" NewTgtAirportId %s\n", NewTgtAirportId);
 994     int len = 150 - puGetStringWidth( puGetDefaultLabelFont(),
 995                                       NewTgtAirportLabel ) / 2;
 996
 997     TgtAptDialog = new puDialogBox (150, 50);
 998     {
 999         TgtAptDialogFrame   = new puFrame           (0,0,350, 150);
1000         TgtAptDialogMessage = new puText            (len, 110);
1001         TgtAptDialogMessage ->    setLabel          (NewTgtAirportLabel);
1002
1003         TgtAptDialogInput   = new puInput           (50, 70, 300, 100);
1004         TgtAptDialogInput   ->    setValue          (NewTgtAirportId);
1005         TgtAptDialogInput   ->    acceptInput();
1006
1007         TgtAptDialogOkButton     =  new puOneShot   (50, 10, 110, 50);
1008         TgtAptDialogOkButton     ->     setLegend   (gui_msg_OK);
1009         TgtAptDialogOkButton     ->     setCallback (TgtAptDialog_OK);
1010         TgtAptDialogOkButton     ->     makeReturnDefault(TRUE);
1011
1012         TgtAptDialogCancelButton =  new puOneShot   (140, 10, 210, 50);
1013         TgtAptDialogCancelButton ->     setLegend   (gui_msg_CANCEL);
1014         TgtAptDialogCancelButton ->     setCallback (TgtAptDialog_Cancel);
1015
1016         TgtAptDialogResetButton  =  new puOneShot   (240, 10, 300, 50);
1017         TgtAptDialogResetButton  ->     setLegend   (gui_msg_RESET);
1018         TgtAptDialogResetButton  ->     setCallback (TgtAptDialog_Reset);
1019     }
1020     FG_FINALIZE_PUI_DIALOG( TgtAptDialog );
1021     printf("leave NewTgtAirportInit()");
1022 }
1023
1024
1025 // Finally actual guts of AutoPilot
1026 void fgAPInit( fgAIRCRAFT *current_aircraft ) {
1027     fgAPDataPtr APData;
1028
1029     FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
1030
1031     APData = ( fgAPDataPtr ) calloc( sizeof( fgAPData ), 1 );
1032
1033     if ( APData == NULL ) {
1034         // I couldn't get the mem.  Dying
1035         FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying." );
1036         exit( -1 );
1037     }
1038
1039     FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot allocated " );
1040
1041     APData->waypoint_hold = false ;     // turn the target hold off
1042     APData->heading_hold = false ;      // turn the heading hold off
1043     APData->altitude_hold = false ;     // turn the altitude hold off
1044
1045     // Initialize target location to startup location
1046     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting startup location" );
1047     APData->old_lat =
1048         APData->TargetLatitude = fgAPget_latitude();
1049     APData->old_lon =
1050         APData->TargetLongitude = fgAPget_longitude();
1051
1052     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting TargetLatitudeStr" );
1053     MakeTargetLatLonStr( APData, APData->TargetLatitude, APData->TargetLongitude);
1054
1055     APData->TargetHeading = 0.0;     // default direction, due north
1056     APData->TargetAltitude = 3000;   // default altitude in meters
1057     APData->alt_error_accum = 0.0;
1058
1059     MakeTargetAltitudeStr( APData, 3000.0);
1060     MakeTargetHeadingStr( APData, 0.0 );
1061
1062     // These eventually need to be read from current_aircaft somehow.
1063
1064 #if 0
1065     // Original values
1066     // the maximum roll, in Deg
1067     APData->MaxRoll = 7;
1068     // the deg from heading to start rolling out at, in Deg
1069     APData->RollOut = 30;
1070     // how far can I move the aleron from center.
1071     APData->MaxAileron = .1;
1072     // Smoothing distance for alerion control
1073     APData->RollOutSmooth = 10;
1074 #endif
1075
1076     // the maximum roll, in Deg
1077     APData->MaxRoll = 20;
1078
1079     // the deg from heading to start rolling out at, in Deg
1080     APData->RollOut = 20;
1081
1082     // how far can I move the aleron from center.
1083     APData->MaxAileron = .2;
1084
1085     // Smoothing distance for alerion control
1086     APData->RollOutSmooth = 10;
1087
1088     //Remove at a later date
1089     APDataGlobal = APData;
1090
1091     // Hardwired for now should be in options
1092     // 25% max control variablilty  0.5 / 2.0
1093     APData->disengage_threshold = 1.0;
1094
1095 #if !defined( USING_SLIDER_CLASS )
1096     MaxRollAdjust = 2 * APData->MaxRoll;
1097     RollOutAdjust = 2 * APData->RollOut;
1098     MaxAileronAdjust = 2 * APData->MaxAileron;
1099     RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
1100 #endif  // !defined( USING_SLIDER_CLASS )
1101
1102     get_control_values();
1103
1104     //  FG_LOG( FG_AUTOPILOT, FG_INFO, " calling NewTgtAirportInit" );
1105     NewTgtAirportInit();
1106     fgAPAdjustInit() ;
1107     NewHeadingInit();
1108     NewAltitudeInit();
1109 };
1110
1111
1112 void fgAPReset( void ) {
1113     fgAPDataPtr APData = APDataGlobal;
1114
1115     if ( fgAPTerrainFollowEnabled() )
1116         fgAPToggleTerrainFollow( );
1117
1118     if ( fgAPAltitudeEnabled() )
1119         fgAPToggleAltitude();
1120
1121     if ( fgAPHeadingEnabled() )
1122         fgAPToggleHeading();
1123
1124     if ( fgAPAutoThrottleEnabled() )
1125         fgAPToggleAutoThrottle();
1126
1127     APData->TargetHeading = 0.0;     // default direction, due north
1128     MakeTargetHeadingStr( APData, APData->TargetHeading );
1129
1130     APData->TargetAltitude = 3000;   // default altitude in meters
1131     MakeTargetAltitudeStr( APData, 3000);
1132
1133     APData->alt_error_accum = 0.0;
1134
1135
1136     get_control_values();
1137
1138     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
1139
1140     APData->TargetLatitude = fgAPget_latitude();
1141     APData->TargetLongitude = fgAPget_longitude();
1142     MakeTargetLatLonStr( APData,
1143                          APData->TargetLatitude,
1144                          APData->TargetLongitude);
1145 }
1146
1147
1148 int fgAPRun( void ) {
1149     // Remove the following lines when the calling funcitons start
1150     // passing in the data pointer
1151
1152     fgAPDataPtr APData;
1153
1154     APData = APDataGlobal;
1155     // end section
1156
1157     // get control settings
1158     double aileron = controls.get_aileron();
1159     double elevator = controls.get_elevator();
1160     double elevator_trim = controls.get_elevator_trim();
1161     double rudder = controls.get_rudder();
1162
1163     double lat = fgAPget_latitude();
1164     double lon = fgAPget_longitude();
1165
1166 #ifdef FG_FORCE_AUTO_DISENGAGE
1167     // see if somebody else has changed them
1168     if( fabs(aileron - APData->old_aileron) > APData->disengage_threshold ||
1169         fabs(elevator - APData->old_elevator) > APData->disengage_threshold ||
1170         fabs(elevator_trim - APData->old_elevator_trim) >
1171         APData->disengage_threshold ||
1172         fabs(rudder - APData->old_rudder) > APData->disengage_threshold )
1173     {
1174         // if controls changed externally turn autopilot off
1175         APData->waypoint_hold = false ;     // turn the target hold off
1176         APData->heading_hold = false ;      // turn the heading hold off
1177         APData->altitude_hold = false ;     // turn the altitude hold off
1178         APData->terrain_follow = false;     // turn the terrain_follow hold off
1179         // APData->auto_throttle = false;      // turn the auto_throttle off
1180
1181         // stash this runs control settings
1182         APData->old_aileron = aileron;
1183         APData->old_elevator = elevator;
1184         APData->old_elevator_trim = elevator_trim;
1185         APData->old_rudder = rudder;
1186
1187         return 0;
1188     }
1189 #endif
1190
1191     // waypoint hold enabled?
1192     if ( APData->waypoint_hold == true )
1193         {
1194             double wp_course, wp_reverse, wp_distance;
1195
1196 #ifdef DO_fgAP_CORRECTED_COURSE
1197             // compute course made good
1198             // this needs lots of special casing before use
1199             double course, reverse, distance, corrected_course;
1200             // need to test for iter
1201             geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1202                                 APData->old_lat,
1203                                 APData->old_lon,
1204                                 lat,
1205                                 lon,
1206                                 &course,
1207                                 &reverse,
1208                                 &distance );
1209 #endif // DO_fgAP_CORRECTED_COURSE
1210
1211             // compute course to way_point
1212             // need to test for iter
1213             if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1214                                       lat,
1215                                       lon,
1216                                       APData->TargetLatitude,
1217                                       APData->TargetLongitude,
1218                                       &wp_course,
1219                                       &wp_reverse,
1220                                       &wp_distance ) ) {
1221
1222 #ifdef DO_fgAP_CORRECTED_COURSE
1223                 corrected_course = course - wp_course;
1224                 if( fabs(corrected_course) > 0.1 )
1225                     printf("fgAP: course %f  wp_course %f  %f  %f\n",
1226                            course, wp_course, fabs(corrected_course), distance );
1227 #endif // DO_fgAP_CORRECTED_COURSE
1228
1229                 if ( wp_distance > 100 ) {
1230                     // corrected_course = course - wp_course;
1231                     APData->TargetHeading = NormalizeDegrees(wp_course);
1232                 } else {
1233                     printf("APData->distance(%f) to close\n", wp_distance);
1234                     // Real Close -- set heading hold to current heading
1235                     // and Ring the arival bell !!
1236                     NewTgtAirport(NULL);
1237                     APData->waypoint_hold = false;
1238                     // use previous
1239                     APData->TargetHeading = fgAPget_heading();
1240                 }
1241                 MakeTargetHeadingStr( APData, APData->TargetHeading );
1242                 // Force this just in case
1243                 APData->TargetDistance = wp_distance;
1244                 MakeTargetDistanceStr( APData, wp_distance );
1245                 // This changes the AutoPilot Heading Read Out
1246                 // following cast needed
1247                 ApHeadingDialogInput   ->    setValue ((float)APData->TargetHeading );
1248             }
1249             APData->heading_hold = true;
1250         }
1251
1252     // heading hold enabled?
1253     if ( APData->heading_hold == true ) {
1254         double RelHeading;
1255         double TargetRoll;
1256         double RelRoll;
1257         double AileronSet;
1258
1259         RelHeading =
1260             NormalizeDegrees( APData->TargetHeading - fgAPget_heading() );
1261         // figure out how far off we are from desired heading
1262
1263         // Now it is time to deterime how far we should be rolled.
1264         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
1265
1266
1267         // Check if we are further from heading than the roll out point
1268         if ( fabs( RelHeading ) > APData->RollOut ) {
1269             // set Target Roll to Max in desired direction
1270             if ( RelHeading < 0 ) {
1271                 TargetRoll = 0 - APData->MaxRoll;
1272             } else {
1273                 TargetRoll = APData->MaxRoll;
1274             }
1275         } else {
1276             // We have to calculate the Target roll
1277
1278             // This calculation engine thinks that the Target roll
1279             // should be a line from (RollOut,MaxRoll) to (-RollOut,
1280             // -MaxRoll) I hope this works well.  If I get ambitious
1281             // some day this might become a fancier curve or
1282             // something.
1283
1284             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
1285                                             -APData->MaxRoll, APData->RollOut,
1286                                             APData->MaxRoll );
1287         }
1288
1289         // Target Roll has now been Found.
1290
1291         // Compare Target roll to Current Roll, Generate Rel Roll
1292
1293         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
1294
1295         RelRoll = NormalizeDegrees( TargetRoll - fgAPget_roll() );
1296
1297         // Check if we are further from heading than the roll out smooth point
1298         if ( fabs( RelRoll ) > APData->RollOutSmooth ) {
1299             // set Target Roll to Max in desired direction
1300             if ( RelRoll < 0 ) {
1301                 AileronSet = 0 - APData->MaxAileron;
1302             } else {
1303                 AileronSet = APData->MaxAileron;
1304             }
1305         } else {
1306             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
1307                                             -APData->MaxAileron,
1308                                             APData->RollOutSmooth,
1309                                             APData->MaxAileron );
1310         }
1311
1312         controls.set_aileron( AileronSet );
1313         controls.set_rudder( AileronSet / 2.0 );
1314         // controls.set_rudder( 0.0 );
1315     }
1316
1317     // altitude hold or terrain follow enabled?
1318     if ( APData->altitude_hold || APData->terrain_follow ) {
1319         double speed, max_climb, error;
1320         double prop_error, int_error;
1321         double prop_adj, int_adj, total_adj;
1322
1323         if ( APData->altitude_hold ) {
1324             // normal altitude hold
1325             APData->TargetClimbRate =
1326                 ( APData->TargetAltitude - fgAPget_altitude() ) * 8.0;
1327         } else if ( APData->terrain_follow ) {
1328             // brain dead ground hugging with no look ahead
1329             APData->TargetClimbRate =
1330                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
1331             // cout << "target agl = " << APData->TargetAGL
1332             //      << "  current agl = " << fgAPget_agl()
1333             //      << "  target climb rate = " << APData->TargetClimbRate
1334             //      << endl;
1335         } else {
1336             // just try to zero out rate of climb ...
1337             APData->TargetClimbRate = 0.0;
1338         }
1339
1340         speed = get_speed();
1341
1342         if ( speed < 90.0 ) {
1343             max_climb = 0.0;
1344         } else if ( speed < 100.0 ) {
1345             max_climb = ( speed - 90.0 ) * 20;
1346             //        } else if ( speed < 150.0 ) {
1347         } else {
1348             max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
1349         } //else { // this is NHV hack
1350         //            max_climb = ( speed - 150.0 ) * 6.0 + 300.0;
1351         //        }
1352
1353         if ( APData->TargetClimbRate > max_climb ) {
1354             APData->TargetClimbRate = max_climb;
1355         }
1356
1357         else if ( APData->TargetClimbRate < -400.0 ) {
1358             APData->TargetClimbRate = -400.0;
1359         }
1360
1361         error = fgAPget_climb() - APData->TargetClimbRate;
1362         // cout << "climb rate = " << fgAPget_climb()
1363         //      << "  error = " << error << endl;
1364
1365         // accumulate the error under the curve ... this really should
1366         // be *= delta t
1367         APData->alt_error_accum += error;
1368
1369         // calculate integral error, and adjustment amount
1370         int_error = APData->alt_error_accum;
1371         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1372         int_adj = int_error / 8000.0;
1373
1374         // caclulate proportional error
1375         prop_error = error;
1376         prop_adj = prop_error / 2000.0;
1377
1378         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1379         if ( total_adj > 0.6 ) {
1380             total_adj = 0.6;
1381         }
1382         else if ( total_adj < -0.2 ) {
1383             total_adj = -0.2;
1384         }
1385
1386         controls.set_elevator( total_adj );
1387     }
1388
1389     // auto throttle enabled?
1390     if ( APData->auto_throttle ) {
1391         double error;
1392         double prop_error, int_error;
1393         double prop_adj, int_adj, total_adj;
1394
1395         error = APData->TargetSpeed - get_speed();
1396
1397         // accumulate the error under the curve ... this really should
1398         // be *= delta t
1399         APData->speed_error_accum += error;
1400         if ( APData->speed_error_accum > 2000.0 ) {
1401             APData->speed_error_accum = 2000.0;
1402         }
1403         else if ( APData->speed_error_accum < -2000.0 ) {
1404             APData->speed_error_accum = -2000.0;
1405         }
1406
1407         // calculate integral error, and adjustment amount
1408         int_error = APData->speed_error_accum;
1409
1410         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1411         int_adj = int_error / 200.0;
1412
1413         // caclulate proportional error
1414         prop_error = error;
1415         prop_adj = 0.5 + prop_error / 50.0;
1416
1417         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1418         if ( total_adj > 1.0 ) {
1419             total_adj = 1.0;
1420         }
1421         else if ( total_adj < 0.0 ) {
1422             total_adj = 0.0;
1423         }
1424
1425         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
1426     }
1427
1428 #ifdef THIS_CODE_IS_NOT_USED
1429     if (APData->Mode == 2) // Glide slope hold
1430         {
1431             double RelSlope;
1432             double RelElevator;
1433
1434             // First, calculate Relative slope and normalize it
1435             RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
1436
1437             // Now calculate the elevator offset from current angle
1438             if ( abs(RelSlope) > APData->SlopeSmooth )
1439                 {
1440                     if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
1441                         RelElevator = -APData->MaxElevator;
1442                     else
1443                         RelElevator = APData->MaxElevator;
1444                 }
1445
1446             else
1447                 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
1448
1449             // set the elevator
1450             fgElevMove(RelElevator);
1451
1452         }
1453 #endif // THIS_CODE_IS_NOT_USED
1454
1455     // stash this runs control settings
1456     //  get_control_values();
1457     APData->old_aileron = controls.get_aileron();
1458     APData->old_elevator = controls.get_elevator();
1459     APData->old_elevator_trim = controls.get_elevator_trim();
1460     APData->old_rudder = controls.get_rudder();
1461
1462     // for cross track error
1463     APData->old_lat = lat;
1464     APData->old_lon = lon;
1465
1466         // Ok, we are done
1467     return 0;
1468 }
1469
1470 /*
1471 void fgAPSetMode( int mode)
1472 {
1473 //Remove the following line when the calling funcitons start passing in the data pointer
1474 fgAPDataPtr APData;
1475
1476 APData = APDataGlobal;
1477 // end section
1478
1479 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
1480
1481 APData->Mode = mode;  // set the new mode
1482 }
1483 */
1484 #if 0
1485 void fgAPset_tgt_airport_id( const string id ) {
1486     FG_LOG( FG_AUTOPILOT, FG_INFO, "entering  fgAPset_tgt_airport_id " << id );
1487     fgAPDataPtr APData;
1488     APData = APDataGlobal;
1489     APData->tgt_airport_id = id;
1490     FG_LOG( FG_AUTOPILOT, FG_INFO, "leaving  fgAPset_tgt_airport_id "
1491             << APData->tgt_airport_id );
1492 };
1493
1494 string fgAPget_tgt_airport_id( void ) {
1495     fgAPDataPtr APData = APDataGlobal;
1496     return APData->tgt_airport_id;
1497 };
1498 #endif
1499
1500 void fgAPToggleHeading( void ) {
1501     // Remove at a later date
1502     fgAPDataPtr APData;
1503
1504     APData = APDataGlobal;
1505     // end section
1506
1507     if ( APData->heading_hold || APData->waypoint_hold ) {
1508         // turn off heading hold
1509         APData->heading_hold = false;
1510         APData->waypoint_hold = false;
1511     } else {
1512         // turn on heading hold, lock at current heading
1513         APData->heading_hold = true;
1514         APData->TargetHeading = fgAPget_heading();
1515         MakeTargetHeadingStr( APData, APData->TargetHeading );
1516         ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1517     }
1518
1519     get_control_values();
1520     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
1521             << APData->heading_hold << ") " << APData->TargetHeading );
1522 }
1523
1524 void fgAPToggleWayPoint( void ) {
1525     // Remove at a later date
1526     fgAPDataPtr APData;
1527
1528     APData = APDataGlobal;
1529     // end section
1530
1531     if ( APData->waypoint_hold ) {
1532         // turn off location hold
1533         APData->waypoint_hold = false;
1534         // set heading hold to current heading
1535         //              APData->heading_hold = true;
1536         APData->TargetHeading = fgAPget_heading();
1537     } else {
1538         double course, reverse, distance;
1539         // turn on location hold
1540         // turn on heading hold
1541         APData->old_lat = fgAPget_latitude();
1542         APData->old_lon = fgAPget_longitude();
1543
1544                         // need to test for iter
1545         if(!geo_inverse_wgs_84( fgAPget_altitude(),
1546                                 fgAPget_latitude(),
1547                                 fgAPget_longitude(),
1548                                 APData->TargetLatitude,
1549                                 APData->TargetLongitude,
1550                                 &course,
1551                                 &reverse,
1552                                 &distance ) ) {
1553             APData->TargetHeading = course;
1554             APData->TargetDistance = distance;
1555             MakeTargetDistanceStr( APData, distance );
1556         }
1557         // Force this !
1558         APData->waypoint_hold = true;
1559         APData->heading_hold = true;
1560     }
1561
1562     // This changes the AutoPilot Heading
1563     // following cast needed
1564     ApHeadingDialogInput->setValue ((float)APData->TargetHeading );
1565     MakeTargetHeadingStr( APData, APData->TargetHeading );
1566
1567     get_control_values();
1568
1569     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
1570             << APData->waypoint_hold   << " "
1571             << APData->TargetLatitude  << " "
1572             << APData->TargetLongitude << " ) "
1573             );
1574 }
1575
1576
1577 void fgAPToggleAltitude( void ) {
1578     // Remove at a later date
1579     fgAPDataPtr APData;
1580
1581     APData = APDataGlobal;
1582     // end section
1583
1584     if ( APData->altitude_hold ) {
1585         // turn off altitude hold
1586         APData->altitude_hold = false;
1587     } else {
1588         // turn on altitude hold, lock at current altitude
1589         APData->altitude_hold = true;
1590         APData->terrain_follow = false;
1591         APData->TargetAltitude = fgAPget_altitude();
1592         APData->alt_error_accum = 0.0;
1593         // alt_error_queue.erase( alt_error_queue.begin(),
1594         //                        alt_error_queue.end() );
1595         float target_alt = APData->TargetAltitude;
1596         if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
1597             target_alt *= METER_TO_FEET;
1598
1599         ApAltitudeDialogInput->setValue(target_alt);
1600         MakeTargetAltitudeStr( APData, target_alt);
1601     }
1602
1603     get_control_values();
1604     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
1605             << APData->altitude_hold << ") " << APData->TargetAltitude );
1606 }
1607
1608
1609 void fgAPToggleAutoThrottle ( void ) {
1610     // Remove at a later date
1611     fgAPDataPtr APData;
1612
1613     APData = APDataGlobal;
1614     // end section
1615
1616     if ( APData->auto_throttle ) {
1617         // turn off altitude hold
1618         APData->auto_throttle = false;
1619     } else {
1620         // turn on terrain follow, lock at current agl
1621         APData->auto_throttle = true;
1622         APData->TargetSpeed = get_speed();
1623         APData->speed_error_accum = 0.0;
1624     }
1625
1626     get_control_values();
1627     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1628             << APData->auto_throttle << ") " << APData->TargetSpeed );
1629 }
1630
1631 void fgAPToggleTerrainFollow( void ) {
1632     // Remove at a later date
1633     fgAPDataPtr APData;
1634
1635     APData = APDataGlobal;
1636     // end section
1637
1638     if ( APData->terrain_follow ) {
1639         // turn off altitude hold
1640         APData->terrain_follow = false;
1641     } else {
1642         // turn on terrain follow, lock at current agl
1643         APData->terrain_follow = true;
1644         APData->altitude_hold = false;
1645         APData->TargetAGL = fgAPget_agl();
1646         APData->alt_error_accum = 0.0;
1647     }
1648     get_control_values();
1649
1650     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
1651             << APData->terrain_follow << ") " << APData->TargetAGL );
1652 }
1653
1654 static double NormalizeDegrees( double Input ) {
1655     // normalize the input to the range (-180,180]
1656     // Input should not be greater than -360 to 360.
1657     // Current rules send the output to an undefined state.
1658     if ( Input > 180 )
1659         while(Input > 180 )
1660             Input -= 360;
1661     else if ( Input <= -180 )
1662         while ( Input <= -180 )
1663             Input += 360;
1664     return ( Input );
1665 };
1666
1667 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
1668     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
1669     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
1670
1671         // Could be
1672         // static double y = 0.0;
1673         // double dx = x2 -x1;
1674         // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
1675         // {
1676
1677     double m, b, y;          // the constants to find in y=mx+b
1678     // double m, b;
1679
1680     m = ( y2 - y1 ) / ( x2 - x1 );   // calculate the m
1681
1682     b = y1 - m * x1;       // calculate the b
1683
1684     y = m * x + b;       // the final calculation
1685
1686     // }
1687
1688     return ( y );
1689
1690 };
1691
1692
1693 /* Direct and inverse distance functions */
1694 /** Proceedings of the 7th International Symposium on Geodetic
1695   Computations, 1985
1696   "The Nested Coefficient Method for Accurate Solutions of Direct
1697   and
1698   Inverse Geodetic Problems With Any Length"
1699   Zhang Xue-Lian
1700   pp 747-763
1701   */
1702 /* modified for FlightGear to use WGS84 only  Norman Vine */
1703
1704 //#include "dstazfns.h"
1705 #include <math.h>
1706 #define GEOD_INV_PI      (3.14159265358979323846)
1707
1708 /* s == distance */
1709 /* az = azimuth */
1710
1711 /* for WGS_84 a = 6378137.000, rf = 298.257223563; */
1712
1713 static double M0( double e2 )
1714 {       //double e4 = e2*e2;
1715     return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + e2*(5.0/256.0) )))/2.0;
1716 }
1717 /* s == distance */
1718 int geo_direct_wgs_84 ( double alt, double lat1, double lon1, double az1, double s,
1719                         double *lat2, double *lon2,  double *az2 )
1720 {
1721     double a = 6378137.000, rf = 298.257223563;
1722     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
1723     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
1724     double b = a*(1.0-f), e2 = f*(2.0-f);
1725     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
1726     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
1727     double azm1 = az1*RADDEG;
1728     double sinaz1 = sin(azm1), cosaz1 = cos(azm1);
1729
1730
1731     if( fabs(s) < 0.01 )  /* distance < centimeter => congruency */
1732         {       *lat2 = lat1;
1733         *lon2 = lon1;
1734         *az2 = 180.0 + az1;
1735         if( *az2 > 360.0 ) *az2 -= 360.0;
1736         return 0;
1737         }
1738     else
1739         if( cosphi1 )    /* non-polar origin */
1740             {   /* u1 is reduced latitude */
1741                 double tanu1 = sqrt(1.0-e2)*sinphi1/cosphi1;
1742                 double sig1 = atan2(tanu1,cosaz1);
1743                 double cosu1 = 1.0/sqrt( 1.0 + tanu1*tanu1 ), sinu1 = tanu1*cosu1;
1744                 double sinaz =  cosu1*sinaz1, cos2saz = 1.0-sinaz*sinaz;
1745                 double us = cos2saz*e2/(1.0-e2);
1746                 /*      Terms */
1747                 double  ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/16384.0,
1748                     tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0,
1749                     tc = 0;
1750                 /*      FIRST ESTIMATE OF SIGMA (SIG) */
1751                 double first = s/(b*ta); /* !!*/
1752                 double sig = first;
1753                 double c2sigm, sinsig,cossig, temp,denom,rnumer, dlams, dlam;
1754                 do
1755                     {   c2sigm = cos(2.0*sig1+sig);
1756                     sinsig = sin(sig); cossig = cos(sig);
1757                     temp = sig;
1758                     sig = first +
1759                         tb*sinsig*(c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm) -
1760                                               tb*c2sigm*(-3.0+4.0*sinsig*sinsig)
1761                                               *(-3.0+4.0*c2sigm*c2sigm)/6.0)
1762                                    /4.0);
1763                     }
1764                 while( fabs(sig-temp) > testv);
1765                 /*      LATITUDE OF POINT 2 */
1766                 /*      DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER) */
1767                 temp = sinu1*sinsig-cosu1*cossig*cosaz1;
1768                 denom = (1.0-f)*sqrt(sinaz*sinaz+temp*temp);
1769                 /* NUMERATOR */
1770                 rnumer = sinu1*cossig+cosu1*sinsig*cosaz1;
1771                 *lat2 = atan2(rnumer,denom)/RADDEG;
1772                 /* DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS ) */
1773                 rnumer = sinsig*sinaz1;
1774                 denom = cosu1*cossig-sinu1*sinsig*cosaz1;
1775                 dlams = atan2(rnumer,denom);
1776                 /* TERM C */
1777                 tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
1778                 /* DIFFERENCE IN LONGITUDE */
1779                 dlam = dlams-(1.0-tc)*f*sinaz*(sig+tc*sinsig*
1780                                                (c2sigm+
1781                                                 tc*cossig*(-1.0+2.0*
1782                                                            c2sigm*c2sigm)));
1783                 *lon2 = (lam1+dlam)/RADDEG;
1784                 if(*lon2 > 180.0  ) *lon2 -= 360.0;
1785                 if(*lon2 < -180.0 ) *lon2 += 360.0;
1786                 /* AZIMUTH - FROM NORTH */
1787                 *az2 = atan2(-sinaz,temp)/RADDEG;
1788                 if( fabs(*az2) < testv ) *az2 = 0.0;
1789                 if( *az2 < 0.0) *az2 += 360.0;
1790                 return 0;
1791             }
1792         else /* phi1 == 90 degrees, polar origin  */
1793             {   double dM = a*M0(e2) - s;
1794             double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
1795             return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM,lat2,lon2,az2 );
1796             }
1797 }
1798
1799 int geo_inverse_wgs_84( double alt, double lat1, double lon1, double lat2,
1800                         double lon2, double *az1, double *az2, double *s )
1801 {
1802     double a = 6378137.000, rf = 298.257223563;
1803     int iter=0;
1804     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
1805     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
1806     double b = a*(1.0-f), e2 = f*(2.0-f);
1807     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
1808     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
1809     double phi2 = lat2*RADDEG, lam2 = lon2*RADDEG;
1810     double sinphi2 = sin(phi2), cosphi2 = cos(phi2);
1811
1812     if( (fabs(lat1-lat2) < testv &&
1813          ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) )
1814     {   /* TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
1815         *az1 = 0.0; *az2 = 0.0; *s = 0.0;
1816         return 0;
1817     } else
1818         if(  fabs(cosphi1) < testv ) /* initial point is polar */
1819         {
1820             int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
1821             b = *az1; *az1 = *az2; *az2 = b;
1822             return 0;
1823         } else
1824             if( fabs(cosphi2) < testv ) /* terminal point is polar */
1825             {
1826                 int k = geo_inverse_wgs_84( alt, lat1,lon1,lat1,lon1+180.0,
1827                                             az1,az2,s );
1828                 *s /= 2.0;
1829                 *az2 = *az1 + 180.0;
1830                 if( *az2 > 360.0 ) *az2 -= 360.0;
1831                 return 0;
1832             } else      /* Geodesic passes through the pole (antipodal) */
1833                 if( (fabs( fabs(lon1-lon2) - 180 ) < testv) &&
1834                     (fabs(lat1+lat2) < testv) )
1835                 {
1836                     double s1,s2;
1837                     geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
1838                     geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
1839                     *az2 = *az1;
1840                     *s = s1 + s2;
1841                     return 0;
1842                 } else  /* antipodal and polar points don't get here */
1843                 {
1844                     double dlam = lam2 - lam1, dlams = dlam;
1845                     double sdlams,cdlams, sig,sinsig,cossig, sinaz,
1846                         cos2saz, c2sigm;
1847                     double tc,temp, us,rnumer,denom, ta,tb;
1848                     double cosu1,sinu1, sinu2,cosu2;
1849                     /* Reduced latitudes */
1850                     temp = (1.0-f)*sinphi1/cosphi1;
1851                     cosu1 = 1.0/sqrt(1.0+temp*temp);
1852                     sinu1 = temp*cosu1;
1853                     temp = (1.0-f)*sinphi2/cosphi2;
1854                     cosu2 = 1.0/sqrt(1.0+temp*temp);
1855                     sinu2 = temp*cosu2;
1856
1857                     do {
1858                         sdlams = sin(dlams), cdlams = cos(dlams);
1859                         sinsig = sqrt(cosu2*cosu2*sdlams*sdlams+
1860                                       (cosu1*sinu2-sinu1*cosu2*cdlams)*
1861                                       (cosu1*sinu2-sinu1*cosu2*cdlams));
1862                         cossig = sinu1*sinu2+cosu1*cosu2*cdlams;
1863
1864                         sig = atan2(sinsig,cossig);
1865                         sinaz = cosu1*cosu2*sdlams/sinsig;
1866                         cos2saz = 1.0-sinaz*sinaz;
1867                         c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig :
1868                                   cossig-2.0*sinu1*sinu2/cos2saz);
1869                         tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
1870                         temp = dlams;
1871                         dlams = dlam+(1.0-tc)*f*sinaz*
1872                             (sig+tc*sinsig*
1873                              (c2sigm+tc*cossig*(-1.0+2.0*c2sigm*c2sigm)));
1874                         if (fabs(dlams) > GEOD_INV_PI && iter++ > 50)
1875                             return iter;
1876                     } while ( fabs(temp-dlams) > testv);
1877
1878                     us = cos2saz*(a*a-b*b)/(b*b);       /* !! */
1879                     /* BACK AZIMUTH FROM NORTH */
1880                     rnumer = -(cosu1*sdlams);
1881                     denom = sinu1*cosu2-cosu1*sinu2*cdlams;
1882                     *az2 = atan2(rnumer,denom)/RADDEG;
1883                     if( fabs(*az2) < testv ) *az2 = 0.0;
1884                     if(*az2 < 0.0) *az2 += 360.0;
1885                     /* FORWARD AZIMUTH FROM NORTH */
1886                     rnumer = cosu2*sdlams;
1887                     denom = cosu1*sinu2-sinu1*cosu2*cdlams;
1888                     *az1 = atan2(rnumer,denom)/RADDEG;
1889                     if( fabs(*az1) < testv ) *az1 = 0.0;
1890                     if(*az1 < 0.0) *az1 += 360.0;
1891                     /* Terms a & b */
1892                     ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/
1893                         16384.0;
1894                     tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0;
1895                     /* GEODETIC DISTANCE */
1896                     *s = b*ta*(sig-tb*sinsig*
1897                                (c2sigm+tb*(cossig*(-1.0+2.0*c2sigm*c2sigm)-tb*
1898                                            c2sigm*(-3.0+4.0*sinsig*sinsig)*
1899                                            (-3.0+4.0*c2sigm*c2sigm)/6.0)/
1900                                 4.0));
1901                     return 0;
1902                 }
1903 }