src/Autopilot/autopilot.cxx

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