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 = cur_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( cur_fdm_state->get_V_equiv_kts() );
 213 }
 214
 215 static inline double get_aoa( void ) {
 216     return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
 217 }
 218
 219 static inline double fgAPget_latitude( void ) {
 220     return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
 221 }
 222
 223 static inline double fgAPget_longitude( void ) {
 224     return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
 225 }
 226
 227 static inline double fgAPget_roll( void ) {
 228     return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
 229 }
 230
 231 static inline double get_pitch( void ) {
 232     return( cur_fdm_state->get_Theta() );
 233 }
 234
 235 double fgAPget_heading( void ) {
 236     return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
 237 }
 238
 239 static inline double fgAPget_altitude( void ) {
 240     return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
 241 }
 242
 243 static inline double fgAPget_climb( void ) {
 244     // return in meters per minute
 245     return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
 246 }
 247
 248 static inline double get_sideslip( void ) {
 249     return( cur_fdm_state->get_Beta() );
 250 }
 251
 252 static inline double fgAPget_agl( void ) {
 253     double agl;
 254
 255     agl = cur_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 = cur_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 #ifdef FG_FORCE_AUTO_DISENGAGE
1164     // see if somebody else has changed them
1165     if( fabs(aileron - APData->old_aileron) > APData->disengage_threshold ||
1166         fabs(elevator - APData->old_elevator) > APData->disengage_threshold ||
1167         fabs(elevator_trim - APData->old_elevator_trim) >
1168         APData->disengage_threshold ||
1169         fabs(rudder - APData->old_rudder) > APData->disengage_threshold )
1170     {
1171         // if controls changed externally turn autopilot off
1172         APData->waypoint_hold = false ;     // turn the target hold off
1173         APData->heading_hold = false ;      // turn the heading hold off
1174         APData->altitude_hold = false ;     // turn the altitude hold off
1175         APData->terrain_follow = false;     // turn the terrain_follow hold off
1176         // APData->auto_throttle = false;      // turn the auto_throttle off
1177
1178         // stash this runs control settings
1179         APData->old_aileron = aileron;
1180         APData->old_elevator = elevator;
1181         APData->old_elevator_trim = elevator_trim;
1182         APData->old_rudder = rudder;
1183
1184         return 0;
1185     }
1186 #endif
1187
1188     // waypoint hold enabled?
1189     if ( APData->waypoint_hold == true )
1190         {
1191             double wp_course, wp_reverse, wp_distance;
1192
1193 #ifdef DO_fgAP_CORRECTED_COURSE
1194             // compute course made good
1195             // this needs lots of special casing before use
1196             double course, reverse, distance, corrected_course;
1197             // need to test for iter
1198             geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1199                                 APData->old_lat,
1200                                 APData->old_lon,
1201                                 lat,
1202                                 lon,
1203                                 &course,
1204                                 &reverse,
1205                                 &distance );
1206 #endif // DO_fgAP_CORRECTED_COURSE
1207
1208             // compute course to way_point
1209             // need to test for iter
1210             if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1211                                       lat,
1212                                       lon,
1213                                       APData->TargetLatitude,
1214                                       APData->TargetLongitude,
1215                                       &wp_course,
1216                                       &wp_reverse,
1217                                       &wp_distance ) ) {
1218
1219 #ifdef DO_fgAP_CORRECTED_COURSE
1220                 corrected_course = course - wp_course;
1221                 if( fabs(corrected_course) > 0.1 )
1222                     printf("fgAP: course %f  wp_course %f  %f  %f\n",
1223                            course, wp_course, fabs(corrected_course), distance );
1224 #endif // DO_fgAP_CORRECTED_COURSE
1225
1226                 if ( wp_distance > 100 ) {
1227                     // corrected_course = course - wp_course;
1228                     APData->TargetHeading = NormalizeDegrees(wp_course);
1229                 } else {
1230                     printf("APData->distance(%f) to close\n", wp_distance);
1231                     // Real Close -- set heading hold to current heading
1232                     // and Ring the arival bell !!
1233                     NewTgtAirport(NULL);
1234                     APData->waypoint_hold = false;
1235                     // use previous
1236                     APData->TargetHeading = fgAPget_heading();
1237                 }
1238                 MakeTargetHeadingStr( APData, APData->TargetHeading );
1239                 // Force this just in case
1240                 APData->TargetDistance = wp_distance;
1241                 MakeTargetDistanceStr( APData, wp_distance );
1242                 // This changes the AutoPilot Heading Read Out
1243                 // following cast needed
1244                 ApHeadingDialogInput   ->    setValue ((float)APData->TargetHeading );
1245             }
1246             APData->heading_hold = true;
1247         }
1248
1249     // heading hold enabled?
1250     if ( APData->heading_hold == true ) {
1251         double RelHeading;
1252         double TargetRoll;
1253         double RelRoll;
1254         double AileronSet;
1255
1256         RelHeading =
1257             NormalizeDegrees( APData->TargetHeading - fgAPget_heading() );
1258         // figure out how far off we are from desired heading
1259
1260         // Now it is time to deterime how far we should be rolled.
1261         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
1262
1263
1264         // Check if we are further from heading than the roll out point
1265         if ( fabs( RelHeading ) > APData->RollOut ) {
1266             // set Target Roll to Max in desired direction
1267             if ( RelHeading < 0 ) {
1268                 TargetRoll = 0 - APData->MaxRoll;
1269             } else {
1270                 TargetRoll = APData->MaxRoll;
1271             }
1272         } else {
1273             // We have to calculate the Target roll
1274
1275             // This calculation engine thinks that the Target roll
1276             // should be a line from (RollOut,MaxRoll) to (-RollOut,
1277             // -MaxRoll) I hope this works well.  If I get ambitious
1278             // some day this might become a fancier curve or
1279             // something.
1280
1281             TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
1282                                             -APData->MaxRoll, APData->RollOut,
1283                                             APData->MaxRoll );
1284         }
1285
1286         // Target Roll has now been Found.
1287
1288         // Compare Target roll to Current Roll, Generate Rel Roll
1289
1290         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
1291
1292         RelRoll = NormalizeDegrees( TargetRoll - fgAPget_roll() );
1293
1294         // Check if we are further from heading than the roll out smooth point
1295         if ( fabs( RelRoll ) > APData->RollOutSmooth ) {
1296             // set Target Roll to Max in desired direction
1297             if ( RelRoll < 0 ) {
1298                 AileronSet = 0 - APData->MaxAileron;
1299             } else {
1300                 AileronSet = APData->MaxAileron;
1301             }
1302         } else {
1303             AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
1304                                             -APData->MaxAileron,
1305                                             APData->RollOutSmooth,
1306                                             APData->MaxAileron );
1307         }
1308
1309         controls.set_aileron( AileronSet );
1310         controls.set_rudder( AileronSet / 2.0 );
1311         // controls.set_rudder( 0.0 );
1312     }
1313
1314     // altitude hold or terrain follow enabled?
1315     if ( APData->altitude_hold || APData->terrain_follow ) {
1316         double speed, max_climb, error;
1317         double prop_error, int_error;
1318         double prop_adj, int_adj, total_adj;
1319
1320         if ( APData->altitude_hold ) {
1321             // normal altitude hold
1322             APData->TargetClimbRate =
1323                 ( APData->TargetAltitude - fgAPget_altitude() ) * 8.0;
1324         } else if ( APData->terrain_follow ) {
1325             // brain dead ground hugging with no look ahead
1326             APData->TargetClimbRate =
1327                 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
1328         } else {
1329             // just try to zero out rate of climb ...
1330             APData->TargetClimbRate = 0.0;
1331         }
1332
1333         speed = get_speed();
1334
1335         if ( speed < 90.0 ) {
1336             max_climb = 0.0;
1337         } else if ( speed < 100.0 ) {
1338             max_climb = ( speed - 90.0 ) * 20;
1339             //        } else if ( speed < 150.0 ) {
1340         } else {
1341             max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
1342         } //else { // this is NHV hack
1343         //            max_climb = ( speed - 150.0 ) * 6.0 + 300.0;
1344         //        }
1345
1346         if ( APData->TargetClimbRate > max_climb ) {
1347             APData->TargetClimbRate = max_climb;
1348         }
1349
1350         else if ( APData->TargetClimbRate < -400.0 ) {
1351             APData->TargetClimbRate = -400.0;
1352         }
1353
1354         error = fgAPget_climb() - APData->TargetClimbRate;
1355
1356         // accumulate the error under the curve ... this really should
1357         // be *= delta t
1358         APData->alt_error_accum += error;
1359
1360         // calculate integral error, and adjustment amount
1361         int_error = APData->alt_error_accum;
1362         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1363         int_adj = int_error / 8000.0;
1364
1365         // caclulate proportional error
1366         prop_error = error;
1367         prop_adj = prop_error / 2000.0;
1368
1369         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1370         if ( total_adj > 0.6 ) {
1371             total_adj = 0.6;
1372         }
1373         else if ( total_adj < -0.2 ) {
1374             total_adj = -0.2;
1375         }
1376
1377         controls.set_elevator( total_adj );
1378     }
1379
1380     // auto throttle enabled?
1381     if ( APData->auto_throttle ) {
1382         double error;
1383         double prop_error, int_error;
1384         double prop_adj, int_adj, total_adj;
1385
1386         error = APData->TargetSpeed - get_speed();
1387
1388         // accumulate the error under the curve ... this really should
1389         // be *= delta t
1390         APData->speed_error_accum += error;
1391         if ( APData->speed_error_accum > 2000.0 ) {
1392             APData->speed_error_accum = 2000.0;
1393         }
1394         else if ( APData->speed_error_accum < -2000.0 ) {
1395             APData->speed_error_accum = -2000.0;
1396         }
1397
1398         // calculate integral error, and adjustment amount
1399         int_error = APData->speed_error_accum;
1400
1401         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
1402         int_adj = int_error / 200.0;
1403
1404         // caclulate proportional error
1405         prop_error = error;
1406         prop_adj = 0.5 + prop_error / 50.0;
1407
1408         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1409         if ( total_adj > 1.0 ) {
1410             total_adj = 1.0;
1411         }
1412         else if ( total_adj < 0.0 ) {
1413             total_adj = 0.0;
1414         }
1415
1416         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
1417     }
1418
1419 #ifdef THIS_CODE_IS_NOT_USED
1420     if (APData->Mode == 2) // Glide slope hold
1421         {
1422             double RelSlope;
1423             double RelElevator;
1424
1425             // First, calculate Relative slope and normalize it
1426             RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
1427
1428             // Now calculate the elevator offset from current angle
1429             if ( abs(RelSlope) > APData->SlopeSmooth )
1430                 {
1431                     if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
1432                         RelElevator = -APData->MaxElevator;
1433                     else
1434                         RelElevator = APData->MaxElevator;
1435                 }
1436
1437             else
1438                 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
1439
1440             // set the elevator
1441             fgElevMove(RelElevator);
1442
1443         }
1444 #endif // THIS_CODE_IS_NOT_USED
1445
1446     // stash this runs control settings
1447     //  get_control_values();
1448     APData->old_aileron = controls.get_aileron();
1449     APData->old_elevator = controls.get_elevator();
1450     APData->old_elevator_trim = controls.get_elevator_trim();
1451     APData->old_rudder = controls.get_rudder();
1452
1453     // for cross track error
1454     APData->old_lat = lat;
1455     APData->old_lon = lon;
1456
1457         // Ok, we are done
1458     return 0;
1459 }
1460
1461 /*
1462 void fgAPSetMode( int mode)
1463 {
1464 //Remove the following line when the calling funcitons start passing in the data pointer
1465 fgAPDataPtr APData;
1466
1467 APData = APDataGlobal;
1468 // end section
1469
1470 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
1471
1472 APData->Mode = mode;  // set the new mode
1473 }
1474 */
1475 #if 0
1476 void fgAPset_tgt_airport_id( const string id ) {
1477     FG_LOG( FG_AUTOPILOT, FG_INFO, "entering  fgAPset_tgt_airport_id " << id );
1478     fgAPDataPtr APData;
1479     APData = APDataGlobal;
1480     APData->tgt_airport_id = id;
1481     FG_LOG( FG_AUTOPILOT, FG_INFO, "leaving  fgAPset_tgt_airport_id "
1482             << APData->tgt_airport_id );
1483 };
1484
1485 string fgAPget_tgt_airport_id( void ) {
1486     fgAPDataPtr APData = APDataGlobal;
1487     return APData->tgt_airport_id;
1488 };
1489 #endif
1490
1491 void fgAPToggleHeading( void ) {
1492     // Remove at a later date
1493     fgAPDataPtr APData;
1494
1495     APData = APDataGlobal;
1496     // end section
1497
1498     if ( APData->heading_hold || APData->waypoint_hold ) {
1499         // turn off heading hold
1500         APData->heading_hold = false;
1501         APData->waypoint_hold = false;
1502     } else {
1503         // turn on heading hold, lock at current heading
1504         APData->heading_hold = true;
1505         APData->TargetHeading = fgAPget_heading();
1506         MakeTargetHeadingStr( APData, APData->TargetHeading );
1507         ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1508     }
1509
1510     get_control_values();
1511     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
1512             << APData->heading_hold << ") " << APData->TargetHeading );
1513 }
1514
1515 void fgAPToggleWayPoint( void ) {
1516     // Remove at a later date
1517     fgAPDataPtr APData;
1518
1519     APData = APDataGlobal;
1520     // end section
1521
1522     if ( APData->waypoint_hold ) {
1523         // turn off location hold
1524         APData->waypoint_hold = false;
1525         // set heading hold to current heading
1526         //              APData->heading_hold = true;
1527         APData->TargetHeading = fgAPget_heading();
1528     } else {
1529         double course, reverse, distance;
1530         // turn on location hold
1531         // turn on heading hold
1532         APData->old_lat = fgAPget_latitude();
1533         APData->old_lon = fgAPget_longitude();
1534
1535                         // need to test for iter
1536         if(!geo_inverse_wgs_84( fgAPget_altitude(),
1537                                 fgAPget_latitude(),
1538                                 fgAPget_longitude(),
1539                                 APData->TargetLatitude,
1540                                 APData->TargetLongitude,
1541                                 &course,
1542                                 &reverse,
1543                                 &distance ) ) {
1544             APData->TargetHeading = course;
1545             APData->TargetDistance = distance;
1546             MakeTargetDistanceStr( APData, distance );
1547         }
1548         // Force this !
1549         APData->waypoint_hold = true;
1550         APData->heading_hold = true;
1551     }
1552
1553     // This changes the AutoPilot Heading
1554     // following cast needed
1555     ApHeadingDialogInput->setValue ((float)APData->TargetHeading );
1556     MakeTargetHeadingStr( APData, APData->TargetHeading );
1557
1558     get_control_values();
1559
1560     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
1561             << APData->waypoint_hold   << " "
1562             << APData->TargetLatitude  << " "
1563             << APData->TargetLongitude << " ) "
1564             );
1565 }
1566
1567
1568 void fgAPToggleAltitude( void ) {
1569     // Remove at a later date
1570     fgAPDataPtr APData;
1571
1572     APData = APDataGlobal;
1573     // end section
1574
1575     if ( APData->altitude_hold ) {
1576         // turn off altitude hold
1577         APData->altitude_hold = false;
1578     } else {
1579         // turn on altitude hold, lock at current altitude
1580         APData->altitude_hold = true;
1581         APData->terrain_follow = false;
1582         APData->TargetAltitude = fgAPget_altitude();
1583         APData->alt_error_accum = 0.0;
1584         // alt_error_queue.erase( alt_error_queue.begin(),
1585         //                        alt_error_queue.end() );
1586         float target_alt = APData->TargetAltitude;
1587         if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
1588             target_alt *= METER_TO_FEET;
1589
1590         ApAltitudeDialogInput->setValue(target_alt);
1591         MakeTargetAltitudeStr( APData, target_alt);
1592     }
1593
1594     get_control_values();
1595     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
1596             << APData->altitude_hold << ") " << APData->TargetAltitude );
1597 }
1598
1599
1600 void fgAPToggleAutoThrottle ( void ) {
1601     // Remove at a later date
1602     fgAPDataPtr APData;
1603
1604     APData = APDataGlobal;
1605     // end section
1606
1607     if ( APData->auto_throttle ) {
1608         // turn off altitude hold
1609         APData->auto_throttle = false;
1610     } else {
1611         // turn on terrain follow, lock at current agl
1612         APData->auto_throttle = true;
1613         APData->TargetSpeed = get_speed();
1614         APData->speed_error_accum = 0.0;
1615     }
1616
1617     get_control_values();
1618     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1619             << APData->auto_throttle << ") " << APData->TargetSpeed );
1620 }
1621
1622 void fgAPToggleTerrainFollow( void ) {
1623     // Remove at a later date
1624     fgAPDataPtr APData;
1625
1626     APData = APDataGlobal;
1627     // end section
1628
1629     if ( APData->terrain_follow ) {
1630         // turn off altitude hold
1631         APData->terrain_follow = false;
1632     } else {
1633         // turn on terrain follow, lock at current agl
1634         APData->terrain_follow = true;
1635         APData->altitude_hold = false;
1636         APData->TargetAGL = fgAPget_agl();
1637         APData->alt_error_accum = 0.0;
1638     }
1639     get_control_values();
1640
1641     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
1642             << APData->terrain_follow << ") " << APData->TargetAGL );
1643 }
1644
1645 static double NormalizeDegrees( double Input ) {
1646     // normalize the input to the range (-180,180]
1647     // Input should not be greater than -360 to 360.
1648     // Current rules send the output to an undefined state.
1649     if ( Input > 180 )
1650         while(Input > 180 )
1651             Input -= 360;
1652     else if ( Input <= -180 )
1653         while ( Input <= -180 )
1654             Input += 360;
1655     return ( Input );
1656 };
1657
1658 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
1659     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
1660     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
1661
1662         // Could be
1663         // static double y = 0.0;
1664         // double dx = x2 -x1;
1665         // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
1666         // {
1667
1668     double m, b, y;          // the constants to find in y=mx+b
1669     // double m, b;
1670
1671     m = ( y2 - y1 ) / ( x2 - x1 );   // calculate the m
1672
1673     b = y1 - m * x1;       // calculate the b
1674
1675     y = m * x + b;       // the final calculation
1676
1677     // }
1678
1679     return ( y );
1680
1681 };
1682
1683
1684 /* Direct and inverse distance functions */
1685 /** Proceedings of the 7th International Symposium on Geodetic
1686   Computations, 1985
1687   "The Nested Coefficient Method for Accurate Solutions of Direct
1688   and
1689   Inverse Geodetic Problems With Any Length"
1690   Zhang Xue-Lian
1691   pp 747-763
1692   */
1693 /* modified for FlightGear to use WGS84 only  Norman Vine */
1694
1695 //#include "dstazfns.h"
1696 #include <math.h>
1697 #define GEOD_INV_PI      (3.14159265358979323846)
1698
1699 /* s == distance */
1700 /* az = azimuth */
1701
1702 /* for WGS_84 a = 6378137.000, rf = 298.257223563; */
1703
1704 static double M0( double e2 )
1705 {       //double e4 = e2*e2;
1706     return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + e2*(5.0/256.0) )))/2.0;
1707 }
1708 /* s == distance */
1709 int geo_direct_wgs_84 ( double alt, double lat1, double lon1, double az1, double s,
1710                         double *lat2, double *lon2,  double *az2 )
1711 {
1712     double a = 6378137.000, rf = 298.257223563;
1713     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
1714     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
1715     double b = a*(1.0-f), e2 = f*(2.0-f);
1716     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
1717     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
1718     double azm1 = az1*RADDEG;
1719     double sinaz1 = sin(azm1), cosaz1 = cos(azm1);
1720
1721
1722     if( fabs(s) < 0.01 )  /* distance < centimeter => congruency */
1723         {       *lat2 = lat1;
1724         *lon2 = lon1;
1725         *az2 = 180.0 + az1;
1726         if( *az2 > 360.0 ) *az2 -= 360.0;
1727         return 0;
1728         }
1729     else
1730         if( cosphi1 )    /* non-polar origin */
1731             {   /* u1 is reduced latitude */
1732                 double tanu1 = sqrt(1.0-e2)*sinphi1/cosphi1;
1733                 double sig1 = atan2(tanu1,cosaz1);
1734                 double cosu1 = 1.0/sqrt( 1.0 + tanu1*tanu1 ), sinu1 = tanu1*cosu1;
1735                 double sinaz =  cosu1*sinaz1, cos2saz = 1.0-sinaz*sinaz;
1736                 double us = cos2saz*e2/(1.0-e2);
1737                 /*      Terms */
1738                 double  ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/16384.0,
1739                     tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0,
1740                     tc = 0;
1741                 /*      FIRST ESTIMATE OF SIGMA (SIG) */
1742                 double first = s/(b*ta); /* !!*/
1743                 double sig = first;
1744                 double c2sigm, sinsig,cossig, temp,denom,rnumer, dlams, dlam;
1745                 do
1746                     {   c2sigm = cos(2.0*sig1+sig);
1747                     sinsig = sin(sig); cossig = cos(sig);
1748                     temp = sig;
1749                     sig = first +
1750                         tb*sinsig*(c2sigm+tb*(cossig*(-1.0+2.0*pow(c2sigm,2.0)) -
1751                                               tb*c2sigm*(-3.0+4.0*pow(sinsig,2.0))*(-3.0+4.0*pow(c2sigm,2.0))/6.0)/4.0);
1752                     }
1753                 while( fabs(sig-temp) > testv);
1754                 /*      LATITUDE OF POINT 2 */
1755                 /*      DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER) */
1756                 temp = sinu1*sinsig-cosu1*cossig*cosaz1;
1757                 denom = (1.0-f)*sqrt(sinaz*sinaz+temp*temp);
1758                 /* NUMERATOR */
1759                 rnumer = sinu1*cossig+cosu1*sinsig*cosaz1;
1760                 *lat2 = atan2(rnumer,denom)/RADDEG;
1761                 /* DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS ) */
1762                 rnumer = sinsig*sinaz1;
1763                 denom = cosu1*cossig-sinu1*sinsig*cosaz1;
1764                 dlams = atan2(rnumer,denom);
1765                 /* TERM C */
1766                 tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
1767                 /* DIFFERENCE IN LONGITUDE */
1768                 dlam = dlams-(1.0-tc)*f*sinaz*(sig+tc*sinsig*(c2sigm+tc*cossig*(-1.0+2.0*
1769                                                                                 pow(c2sigm,2.0))));
1770                 *lon2 = (lam1+dlam)/RADDEG;
1771                 if(*lon2 > 180.0  ) *lon2 -= 360.0;
1772                 if(*lon2 < -180.0 ) *lon2 += 360.0;
1773                 /* AZIMUTH - FROM NORTH */
1774                 *az2 = atan2(-sinaz,temp)/RADDEG;
1775                 if( fabs(*az2) < testv ) *az2 = 0.0;
1776                 if( *az2 < 0.0) *az2 += 360.0;
1777                 return 0;
1778             }
1779         else /* phi1 == 90 degrees, polar origin  */
1780             {   double dM = a*M0(e2) - s;
1781             double paz = ( phi1 < 0.0 ? 180.0 : 0.0 );
1782             return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM,lat2,lon2,az2 );
1783             }
1784 }
1785
1786 int geo_inverse_wgs_84( double alt, double lat1, double lon1, double lat2,
1787                         double lon2, double *az1, double *az2, double *s )
1788 {
1789     double a = 6378137.000, rf = 298.257223563;
1790     int iter=0;
1791     double RADDEG = (GEOD_INV_PI)/180.0, testv = 1.0E-10;
1792     double f = ( rf > 0.0 ? 1.0/rf : 0.0 );
1793     double b = a*(1.0-f), e2 = f*(2.0-f);
1794     double phi1 = lat1*RADDEG, lam1 = lon1*RADDEG;
1795     double sinphi1 = sin(phi1), cosphi1 = cos(phi1);
1796     double phi2 = lat2*RADDEG, lam2 = lon2*RADDEG;
1797     double sinphi2 = sin(phi2), cosphi2 = cos(phi2);
1798
1799     if( (fabs(lat1-lat2) < testv &&
1800          ( fabs(lon1-lon2) < testv) || fabs(lat1-90.0) < testv ) )
1801     {   /* TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */
1802         *az1 = 0.0; *az2 = 0.0; *s = 0.0;
1803         return 0;
1804     } else
1805         if(  fabs(cosphi1) < testv ) /* initial point is polar */
1806         {
1807             int k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s );
1808             b = *az1; *az1 = *az2; *az2 = b;
1809             return 0;
1810         } else
1811             if( fabs(cosphi2) < testv ) /* terminal point is polar */
1812             {
1813                 int k = geo_inverse_wgs_84( alt, lat1,lon1,lat1,lon1+180.0,
1814                                             az1,az2,s );
1815                 *s /= 2.0;
1816                 *az2 = *az1 + 180.0;
1817                 if( *az2 > 360.0 ) *az2 -= 360.0;
1818                 return 0;
1819             } else      /* Geodesic passes through the pole (antipodal) */
1820                 if( (fabs( fabs(lon1-lon2) - 180 ) < testv) &&
1821                     (fabs(lat1+lat2) < testv) )
1822                 {
1823                     double s1,s2;
1824                     geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 );
1825                     geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 );
1826                     *az2 = *az1;
1827                     *s = s1 + s2;
1828                     return 0;
1829                 } else  /* antipodal and polar points don't get here */
1830                 {
1831                     double dlam = lam2 - lam1, dlams = dlam;
1832                     double sdlams,cdlams, sig,sinsig,cossig, sinaz,
1833                         cos2saz, c2sigm;
1834                     double tc,temp, us,rnumer,denom, ta,tb;
1835                     double cosu1,sinu1, sinu2,cosu2;
1836                     /* Reduced latitudes */
1837                     temp = (1.0-f)*sinphi1/cosphi1;
1838                     cosu1 = 1.0/sqrt(1.0+temp*temp);
1839                     sinu1 = temp*cosu1;
1840                     temp = (1.0-f)*sinphi2/cosphi2;
1841                     cosu2 = 1.0/sqrt(1.0+temp*temp);
1842                     sinu2 = temp*cosu2;
1843
1844                     do {
1845                         sdlams = sin(dlams), cdlams = cos(dlams);
1846                         sinsig = sqrt(pow(cosu2*sdlams,2.0)+
1847                                       pow(cosu1*sinu2-sinu1*cosu2*cdlams,2.0));
1848                         cossig = sinu1*sinu2+cosu1*cosu2*cdlams;
1849
1850                         sig = atan2(sinsig,cossig);
1851                         sinaz = cosu1*cosu2*sdlams/sinsig;
1852                         cos2saz = 1.0-sinaz*sinaz;
1853                         c2sigm = (sinu1 == 0.0 || sinu2 == 0.0 ? cossig :
1854                                   cossig-2.0*sinu1*sinu2/cos2saz);
1855                         tc = f*cos2saz*(4.0+f*(4.0-3.0*cos2saz))/16.0;
1856                         temp = dlams;
1857                         dlams = dlam+(1.0-tc)*f*sinaz*
1858                             (sig+tc*sinsig*
1859                              (c2sigm+tc*cossig*(-1.0+2.0*pow(c2sigm,2.0))));
1860                         if (fabs(dlams) > GEOD_INV_PI && iter++ > 50)
1861                             return iter;
1862                     } while ( fabs(temp-dlams) > testv);
1863
1864                     us = cos2saz*(pow(a,2.0)-pow(b,2.0))/pow(b,2.0);    /* !! */
1865                     /* BACK AZIMUTH FROM NORTH */
1866                     rnumer = -(cosu1*sdlams);
1867                     denom = sinu1*cosu2-cosu1*sinu2*cdlams;
1868                     *az2 = atan2(rnumer,denom)/RADDEG;
1869                     if( fabs(*az2) < testv ) *az2 = 0.0;
1870                     if(*az2 < 0.0) *az2 += 360.0;
1871                     /* FORWARD AZIMUTH FROM NORTH */
1872                     rnumer = cosu2*sdlams;
1873                     denom = cosu1*sinu2-sinu1*cosu2*cdlams;
1874                     *az1 = atan2(rnumer,denom)/RADDEG;
1875                     if( fabs(*az1) < testv ) *az1 = 0.0;
1876                     if(*az1 < 0.0) *az1 += 360.0;
1877                     /* Terms a & b */
1878                     ta = 1.0+us*(4096.0+us*(-768.0+us*(320.0-175.0*us)))/
1879                         16384.0;
1880                     tb = us*(256.0+us*(-128.0+us*(74.0-47.0*us)))/1024.0;
1881                     /* GEODETIC DISTANCE */
1882                     *s = b*ta*(sig-tb*sinsig*
1883                                (c2sigm+tb*(cossig*(-1.0+2.0*pow(c2sigm,2.0))-tb*
1884                                            c2sigm*(-3.0+4.0*pow(sinsig,2.0))*
1885                                            (-3.0+4.0*pow(c2sigm,2.0))/6.0)/
1886                                 4.0));
1887                     return 0;
1888                 }
1889 }