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