src/Autopilot/newauto.cxx

   1 // newauto.cxx -- autopilot defines and prototypes (very alpha)
   2 //
   3 // Started April 1998  Copyright (C) 1998
   4 //
   5 // Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
   6 //                  Norman Vine <nhv@cape.com>
   7 //                  Curtis Olson <curt@flightgear.org>
   8 //
   9 // This program is free software; you can redistribute it and/or
  10 // modify it under the terms of the GNU General Public License as
  11 // published by the Free Software Foundation; either version 2 of the
  12 // License, or (at your option) any later version.
  13 //
  14 // This program is distributed in the hope that it will be useful, but
  15 // WITHOUT ANY WARRANTY; without even the implied warranty of
  16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 // General Public License for more details.
  18 //
  19 // You should have received a copy of the GNU General Public License
  20 // along with this program; if not, write to the Free Software
  21 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  22 //
  23 // $Id$
  24
  25
  26 #ifdef HAVE_CONFIG_H
  27 #  include <config.h>
  28 #endif
  29
  30 #include <stdio.h>              // sprintf()
  31
  32 #include <simgear/constants.h>
  33 #include <simgear/sg_inlines.h>
  34 #include <simgear/debug/logstream.hxx>
  35 #include <simgear/math/sg_geodesy.hxx>
  36 #include <simgear/math/sg_random.h>
  37
  38 #include <Cockpit/steam.hxx>
  39 #include <Cockpit/radiostack.hxx>
  40 #include <Controls/controls.hxx>
  41 #include <FDM/flight.hxx>
  42 #include <Main/globals.hxx>
  43 #include <Scenery/scenery.hxx>
  44
  45 #include "newauto.hxx"
  46
  47
  48 FGAutopilot *current_autopilot;
  49
  50
  51 // Climb speed constants
  52 // const double min_climb = 70.0;       // kts
  53 // const double best_climb = 75.0;      // kts
  54 // const double ideal_climb_rate = 500.0 * SG_FEET_TO_METER; // fpm -> mpm
  55 // const double ideal_decent_rate = 1000.0 * SG_FEET_TO_METER; // fpm -> mpm
  56
  57 /// These statics will eventually go into the class
  58 /// they are just here while I am experimenting -- NHV :-)
  59 // AutoPilot Gain Adjuster members
  60 static double MaxRollAdjust;        // MaxRollAdjust       = 2 * APData->MaxRoll;
  61 static double RollOutAdjust;        // RollOutAdjust       = 2 * APData->RollOut;
  62 static double MaxAileronAdjust;     // MaxAileronAdjust    = 2 * APData->MaxAileron;
  63 static double RollOutSmoothAdjust;  // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
  64
  65 static char NewTgtAirportId[16];
  66 // static char NewTgtAirportLabel[] = "Enter New TgtAirport ID";
  67
  68 extern char *coord_format_lat(float);
  69 extern char *coord_format_lon(float);
  70
  71
  72 // constructor
  73 FGAutopilot::FGAutopilot()
  74 {
  75     TargetClimbRate
  76         = fgGetNode("/autopilot/config/target-climb-rate-fpm", true);
  77     TargetDescentRate
  78         = fgGetNode("/autopilot/config/target-descent-rate-fpm", true);
  79     min_climb = fgGetNode("/autopilot/config/min-climb-speed-kt", true);
  80     best_climb = fgGetNode("/autopilot/config/best-climb-speed-kt", true);
  81     elevator_adj_factor
  82         = fgGetNode("/autopilot/config/elevator-adj-factor", true);
  83     integral_contrib
  84         = fgGetNode("/autopilot/config/integral-contribution", true);
  85     zero_pitch_throttle
  86         = fgGetNode("/autopilot/config/zero-pitch-throttle", true);
  87     zero_pitch_trim_full_throttle
  88         = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true);
  89     current_throttle = fgGetNode("/controls/throttle");
  90     // initialize with defaults (in case config isn't there)
  91     if ( TargetClimbRate->getFloatValue() < 1 )
  92         fgSetFloat( "/autopilot/config/target-climb-rate-fpm", 500);
  93     if ( TargetDescentRate->getFloatValue() < 1 )
  94         fgSetFloat( "/autopilot/config/target-descent-rate-fpm", 1000 );
  95     if ( min_climb->getFloatValue() < 1)
  96         fgSetFloat( "/autopilot/config/min-climb-speed-kt", 70 );
  97     if (best_climb->getFloatValue() < 1)
  98         fgSetFloat( "/autopilot/config/best-climb-speed-kt", 120 );
  99     if (elevator_adj_factor->getFloatValue() < 1)
 100         fgSetFloat( "/autopilot/config/elevator-adj-factor", 5000 );
 101     if ( integral_contrib->getFloatValue() < 0.0000001 )
 102         fgSetFloat( "/autopilot/config/integral-contribution", 0.01 );
 103     if ( zero_pitch_throttle->getFloatValue() < 0.0000001 )
 104         fgSetFloat( "/autopilot/config/zero-pitch-throttle", 0.60 );
 105     if ( zero_pitch_trim_full_throttle->getFloatValue() < 0.0000001 )
 106         fgSetFloat( "/autopilot/config/zero-pitch-trim-full-throttle", 0.15 );
 107 }
 108
 109 // destructor
 110 FGAutopilot::~FGAutopilot() {}
 111
 112
 113 void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
 114     sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
 115     sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
 116     sprintf( TargetLatLonStr, "%s  %s", TargetLatitudeStr, TargetLongitudeStr );
 117 }
 118
 119
 120 void FGAutopilot::MakeTargetAltitudeStr( double altitude ) {
 121     if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 122         sprintf( TargetAltitudeStr, "APAltitude  %6.0f+", altitude );
 123     } else {
 124         sprintf( TargetAltitudeStr, "APAltitude  %6.0f", altitude );
 125     }
 126 }
 127
 128
 129 void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
 130     if ( bearing < 0. ) {
 131         bearing += 360.;
 132     } else if (bearing > 360. ) {
 133         bearing -= 360.;
 134     }
 135     sprintf( TargetHeadingStr, "APHeading  %6.1f", bearing );
 136 }
 137
 138
 139 static inline double get_speed( void ) {
 140     return( cur_fdm_state->get_V_equiv_kts() );
 141 }
 142
 143 static inline double get_ground_speed() {
 144     // starts in ft/s so we convert to kts
 145     static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
 146
 147     double ft_s = cur_fdm_state->get_V_ground_speed()
 148         * speedup_node->getIntValue();
 149     double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM;
 150
 151     return kts;
 152 }
 153
 154
 155 void FGAutopilot::MakeTargetWPStr( double distance ) {
 156     static time_t last_time = 0;
 157     time_t current_time = time(NULL);
 158     if ( last_time == current_time ) {
 159         return;
 160     }
 161
 162     last_time = current_time;
 163
 164     double accum = 0.0;
 165
 166     int size = globals->get_route()->size();
 167
 168     // start by wiping the strings
 169     TargetWP1Str[0] = 0;
 170     TargetWP2Str[0] = 0;
 171     TargetWP3Str[0] = 0;
 172
 173     // current route
 174     if ( size > 0 ) {
 175         SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 );
 176         accum += distance;
 177         double eta = accum * SG_METER_TO_NM / get_ground_speed();
 178         if ( eta >= 100.0 ) { eta = 99.999; }
 179         int major, minor;
 180         if ( eta < (1.0/6.0) ) {
 181             // within 10 minutes, bump up to min/secs
 182             eta *= 60.0;
 183         }
 184         major = (int)eta;
 185         minor = (int)((eta - (int)eta) * 60.0);
 186         sprintf( TargetWP1Str, "%s %.2f NM  ETA %d:%02d",
 187                  wp1.get_id().c_str(),
 188                  accum*SG_METER_TO_NM, major, minor );
 189         // cout << "distance = " << distance*SG_METER_TO_NM
 190         //      << "  gndsp = " << get_ground_speed()
 191         //      << "  time = " << eta
 192         //      << "  major = " << major
 193         //      << "  minor = " << minor
 194         //      << endl;
 195     }
 196
 197     // next route
 198     if ( size > 1 ) {
 199         SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 );
 200         accum += wp2.get_distance();
 201
 202         double eta = accum * SG_METER_TO_NM / get_ground_speed();
 203         if ( eta >= 100.0 ) { eta = 99.999; }
 204         int major, minor;
 205         if ( eta < (1.0/6.0) ) {
 206             // within 10 minutes, bump up to min/secs
 207             eta *= 60.0;
 208         }
 209         major = (int)eta;
 210         minor = (int)((eta - (int)eta) * 60.0);
 211         sprintf( TargetWP2Str, "%s %.2f NM  ETA %d:%02d",
 212                  wp2.get_id().c_str(),
 213                  accum*SG_METER_TO_NM, major, minor );
 214     }
 215
 216     // next route
 217     if ( size > 2 ) {
 218         for ( int i = 2; i < size; ++i ) {
 219             accum += globals->get_route()->get_waypoint( i ).get_distance();
 220         }
 221
 222         SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 );
 223
 224         double eta = accum * SG_METER_TO_NM / get_ground_speed();
 225         if ( eta >= 100.0 ) { eta = 99.999; }
 226         int major, minor;
 227         if ( eta < (1.0/6.0) ) {
 228             // within 10 minutes, bump up to min/secs
 229             eta *= 60.0;
 230         }
 231         major = (int)eta;
 232         minor = (int)((eta - (int)eta) * 60.0);
 233         sprintf( TargetWP3Str, "%s %.2f NM  ETA %d:%02d",
 234                  wpn.get_id().c_str(),
 235                  accum*SG_METER_TO_NM, major, minor );
 236     }
 237 }
 238
 239
 240 void FGAutopilot::update_old_control_values() {
 241     old_aileron = globals->get_controls()->get_aileron();
 242     old_elevator = globals->get_controls()->get_elevator();
 243     old_elevator_trim = globals->get_controls()->get_elevator_trim();
 244     old_rudder = globals->get_controls()->get_rudder();
 245 }
 246
 247
 248 // Initialize autopilot subsystem
 249 void FGAutopilot::init() {
 250     SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" );
 251
 252     latitude_node = fgGetNode("/position/latitude-deg", true);
 253     longitude_node = fgGetNode("/position/longitude-deg", true);
 254     altitude_node = fgGetNode("/position/altitude-ft", true);
 255     altitude_agl_node = fgGetNode("/position/altitude-agl-ft", true);
 256     vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true);
 257     heading_node = fgGetNode("/orientation/heading-deg", true);
 258     roll_node = fgGetNode("/orientation/roll-deg", true);
 259
 260     heading_hold = false ;      // turn the heading hold off
 261     altitude_hold = false ;     // turn the altitude hold off
 262     auto_throttle = false ;     // turn the auto throttle off
 263     heading_mode = DEFAULT_AP_HEADING_LOCK;
 264
 265     sg_srandom_time();
 266     DGTargetHeading = sg_random() * 360.0;
 267
 268     // Initialize target location to startup location
 269     old_lat = latitude_node->getDoubleValue();
 270     old_lon = longitude_node->getDoubleValue();
 271     // set_WayPoint( old_lon, old_lat, 0.0, "default" );
 272
 273     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 274
 275     TargetHeading = 0.0;        // default direction, due north
 276     TargetAltitude = 3000;      // default altitude in meters
 277     alt_error_accum = 0.0;
 278     climb_error_accum = 0.0;
 279
 280     MakeTargetAltitudeStr( TargetAltitude );
 281     MakeTargetHeadingStr( TargetHeading );
 282
 283     // These eventually need to be read from current_aircaft somehow.
 284
 285     // the maximum roll, in Deg
 286     MaxRoll = 20;
 287
 288     // the deg from heading to start rolling out at, in Deg
 289     RollOut = 20;
 290
 291     // how far can I move the aleron from center.
 292     MaxAileron = .2;
 293
 294     // Smoothing distance for alerion control
 295     RollOutSmooth = 10;
 296
 297     // Hardwired for now should be in options
 298     // 25% max control variablilty  0.5 / 2.0
 299     disengage_threshold = 1.0;
 300
 301 #if !defined( USING_SLIDER_CLASS )
 302     MaxRollAdjust = 2 * MaxRoll;
 303     RollOutAdjust = 2 * RollOut;
 304     MaxAileronAdjust = 2 * MaxAileron;
 305     RollOutSmoothAdjust = 2 * RollOutSmooth;
 306 #endif  // !defined( USING_SLIDER_CLASS )
 307
 308     update_old_control_values();
 309
 310 };
 311
 312
 313 // Reset the autopilot system
 314 void FGAutopilot::reset() {
 315
 316     heading_hold = false ;      // turn the heading hold off
 317     altitude_hold = false ;     // turn the altitude hold off
 318     auto_throttle = false ;     // turn the auto throttle off
 319     heading_mode = DEFAULT_AP_HEADING_LOCK;
 320
 321     // TargetHeading = 0.0;     // default direction, due north
 322     MakeTargetHeadingStr( TargetHeading );
 323
 324     // TargetAltitude = 3000;   // default altitude in meters
 325     MakeTargetAltitudeStr( TargetAltitude );
 326
 327     alt_error_accum = 0.0;
 328     climb_error_accum = 0.0;
 329
 330     update_old_control_values();
 331
 332     sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id").c_str() );
 333
 334     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 335 }
 336
 337
 338 static double NormalizeDegrees( double Input ) {
 339     // normalize the input to the range (-180,180]
 340     // Input should not be greater than -360 to 360.
 341     // Current rules send the output to an undefined state.
 342     while ( Input > 180.0 ) { Input -= 360.0; }
 343     while ( Input <= -180.0 ) { Input += 360.0; }
 344
 345     return Input;
 346 };
 347
 348 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
 349     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 350     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 351
 352         // Could be
 353         // static double y = 0.0;
 354         // double dx = x2 -x1;
 355         // if( (dx < -SG_EPSILON ) || ( dx > SG_EPSILON ) )
 356         // {
 357
 358     double m, b, y;          // the constants to find in y=mx+b
 359     // double m, b;
 360
 361     m = ( y2 - y1 ) / ( x2 - x1 );   // calculate the m
 362
 363     b = y1 - m * x1;       // calculate the b
 364
 365     y = m * x + b;       // the final calculation
 366
 367     // }
 368
 369     return ( y );
 370
 371 };
 372
 373
 374 int FGAutopilot::run() {
 375     // Remove the following lines when the calling funcitons start
 376     // passing in the data pointer
 377
 378     // get control settings
 379
 380     double lat = latitude_node->getDoubleValue();
 381     double lon = longitude_node->getDoubleValue();
 382     double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER;
 383
 384     SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run()  lat = " << lat <<
 385             "  lon = " << lon << "  alt = " << alt );
 386
 387 #ifdef FG_FORCE_AUTO_DISENGAGE
 388     // see if somebody else has changed them
 389     if( fabs(aileron - old_aileron) > disengage_threshold ||
 390         fabs(elevator - old_elevator) > disengage_threshold ||
 391         fabs(elevator_trim - old_elevator_trim) >
 392         disengage_threshold ||
 393         fabs(rudder - old_rudder) > disengage_threshold )
 394     {
 395         // if controls changed externally turn autopilot off
 396         waypoint_hold = false ;   // turn the target hold off
 397         heading_hold = false ;    // turn the heading hold off
 398         altitude_hold = false ;   // turn the altitude hold off
 399         terrain_follow = false;   // turn the terrain_follow hold off
 400         // auto_throttle = false; // turn the auto_throttle off
 401
 402         // stash this runs control settings
 403         old_aileron = aileron;
 404         old_elevator = elevator;
 405         old_elevator_trim = elevator_trim;
 406         old_rudder = rudder;
 407
 408         return 0;
 409     }
 410 #endif
 411
 412     // heading hold
 413     if ( heading_hold == true ) {
 414         if ( heading_mode == FG_DG_HEADING_LOCK ) {
 415             // cout << "DG heading = " << FGSteam::get_DG_deg()
 416             //      << " DG error = " << FGSteam::get_DG_err() << endl;
 417
 418             TargetHeading = DGTargetHeading + FGSteam::get_DG_err();
 419             while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
 420             while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
 421             MakeTargetHeadingStr( TargetHeading );
 422         } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
 423             // we don't set a specific target heading in
 424             // TC_HEADING_LOCK mode, we instead try to keep the turn
 425             // coordinator zero'd
 426         } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
 427             // leave "true" target heading as is
 428             while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
 429             while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
 430             MakeTargetHeadingStr( TargetHeading );
 431         } else if ( heading_mode == FG_HEADING_NAV1 ) {
 432             // track the NAV1 heading needle deflection
 433
 434             // determine our current radial position relative to the
 435             // navaid in "true" heading.
 436             double cur_radial = current_radiostack->get_nav1_heading();
 437             if ( current_radiostack->get_nav1_loc() ) {
 438                 // ILS localizers radials are already "true" in our
 439                 // database
 440             } else {
 441                 cur_radial += current_radiostack->get_nav1_magvar();
 442             }
 443             if ( current_radiostack->get_nav1_from_flag() ) {
 444                 cur_radial += 180.0;
 445                 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
 446             }
 447
 448             // determine the target radial in "true" heading
 449             double tgt_radial = current_radiostack->get_nav1_radial();
 450             if ( current_radiostack->get_nav1_loc() ) {
 451                 // ILS localizers radials are already "true" in our
 452                 // database
 453             } else {
 454                 // VOR radials need to have that vor's offset added in
 455                 tgt_radial += current_radiostack->get_nav1_magvar();
 456             }
 457
 458             // determine the heading adjustment needed.
 459             double adjustment =
 460                 current_radiostack->get_nav1_heading_needle_deflection()
 461                 * (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM);
 462             SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
 463
 464             // determine the target heading to fly to intercept the
 465             // tgt_radial
 466             TargetHeading = tgt_radial + adjustment;
 467             while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
 468             while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
 469
 470             MakeTargetHeadingStr( TargetHeading );
 471             // cout << "target course (true) = " << TargetHeading << endl;
 472         } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 473             // update target heading to waypoint
 474
 475             double wp_course, wp_distance;
 476
 477 #ifdef DO_fgAP_CORRECTED_COURSE
 478             // compute course made good
 479             // this needs lots of special casing before use
 480             double course, reverse, distance, corrected_course;
 481             // need to test for iter
 482             geo_inverse_wgs_84( 0, //fgAPget_altitude(),
 483                                 old_lat,
 484                                 old_lon,
 485                                 lat,
 486                                 lon,
 487                                 &course,
 488                                 &reverse,
 489                                 &distance );
 490 #endif // DO_fgAP_CORRECTED_COURSE
 491
 492             // compute course to way_point
 493             // need to test for iter
 494             SGWayPoint wp = globals->get_route()->get_first();
 495             wp.CourseAndDistance( lon, lat, alt,
 496                                   &wp_course, &wp_distance );
 497
 498 #ifdef DO_fgAP_CORRECTED_COURSE
 499             corrected_course = course - wp_course;
 500             if( fabs(corrected_course) > 0.1 )
 501                 printf("fgAP: course %f  wp_course %f  %f  %f\n",
 502                        course, wp_course, fabs(corrected_course),
 503                        distance );
 504 #endif // DO_fgAP_CORRECTED_COURSE
 505
 506             if ( wp_distance > 100 ) {
 507                 // corrected_course = course - wp_course;
 508                 TargetHeading = NormalizeDegrees(wp_course);
 509             } else {
 510                 cout << "Reached waypoint within " << wp_distance << "meters"
 511                      << endl;
 512
 513                 // pop off this waypoint from the list
 514                 if ( globals->get_route()->size() ) {
 515                     globals->get_route()->delete_first();
 516                 }
 517
 518                 // see if there are more waypoints on the list
 519                 if ( globals->get_route()->size() ) {
 520                     // more waypoints
 521                     set_HeadingMode( FG_HEADING_WAYPOINT );
 522                 } else {
 523                     // end of the line
 524                     heading_mode = FG_TRUE_HEADING_LOCK;
 525                     // use current heading
 526                     TargetHeading = heading_node->getDoubleValue();
 527                 }
 528             }
 529             MakeTargetHeadingStr( TargetHeading );
 530             // Force this just in case
 531             TargetDistance = wp_distance;
 532             MakeTargetWPStr( wp_distance );
 533         }
 534
 535         if ( heading_mode == FG_TC_HEADING_LOCK ) {
 536             // drive the turn coordinator to zero
 537             double turn = FGSteam::get_TC_std();
 538             // cout << "turn rate = " << turn << endl;
 539             double AileronSet = -turn / 2.0;
 540             SG_CLAMP_RANGE( AileronSet, -1.0, 1.0 );
 541             globals->get_controls()->set_aileron( AileronSet );
 542             globals->get_controls()->set_rudder( AileronSet / 4.0 );
 543         } else {
 544             // steer towards the target heading
 545
 546             double RelHeading;
 547             double TargetRoll;
 548             double RelRoll;
 549             double AileronSet;
 550
 551             RelHeading
 552                 = NormalizeDegrees( TargetHeading
 553                                     - heading_node->getDoubleValue() );
 554             // figure out how far off we are from desired heading
 555
 556             // Now it is time to deterime how far we should be rolled.
 557             SG_LOG( SG_AUTOPILOT, SG_DEBUG, "RelHeading: " << RelHeading );
 558
 559
 560             // Check if we are further from heading than the roll out point
 561             if ( fabs( RelHeading ) > RollOut ) {
 562                 // set Target Roll to Max in desired direction
 563                 if ( RelHeading < 0 ) {
 564                     TargetRoll = 0 - MaxRoll;
 565                 } else {
 566                     TargetRoll = MaxRoll;
 567                 }
 568             } else {
 569                 // We have to calculate the Target roll
 570
 571                 // This calculation engine thinks that the Target roll
 572                 // should be a line from (RollOut,MaxRoll) to (-RollOut,
 573                 // -MaxRoll) I hope this works well.  If I get ambitious
 574                 // some day this might become a fancier curve or
 575                 // something.
 576
 577                 TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
 578                                                 -MaxRoll, RollOut,
 579                                                 MaxRoll );
 580             }
 581
 582             // Target Roll has now been Found.
 583
 584             // Compare Target roll to Current Roll, Generate Rel Roll
 585
 586             SG_LOG( SG_COCKPIT, SG_BULK, "TargetRoll: " << TargetRoll );
 587
 588             RelRoll = NormalizeDegrees( TargetRoll
 589                                         - roll_node->getDoubleValue() );
 590
 591             // Check if we are further from heading than the roll out
 592             // smooth point
 593             if ( fabs( RelRoll ) > RollOutSmooth ) {
 594                 // set Target Roll to Max in desired direction
 595                 if ( RelRoll < 0 ) {
 596                 AileronSet = 0 - MaxAileron;
 597                 } else {
 598                     AileronSet = MaxAileron;
 599                 }
 600             } else {
 601                 AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
 602                                                 -MaxAileron,
 603                                                 RollOutSmooth,
 604                                                 MaxAileron );
 605             }
 606
 607             globals->get_controls()->set_aileron( AileronSet );
 608             globals->get_controls()->set_rudder( AileronSet / 4.0 );
 609             // controls.set_rudder( 0.0 );
 610         }
 611     }
 612
 613     // altitude hold
 614     if ( altitude_hold ) {
 615         double climb_rate;
 616         double speed, max_climb, error;
 617         double prop_error, int_error;
 618         double prop_adj, int_adj, total_adj;
 619
 620         if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 621             climb_rate =
 622                 ( TargetAltitude - FGSteam::get_ALT_ft() * SG_FEET_TO_METER ) * 8.0;
 623         } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
 624             double x = current_radiostack->get_nav1_gs_dist();
 625             double y = (altitude_node->getDoubleValue()
 626                         - current_radiostack->get_nav1_elev()) * SG_FEET_TO_METER;
 627             double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 628             // cout << "current angle = " << current_angle << endl;
 629
 630             double target_angle = current_radiostack->get_nav1_target_gs();
 631             // cout << "target angle = " << target_angle << endl;
 632
 633             double gs_diff = target_angle - current_angle;
 634             // cout << "difference from desired = " << gs_diff << endl;
 635
 636             // convert desired vertical path angle into a climb rate
 637             double des_angle = current_angle - 10 * gs_diff;
 638             // cout << "desired angle = " << des_angle << endl;
 639
 640             // convert to meter/min
 641             // cout << "raw ground speed = " << cur_fdm_state->get_V_ground_speed() << endl;
 642             double horiz_vel = cur_fdm_state->get_V_ground_speed()
 643                 * SG_FEET_TO_METER * 60.0;
 644             // cout << "Horizontal vel = " << horiz_vel << endl;
 645             climb_rate = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) * horiz_vel;
 646             // cout << "climb_rate = " << climb_rate << endl;
 647             /* climb_error_accum += gs_diff * 2.0; */
 648             /* climb_rate = gs_diff * 200.0 + climb_error_accum; */
 649         } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 650             // brain dead ground hugging with no look ahead
 651             climb_rate =
 652                 ( TargetAGL - altitude_agl_node->getDoubleValue()
 653                   * SG_FEET_TO_METER ) * 16.0;
 654             // cout << "target agl = " << TargetAGL
 655             //      << "  current agl = " << fgAPget_agl()
 656             //      << "  target climb rate = " << climb_rate
 657             //      << endl;
 658         } else {
 659             // just try to zero out rate of climb ...
 660             climb_rate = 0.0;
 661         }
 662
 663         speed = get_speed();
 664
 665         if ( speed < min_climb->getFloatValue() ) {
 666             max_climb = 0.0;
 667         } else if ( speed < best_climb->getFloatValue() ) {
 668             max_climb = ((best_climb->getFloatValue()
 669                           - min_climb->getFloatValue())
 670                          - (best_climb->getFloatValue() - speed))
 671                 * fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER)
 672                 / (best_climb->getFloatValue() - min_climb->getFloatValue());
 673         } else {
 674             max_climb = ( speed - best_climb->getFloatValue() ) * 10.0
 675                 + fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
 676         }
 677
 678         // this first one could be optional if we wanted to allow
 679         // better climb performance assuming we have the airspeed to
 680         // support it.
 681         if ( climb_rate >
 682              fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) ) {
 683             climb_rate
 684                 = fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
 685         }
 686
 687         if ( climb_rate > max_climb ) {
 688             climb_rate = max_climb;
 689         }
 690
 691         if ( climb_rate <
 692              -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER) ) {
 693             climb_rate
 694                 = -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER);
 695         }
 696
 697         // cout << "Target climb rate = " << TargetClimbRate->getFloatValue() << endl;
 698         // cout << "given our speed, modified desired climb rate = "
 699         //      << climb_rate * SG_METER_TO_FEET
 700         //      << " fpm" << endl;
 701         // cout << "Current climb rate = "
 702         //      << vertical_speed_node->getDoubleValue() * 60 << " fpm" << endl;
 703
 704         error = vertical_speed_node->getDoubleValue() * 60
 705             - climb_rate * SG_METER_TO_FEET;
 706
 707         // accumulate the error under the curve ... this really should
 708         // be *= delta t
 709         alt_error_accum += error;
 710
 711         // calculate integral error, and adjustment amount
 712         int_error = alt_error_accum;
 713         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 714         int_adj = int_error / elevator_adj_factor->getFloatValue();
 715
 716         // caclulate proportional error
 717         prop_error = error;
 718         prop_adj = prop_error / elevator_adj_factor->getDoubleValue();
 719
 720         // cout << "Error=" << error << endl;
 721         // cout << "integral_error=" << int_error << endl;
 722         // cout << "integral_contrib=" << integral_contrib->getFloatValue() << endl;
 723         // cout << "Proportional Adj=" << prop_adj << endl;
 724         // cout << "Integral Adj" << int_adj << endl;
 725         total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj
 726             + (double) integral_contrib->getFloatValue() * int_adj;
 727
 728         // stop on autopilot trim at 30% +/-
 729         if ( total_adj > 0.3 ) {
 730              total_adj = 0.3;
 731          } else if ( total_adj < -0.3 ) {
 732              total_adj = -0.3;
 733          }
 734
 735         // adjust for throttle pitch gain
 736         total_adj += ((current_throttle->getFloatValue() - zero_pitch_throttle->getFloatValue())
 737                      / (1 - zero_pitch_throttle->getFloatValue()))
 738                      * zero_pitch_trim_full_throttle->getFloatValue();
 739
 740         // cout << "Total Adj" << total_adj << endl;
 741
 742         globals->get_controls()->set_elevator_trim( total_adj );
 743     }
 744
 745     // auto throttle
 746     if ( auto_throttle ) {
 747         double error;
 748         double prop_error, int_error;
 749         double prop_adj, int_adj, total_adj;
 750
 751         error = TargetSpeed - get_speed();
 752
 753         // accumulate the error under the curve ... this really should
 754         // be *= delta t
 755         speed_error_accum += error;
 756         if ( speed_error_accum > 2000.0 ) {
 757             speed_error_accum = 2000.0;
 758         }
 759         else if ( speed_error_accum < -2000.0 ) {
 760             speed_error_accum = -2000.0;
 761         }
 762
 763         // calculate integral error, and adjustment amount
 764         int_error = speed_error_accum;
 765
 766         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 767         int_adj = int_error / 200.0;
 768
 769         // caclulate proportional error
 770         prop_error = error;
 771         prop_adj = 0.5 + prop_error / 50.0;
 772
 773         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 774         if ( total_adj > 1.0 ) {
 775             total_adj = 1.0;
 776         }
 777         else if ( total_adj < 0.0 ) {
 778             total_adj = 0.0;
 779         }
 780
 781         globals->get_controls()->set_throttle( FGControls::ALL_ENGINES,
 782                                                total_adj );
 783     }
 784
 785 #ifdef THIS_CODE_IS_NOT_USED
 786     if (Mode == 2) // Glide slope hold
 787         {
 788             double RelSlope;
 789             double RelElevator;
 790
 791             // First, calculate Relative slope and normalize it
 792             RelSlope = NormalizeDegrees( TargetSlope - get_pitch());
 793
 794             // Now calculate the elevator offset from current angle
 795             if ( abs(RelSlope) > SlopeSmooth )
 796                 {
 797                     if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
 798                         RelElevator = -MaxElevator;
 799                     else
 800                         RelElevator = MaxElevator;
 801                 }
 802
 803             else
 804                 RelElevator = LinearExtrapolate(RelSlope,-SlopeSmooth,-MaxElevator,SlopeSmooth,MaxElevator);
 805
 806             // set the elevator
 807             fgElevMove(RelElevator);
 808
 809         }
 810 #endif // THIS_CODE_IS_NOT_USED
 811
 812     // stash this runs control settings
 813     //  update_old_control_values();
 814     old_aileron = globals->get_controls()->get_aileron();
 815     old_elevator = globals->get_controls()->get_elevator();
 816     old_elevator_trim = globals->get_controls()->get_elevator_trim();
 817     old_rudder = globals->get_controls()->get_rudder();
 818
 819     // for cross track error
 820     old_lat = lat;
 821     old_lon = lon;
 822
 823     // Ok, we are done
 824     SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run( returns )" );
 825
 826     return 0;
 827 }
 828
 829
 830 void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 831     heading_mode = mode;
 832
 833     if ( heading_mode == FG_DG_HEADING_LOCK ) {
 834         // set heading hold to current heading (as read from DG)
 835         // ... no, leave target heading along ... just use the current
 836         // heading bug value
 837         //  DGTargetHeading = FGSteam::get_DG_deg();
 838     } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
 839         // set autopilot to hold a zero turn (as reported by the TC)
 840     } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
 841         // set heading hold to current heading
 842         TargetHeading = heading_node->getDoubleValue();
 843     } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 844         if ( globals->get_route()->size() ) {
 845             double course, distance;
 846
 847             old_lat = latitude_node->getDoubleValue();
 848             old_lon = longitude_node->getDoubleValue();
 849
 850             waypoint = globals->get_route()->get_first();
 851             waypoint.CourseAndDistance( longitude_node->getDoubleValue(),
 852                                         latitude_node->getDoubleValue(),
 853                                         altitude_node->getDoubleValue()
 854                                         * SG_FEET_TO_METER,
 855                                         &course, &distance );
 856             TargetHeading = course;
 857             TargetDistance = distance;
 858             MakeTargetLatLonStr( waypoint.get_target_lat(),
 859                                  waypoint.get_target_lon() );
 860             MakeTargetWPStr( distance );
 861
 862             if ( waypoint.get_target_alt() > 0.0 ) {
 863                 TargetAltitude = waypoint.get_target_alt();
 864                 altitude_mode = FG_ALTITUDE_LOCK;
 865                 set_AltitudeEnabled( true );
 866                 MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 867             }
 868
 869             SG_LOG( SG_COCKPIT, SG_INFO, " set_HeadingMode: ( "
 870                     << get_TargetLatitude()  << " "
 871                     << get_TargetLongitude() << " ) "
 872                     );
 873         } else {
 874             // no more way points, default to heading lock.
 875             heading_mode = FG_TC_HEADING_LOCK;
 876         }
 877     }
 878
 879     MakeTargetHeadingStr( TargetHeading );
 880     update_old_control_values();
 881 }
 882
 883
 884 void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
 885     altitude_mode = mode;
 886
 887     alt_error_accum = 0.0;
 888
 889
 890     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 891         if ( TargetAltitude < altitude_agl_node->getDoubleValue()
 892              * SG_FEET_TO_METER ) {
 893         }
 894
 895         if ( fgGetString("/sim/startup/units") == "feet" ) {
 896             MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 897         } else {
 898             MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
 899         }
 900     } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
 901         climb_error_accum = 0.0;
 902
 903     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 904         TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER;
 905
 906         if ( fgGetString("/sim/startup/units") == "feet" ) {
 907             MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
 908         } else {
 909             MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
 910         }
 911     }
 912
 913     update_old_control_values();
 914     SG_LOG( SG_COCKPIT, SG_INFO, " set_AltitudeMode():" );
 915 }
 916
 917
 918 #if 0
 919 static inline double get_aoa( void ) {
 920     return( cur_fdm_state->get_Gamma_vert_rad() * SGD_RADIANS_TO_DEGREES );
 921 }
 922
 923 static inline double fgAPget_latitude( void ) {
 924     return( cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES );
 925 }
 926
 927 static inline double fgAPget_longitude( void ) {
 928     return( cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES );
 929 }
 930
 931 static inline double fgAPget_roll( void ) {
 932     return( cur_fdm_state->get_Phi() * SGD_RADIANS_TO_DEGREES );
 933 }
 934
 935 static inline double get_pitch( void ) {
 936     return( cur_fdm_state->get_Theta() );
 937 }
 938
 939 double fgAPget_heading( void ) {
 940     return( cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES );
 941 }
 942
 943 static inline double fgAPget_altitude( void ) {
 944     return( cur_fdm_state->get_Altitude() * SG_FEET_TO_METER );
 945 }
 946
 947 static inline double fgAPget_climb( void ) {
 948     // return in meters per minute
 949     return( cur_fdm_state->get_Climb_Rate() * SG_FEET_TO_METER * 60 );
 950 }
 951
 952 static inline double get_sideslip( void ) {
 953     return( cur_fdm_state->get_Beta() );
 954 }
 955
 956 static inline double fgAPget_agl( void ) {
 957     double agl;
 958
 959     agl = cur_fdm_state->get_Altitude() * SG_FEET_TO_METER
 960         - scenery.get_cur_elev();
 961
 962     return( agl );
 963 }
 964 #endif
 965
 966
 967 void FGAutopilot::AltitudeSet( double new_altitude ) {
 968     double target_alt = new_altitude;
 969
 970     // cout << "new altitude = " << new_altitude << endl;
 971
 972     if ( fgGetString("/sim/startup/units") == "feet" ) {
 973         target_alt = new_altitude * SG_FEET_TO_METER;
 974     }
 975
 976     if( target_alt < scenery.get_cur_elev() ) {
 977         target_alt = scenery.get_cur_elev();
 978     }
 979
 980     TargetAltitude = target_alt;
 981     altitude_mode = FG_ALTITUDE_LOCK;
 982
 983     // cout << "TargetAltitude = " << TargetAltitude << endl;
 984
 985     if ( fgGetString("/sim/startup/units") == "feet" ) {
 986         target_alt *= SG_METER_TO_FEET;
 987     }
 988     // ApAltitudeDialogInput->setValue((float)target_alt);
 989     MakeTargetAltitudeStr( target_alt );
 990
 991     update_old_control_values();
 992 }
 993
 994
 995 void FGAutopilot::AltitudeAdjust( double inc )
 996 {
 997     double target_alt, target_agl;
 998
 999     if ( fgGetString("/sim/startup/units") == "feet" ) {
1000         target_alt = TargetAltitude * SG_METER_TO_FEET;
1001         target_agl = TargetAGL * SG_METER_TO_FEET;
1002     } else {
1003         target_alt = TargetAltitude;
1004         target_agl = TargetAGL;
1005     }
1006
1007     // cout << "target_agl = " << target_agl << endl;
1008     // cout << "target_agl / inc = " << target_agl / inc << endl;
1009     // cout << "(int)(target_agl / inc) = " << (int)(target_agl / inc) << endl;
1010
1011     if ( fabs((int)(target_alt / inc) * inc - target_alt) < SG_EPSILON ) {
1012         target_alt += inc;
1013     } else {
1014         target_alt = ( int ) ( target_alt / inc ) * inc + inc;
1015     }
1016
1017     if ( fabs((int)(target_agl / inc) * inc - target_agl) < SG_EPSILON ) {
1018         target_agl += inc;
1019     } else {
1020         target_agl = ( int ) ( target_agl / inc ) * inc + inc;
1021     }
1022
1023     if ( fgGetString("/sim/startup/units") == "feet" ) {
1024         target_alt *= SG_FEET_TO_METER;
1025         target_agl *= SG_FEET_TO_METER;
1026     }
1027
1028     TargetAltitude = target_alt;
1029     TargetAGL = target_agl;
1030
1031     if ( fgGetString("/sim/startup/units") == "feet" )
1032         target_alt *= SG_METER_TO_FEET;
1033     if ( fgGetString("/sim/startup/units") == "feet" )
1034         target_agl *= SG_METER_TO_FEET;
1035
1036     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
1037         MakeTargetAltitudeStr( target_alt );
1038     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
1039         MakeTargetAltitudeStr( target_agl );
1040     }
1041
1042     update_old_control_values();
1043 }
1044
1045
1046 void FGAutopilot::HeadingAdjust( double inc ) {
1047     if ( heading_mode != FG_DG_HEADING_LOCK
1048          && heading_mode != FG_TRUE_HEADING_LOCK )
1049     {
1050         heading_mode = FG_DG_HEADING_LOCK;
1051     }
1052
1053     if ( heading_mode == FG_DG_HEADING_LOCK ) {
1054         double target = ( int ) ( DGTargetHeading / inc ) * inc + inc;
1055         DGTargetHeading = NormalizeDegrees( target );
1056     } else {
1057         double target = ( int ) ( TargetHeading / inc ) * inc + inc;
1058         TargetHeading = NormalizeDegrees( target );
1059     }
1060
1061     update_old_control_values();
1062 }
1063
1064
1065 void FGAutopilot::HeadingSet( double new_heading ) {
1066     if( heading_mode == FG_TRUE_HEADING_LOCK ) {
1067         new_heading = NormalizeDegrees( new_heading );
1068         TargetHeading = new_heading;
1069         MakeTargetHeadingStr( TargetHeading );
1070     } else {
1071         heading_mode = FG_DG_HEADING_LOCK;
1072
1073         new_heading = NormalizeDegrees( new_heading );
1074         DGTargetHeading = new_heading;
1075         // following cast needed ambiguous plib
1076         // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1077         MakeTargetHeadingStr( DGTargetHeading );
1078     }
1079     update_old_control_values();
1080 }
1081
1082 void FGAutopilot::AutoThrottleAdjust( double inc ) {
1083     double target = ( int ) ( TargetSpeed / inc ) * inc + inc;
1084
1085     TargetSpeed = target;
1086 }
1087
1088
1089 void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
1090     auto_throttle = value;
1091
1092     if ( auto_throttle == true ) {
1093         TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
1094         speed_error_accum = 0.0;
1095     }
1096
1097     update_old_control_values();
1098     SG_LOG( SG_COCKPIT, SG_INFO, " fgAPSetAutoThrottle: ("
1099             << auto_throttle << ") " << TargetSpeed );
1100 }