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/debug/logstream.hxx>
  34 #include <simgear/math/sg_geodesy.hxx>
  35
  36 #include <Cockpit/radiostack.hxx>
  37 #include <Controls/controls.hxx>
  38 #include <FDM/flight.hxx>
  39 #include <Main/bfi.hxx>
  40 #include <Main/globals.hxx>
  41 #include <Main/options.hxx>
  42 #include <Scenery/scenery.hxx>
  43
  44 #include "newauto.hxx"
  45
  46
  47 FGAutopilot *current_autopilot;
  48
  49
  50 // Climb speed constants
  51 const double min_climb = 70.0;  // kts
  52 const double best_climb = 75.0; // kts
  53 const double ideal_climb_rate = 500.0; // fpm
  54
  55 /// These statics will eventually go into the class
  56 /// they are just here while I am experimenting -- NHV :-)
  57 // AutoPilot Gain Adjuster members
  58 static double MaxRollAdjust;        // MaxRollAdjust       = 2 * APData->MaxRoll;
  59 static double RollOutAdjust;        // RollOutAdjust       = 2 * APData->RollOut;
  60 static double MaxAileronAdjust;     // MaxAileronAdjust    = 2 * APData->MaxAileron;
  61 static double RollOutSmoothAdjust;  // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
  62
  63 static char NewTgtAirportId[16];
  64 // static char NewTgtAirportLabel[] = "Enter New TgtAirport ID";
  65
  66 extern char *coord_format_lat(float);
  67 extern char *coord_format_lon(float);
  68
  69
  70 void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
  71     sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
  72     sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
  73     sprintf( TargetLatLonStr, "%s  %s", TargetLatitudeStr, TargetLongitudeStr );
  74 }
  75
  76
  77 void FGAutopilot::MakeTargetAltitudeStr( double altitude ) {
  78     if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
  79         sprintf( TargetAltitudeStr, "APAltitude  %6.0f+", altitude );
  80     } else {
  81         sprintf( TargetAltitudeStr, "APAltitude  %6.0f", altitude );
  82     }
  83 }
  84
  85
  86 void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
  87     if( bearing < 0. ) {
  88         bearing += 360.;
  89     } else if (bearing > 360. ) {
  90         bearing -= 360.;
  91     }
  92     sprintf( TargetHeadingStr, "APHeading  %6.1f", bearing );
  93 }
  94
  95
  96 static inline double get_speed( void ) {
  97     return( cur_fdm_state->get_V_equiv_kts() );
  98 }
  99
 100 static inline double get_ground_speed() {
 101     // starts in ft/s so we convert to kts
 102     double ft_s = cur_fdm_state->get_V_ground_speed()
 103         * current_options.get_speed_up();;
 104     double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
 105
 106     return kts;
 107 }
 108
 109
 110 void FGAutopilot::MakeTargetDistanceStr( double distance ) {
 111     double eta = distance*METER_TO_NM / get_ground_speed();
 112     if ( eta >= 100.0 ) { eta = 99.999; }
 113     int major, minor;
 114     if ( eta < (1.0/6.0) ) {
 115         // within 10 minutes, bump up to min/secs
 116         eta *= 60.0;
 117     }
 118     major = (int)eta;
 119     minor = (int)((eta - (int)eta) * 60.0);
 120     sprintf( TargetDistanceStr, "%s %.2f NM  ETA %d:%02d",
 121              waypoint.get_id().c_str(),
 122              distance*METER_TO_NM, major, minor );
 123     // cout << "distance = " << distance*METER_TO_NM
 124     //      << "  gndsp = " << get_ground_speed()
 125     //      << "  time = " << eta
 126     //      << "  major = " << major
 127     //      << "  minor = " << minor
 128     //      << endl;
 129 }
 130
 131
 132 void FGAutopilot::update_old_control_values() {
 133     old_aileron = FGBFI::getAileron();
 134     old_elevator = FGBFI::getElevator();
 135     old_elevator_trim = FGBFI::getElevatorTrim();
 136     old_rudder = FGBFI::getRudder();
 137 }
 138
 139
 140 // Initialize autopilot subsystem
 141 void FGAutopilot::init() {
 142     FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
 143
 144     heading_hold = false ;      // turn the heading hold off
 145     altitude_hold = false ;     // turn the altitude hold off
 146     auto_throttle = false ;     // turn the auto throttle off
 147
 148     // Initialize target location to startup location
 149     old_lat = FGBFI::getLatitude();
 150     old_lon = FGBFI::getLongitude();
 151     // set_WayPoint( old_lon, old_lat, 0.0, "default" );
 152
 153     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 154
 155     TargetHeading = 0.0;        // default direction, due north
 156     TargetAltitude = 3000;      // default altitude in meters
 157     alt_error_accum = 0.0;
 158     climb_error_accum = 0.0;
 159
 160     MakeTargetAltitudeStr( 3000.0);
 161     MakeTargetHeadingStr( 0.0 );
 162
 163     // These eventually need to be read from current_aircaft somehow.
 164
 165     // the maximum roll, in Deg
 166     MaxRoll = 20;
 167
 168     // the deg from heading to start rolling out at, in Deg
 169     RollOut = 20;
 170
 171     // how far can I move the aleron from center.
 172     MaxAileron = .2;
 173
 174     // Smoothing distance for alerion control
 175     RollOutSmooth = 10;
 176
 177     // Hardwired for now should be in options
 178     // 25% max control variablilty  0.5 / 2.0
 179     disengage_threshold = 1.0;
 180
 181 #if !defined( USING_SLIDER_CLASS )
 182     MaxRollAdjust = 2 * MaxRoll;
 183     RollOutAdjust = 2 * RollOut;
 184     MaxAileronAdjust = 2 * MaxAileron;
 185     RollOutSmoothAdjust = 2 * RollOutSmooth;
 186 #endif  // !defined( USING_SLIDER_CLASS )
 187
 188     update_old_control_values();
 189
 190     // Initialize GUI components of autopilot
 191     // NewTgtAirportInit();
 192     // fgAPAdjustInit() ;
 193     // NewHeadingInit();
 194     // NewAltitudeInit();
 195 };
 196
 197
 198 // Reset the autopilot system
 199 void FGAutopilot::reset() {
 200
 201     heading_hold = false ;      // turn the heading hold off
 202     altitude_hold = false ;     // turn the altitude hold off
 203     auto_throttle = false ;     // turn the auto throttle off
 204
 205     TargetHeading = 0.0;        // default direction, due north
 206     MakeTargetHeadingStr( TargetHeading );
 207
 208     TargetAltitude = 3000;   // default altitude in meters
 209     MakeTargetAltitudeStr( TargetAltitude );
 210
 211     alt_error_accum = 0.0;
 212     climb_error_accum = 0.0;
 213
 214     update_old_control_values();
 215
 216     sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
 217
 218     // TargetLatitude = FGBFI::getLatitude();
 219     // TargetLongitude = FGBFI::getLongitude();
 220     // s<et_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" );
 221
 222     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 223 }
 224
 225
 226 static double NormalizeDegrees( double Input ) {
 227     // normalize the input to the range (-180,180]
 228     // Input should not be greater than -360 to 360.
 229     // Current rules send the output to an undefined state.
 230     if ( Input > 180 )
 231         while(Input > 180 )
 232             Input -= 360;
 233     else if ( Input <= -180 )
 234         while ( Input <= -180 )
 235             Input += 360;
 236     return ( Input );
 237 };
 238
 239 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
 240     // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
 241     //assert(x1 != x2); // Divide by zero error.  Cold abort for now
 242
 243         // Could be
 244         // static double y = 0.0;
 245         // double dx = x2 -x1;
 246         // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
 247         // {
 248
 249     double m, b, y;          // the constants to find in y=mx+b
 250     // double m, b;
 251
 252     m = ( y2 - y1 ) / ( x2 - x1 );   // calculate the m
 253
 254     b = y1 - m * x1;       // calculate the b
 255
 256     y = m * x + b;       // the final calculation
 257
 258     // }
 259
 260     return ( y );
 261
 262 };
 263
 264
 265 int FGAutopilot::run() {
 266     // Remove the following lines when the calling funcitons start
 267     // passing in the data pointer
 268
 269     // get control settings
 270     // double aileron = FGBFI::getAileron();
 271     // double elevator = FGBFI::getElevator();
 272     // double elevator_trim = FGBFI::getElevatorTrim();
 273     // double rudder = FGBFI::getRudder();
 274
 275     double lat = FGBFI::getLatitude();
 276     double lon = FGBFI::getLongitude();
 277     double alt = FGBFI::getAltitude() * FEET_TO_METER;
 278
 279 #ifdef FG_FORCE_AUTO_DISENGAGE
 280     // see if somebody else has changed them
 281     if( fabs(aileron - old_aileron) > disengage_threshold ||
 282         fabs(elevator - old_elevator) > disengage_threshold ||
 283         fabs(elevator_trim - old_elevator_trim) >
 284         disengage_threshold ||
 285         fabs(rudder - old_rudder) > disengage_threshold )
 286     {
 287         // if controls changed externally turn autopilot off
 288         waypoint_hold = false ;   // turn the target hold off
 289         heading_hold = false ;    // turn the heading hold off
 290         altitude_hold = false ;   // turn the altitude hold off
 291         terrain_follow = false;   // turn the terrain_follow hold off
 292         // auto_throttle = false; // turn the auto_throttle off
 293
 294         // stash this runs control settings
 295         old_aileron = aileron;
 296         old_elevator = elevator;
 297         old_elevator_trim = elevator_trim;
 298         old_rudder = rudder;
 299
 300         return 0;
 301     }
 302 #endif
 303
 304     // heading hold
 305     if ( heading_hold == true ) {
 306
 307         if ( heading_mode == FG_HEADING_LOCK ) {
 308             // leave target heading alone
 309         } else if ( heading_mode == FG_HEADING_NAV1 ) {
 310             double tgt_radial;
 311             double cur_radial;
 312             if ( current_radiostack->get_nav1_loc() ) {
 313                 // localizers radials are "true"
 314                 tgt_radial = current_radiostack->get_nav1_radial();
 315             } else {
 316                 tgt_radial = current_radiostack->get_nav1_radial()
 317                     + FGBFI::getMagVar();
 318             }
 319             cur_radial = current_radiostack->get_nav1_heading();
 320             // cout << "target rad (true) = " << tgt_radial
 321             //      << "  current rad (true) = " << cur_radial
 322             //      << endl;
 323
 324             double diff = (tgt_radial - cur_radial);
 325             while ( diff < -180.0 ) { diff += 360.0; }
 326             while ( diff > 180.0 ) { diff -= 360.0; }
 327
 328             diff *= (current_radiostack->get_nav1_loc_dist() * METER_TO_NM);
 329             if ( diff < -30.0 ) { diff = -30.0; }
 330             if ( diff >  30.0 ) { diff =  30.0; }
 331
 332             TargetHeading = cur_radial - diff;
 333             while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
 334             while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
 335             // cout << "target course (true) = " << TargetHeading << endl;
 336         } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 337             // update target heading to waypoint
 338
 339             double wp_course, wp_distance;
 340
 341 #ifdef DO_fgAP_CORRECTED_COURSE
 342             // compute course made good
 343             // this needs lots of special casing before use
 344             double course, reverse, distance, corrected_course;
 345             // need to test for iter
 346             geo_inverse_wgs_84( 0, //fgAPget_altitude(),
 347                                 old_lat,
 348                                 old_lon,
 349                                 lat,
 350                                 lon,
 351                                 &course,
 352                                 &reverse,
 353                                 &distance );
 354 #endif // DO_fgAP_CORRECTED_COURSE
 355
 356             // compute course to way_point
 357             // need to test for iter
 358             waypoint.CourseAndDistance( lon, lat, alt,
 359                                         &wp_course, &wp_distance );
 360
 361 #ifdef DO_fgAP_CORRECTED_COURSE
 362             corrected_course = course - wp_course;
 363             if( fabs(corrected_course) > 0.1 )
 364                 printf("fgAP: course %f  wp_course %f  %f  %f\n",
 365                        course, wp_course, fabs(corrected_course),
 366                        distance );
 367 #endif // DO_fgAP_CORRECTED_COURSE
 368
 369             if ( wp_distance > 100 ) {
 370                 // corrected_course = course - wp_course;
 371                 TargetHeading = NormalizeDegrees(wp_course);
 372             } else {
 373                 cout << "Reached waypoint within " << wp_distance << "meters"
 374                      << endl;
 375
 376                 // pop off this waypoint from the list
 377                 if ( globals->get_route()->size() ) {
 378                     globals->get_route()->delete_first();
 379                 }
 380
 381                 // see if there are more waypoints on the list
 382                 if ( globals->get_route()->size() ) {
 383                     // more waypoints
 384                     set_HeadingMode( FG_HEADING_WAYPOINT );
 385                 } else {
 386                     // end of the line
 387                     heading_mode = FG_HEADING_LOCK;
 388                     // use current heading
 389                     TargetHeading = FGBFI::getHeading();
 390                 }
 391             }
 392             MakeTargetHeadingStr( TargetHeading );
 393             // Force this just in case
 394             TargetDistance = wp_distance;
 395             MakeTargetDistanceStr( wp_distance );
 396         }
 397
 398         double RelHeading;
 399         double TargetRoll;
 400         double RelRoll;
 401         double AileronSet;
 402
 403         RelHeading = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
 404         // figure out how far off we are from desired heading
 405
 406         // Now it is time to deterime how far we should be rolled.
 407         FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
 408
 409
 410         // Check if we are further from heading than the roll out point
 411         if ( fabs( RelHeading ) > RollOut ) {
 412             // set Target Roll to Max in desired direction
 413             if ( RelHeading < 0 ) {
 414                 TargetRoll = 0 - MaxRoll;
 415             } else {
 416                 TargetRoll = MaxRoll;
 417             }
 418         } else {
 419             // We have to calculate the Target roll
 420
 421             // This calculation engine thinks that the Target roll
 422             // should be a line from (RollOut,MaxRoll) to (-RollOut,
 423             // -MaxRoll) I hope this works well.  If I get ambitious
 424             // some day this might become a fancier curve or
 425             // something.
 426
 427             TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
 428                                             -MaxRoll, RollOut,
 429                                             MaxRoll );
 430         }
 431
 432         // Target Roll has now been Found.
 433
 434         // Compare Target roll to Current Roll, Generate Rel Roll
 435
 436         FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
 437
 438         RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() );
 439
 440         // Check if we are further from heading than the roll out smooth point
 441         if ( fabs( RelRoll ) > RollOutSmooth ) {
 442             // set Target Roll to Max in desired direction
 443             if ( RelRoll < 0 ) {
 444                 AileronSet = 0 - MaxAileron;
 445             } else {
 446                 AileronSet = MaxAileron;
 447             }
 448         } else {
 449             AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
 450                                             -MaxAileron,
 451                                             RollOutSmooth,
 452                                             MaxAileron );
 453         }
 454
 455         controls.set_aileron( AileronSet );
 456         controls.set_rudder( AileronSet / 4.0 );
 457         // controls.set_rudder( 0.0 );
 458     }
 459
 460     // altitude hold
 461     if ( altitude_hold ) {
 462         double speed, max_climb, error;
 463         double prop_error, int_error;
 464         double prop_adj, int_adj, total_adj;
 465
 466         if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 467             // normal altitude hold
 468             // cout << "TargetAltitude = " << TargetAltitude
 469             //      << "Altitude = " << FGBFI::getAltitude() * FEET_TO_METER
 470             //      << endl;
 471             TargetClimbRate =
 472                 ( TargetAltitude - FGBFI::getAltitude() * FEET_TO_METER ) * 8.0;
 473         } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
 474             double x = current_radiostack->get_nav1_gs_dist();
 475             double y = (FGBFI::getAltitude()
 476                         - current_radiostack->get_nav1_elev()) * FEET_TO_METER;
 477             double angle = atan2( y, x ) * RAD_TO_DEG;
 478             double gs_diff = current_radiostack->get_nav1_target_gs() - angle;
 479             climb_error_accum += gs_diff * 2.0;
 480             TargetClimbRate = gs_diff * 200.0 + climb_error_accum;
 481         } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 482             // brain dead ground hugging with no look ahead
 483             TargetClimbRate =
 484                 ( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0;
 485             // cout << "target agl = " << TargetAGL
 486             //      << "  current agl = " << fgAPget_agl()
 487             //      << "  target climb rate = " << TargetClimbRate
 488             //      << endl;
 489         } else {
 490             // just try to zero out rate of climb ...
 491             TargetClimbRate = 0.0;
 492         }
 493
 494         speed = get_speed();
 495
 496         if ( speed < min_climb ) {
 497             max_climb = 0.0;
 498         } else if ( speed < best_climb ) {
 499             max_climb = ((best_climb - min_climb) - (best_climb - speed))
 500                 * ideal_climb_rate
 501                 / (best_climb - min_climb);
 502         } else {
 503             max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
 504         }
 505
 506         // this first one could be optional if we wanted to allow
 507         // better climb performance assuming we have the airspeed to
 508         // support it.
 509         if ( TargetClimbRate > ideal_climb_rate ) {
 510             TargetClimbRate = ideal_climb_rate;
 511         }
 512
 513         if ( TargetClimbRate > max_climb ) {
 514             TargetClimbRate = max_climb;
 515         }
 516
 517         if ( TargetClimbRate < -ideal_climb_rate ) {
 518             TargetClimbRate = -ideal_climb_rate;
 519         }
 520
 521         error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate;
 522         // cout << "climb rate = " << fgAPget_climb()
 523         //      << "  error = " << error << endl;
 524
 525         // accumulate the error under the curve ... this really should
 526         // be *= delta t
 527         alt_error_accum += error;
 528
 529         // calculate integral error, and adjustment amount
 530         int_error = alt_error_accum;
 531         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 532         int_adj = int_error / 8000.0;
 533
 534         // caclulate proportional error
 535         prop_error = error;
 536         prop_adj = prop_error / 2000.0;
 537
 538         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 539         // if ( total_adj > 0.6 ) {
 540         //     total_adj = 0.6;
 541         // } else if ( total_adj < -0.2 ) {
 542         //     total_adj = -0.2;
 543         // }
 544         if ( total_adj > 1.0 ) {
 545             total_adj = 1.0;
 546         } else if ( total_adj < -1.0 ) {
 547             total_adj = -1.0;
 548         }
 549
 550         controls.set_elevator( total_adj );
 551     }
 552
 553     // auto throttle
 554     if ( auto_throttle ) {
 555         double error;
 556         double prop_error, int_error;
 557         double prop_adj, int_adj, total_adj;
 558
 559         error = TargetSpeed - get_speed();
 560
 561         // accumulate the error under the curve ... this really should
 562         // be *= delta t
 563         speed_error_accum += error;
 564         if ( speed_error_accum > 2000.0 ) {
 565             speed_error_accum = 2000.0;
 566         }
 567         else if ( speed_error_accum < -2000.0 ) {
 568             speed_error_accum = -2000.0;
 569         }
 570
 571         // calculate integral error, and adjustment amount
 572         int_error = speed_error_accum;
 573
 574         // printf("error = %.2f  int_error = %.2f\n", error, int_error);
 575         int_adj = int_error / 200.0;
 576
 577         // caclulate proportional error
 578         prop_error = error;
 579         prop_adj = 0.5 + prop_error / 50.0;
 580
 581         total_adj = 0.9 * prop_adj + 0.1 * int_adj;
 582         if ( total_adj > 1.0 ) {
 583             total_adj = 1.0;
 584         }
 585         else if ( total_adj < 0.0 ) {
 586             total_adj = 0.0;
 587         }
 588
 589         controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
 590     }
 591
 592 #ifdef THIS_CODE_IS_NOT_USED
 593     if (Mode == 2) // Glide slope hold
 594         {
 595             double RelSlope;
 596             double RelElevator;
 597
 598             // First, calculate Relative slope and normalize it
 599             RelSlope = NormalizeDegrees( TargetSlope - get_pitch());
 600
 601             // Now calculate the elevator offset from current angle
 602             if ( abs(RelSlope) > SlopeSmooth )
 603                 {
 604                     if ( RelSlope < 0 )     //  set RelElevator to max in the correct direction
 605                         RelElevator = -MaxElevator;
 606                     else
 607                         RelElevator = MaxElevator;
 608                 }
 609
 610             else
 611                 RelElevator = LinearExtrapolate(RelSlope,-SlopeSmooth,-MaxElevator,SlopeSmooth,MaxElevator);
 612
 613             // set the elevator
 614             fgElevMove(RelElevator);
 615
 616         }
 617 #endif // THIS_CODE_IS_NOT_USED
 618
 619     // stash this runs control settings
 620     //  update_old_control_values();
 621     old_aileron = controls.get_aileron();
 622     old_elevator = controls.get_elevator();
 623     old_elevator_trim = controls.get_elevator_trim();
 624     old_rudder = controls.get_rudder();
 625
 626     // for cross track error
 627     old_lat = lat;
 628     old_lon = lon;
 629
 630         // Ok, we are done
 631     return 0;
 632 }
 633
 634
 635 void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 636     heading_mode = mode;
 637
 638     if ( heading_mode == FG_HEADING_LOCK ) {
 639         // set heading hold to current heading
 640         TargetHeading = FGBFI::getHeading();
 641     } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
 642         if ( globals->get_route()->size() ) {
 643             double course, distance;
 644
 645             old_lat = FGBFI::getLatitude();
 646             old_lon = FGBFI::getLongitude();
 647
 648             waypoint = globals->get_route()->get_first();
 649             waypoint.CourseAndDistance( FGBFI::getLongitude(),
 650                                         FGBFI::getLatitude(),
 651                                         FGBFI::getLatitude() * FEET_TO_METER,
 652                                         &course, &distance );
 653             TargetHeading = course;
 654             TargetDistance = distance;
 655             MakeTargetLatLonStr( waypoint.get_target_lat(),
 656                                  waypoint.get_target_lon() );
 657             MakeTargetDistanceStr( distance );
 658
 659             if ( waypoint.get_target_alt() > 0.0 ) {
 660                 TargetAltitude = waypoint.get_target_alt();
 661                 altitude_mode = FG_ALTITUDE_LOCK;
 662                 set_AltitudeEnabled( true );
 663                 MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
 664             }
 665
 666             FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( "
 667                     << get_TargetLatitude()  << " "
 668                     << get_TargetLongitude() << " ) "
 669                     );
 670         } else {
 671             // no more way points, default to heading lock.
 672             heading_mode = FG_HEADING_LOCK;
 673             TargetHeading = FGBFI::getHeading();
 674         }
 675     }
 676
 677     MakeTargetHeadingStr( TargetHeading );
 678     update_old_control_values();
 679 }
 680
 681
 682 void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
 683     altitude_mode = mode;
 684
 685     alt_error_accum = 0.0;
 686
 687     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 688         // lock at current altitude
 689         TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
 690
 691         if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 692             MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
 693         } else {
 694             MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
 695         }
 696     } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
 697         climb_error_accum = 0.0;
 698
 699     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 700         TargetAGL = FGBFI::getAGL() * FEET_TO_METER;
 701
 702         if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 703             MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
 704         } else {
 705             MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
 706         }
 707     }
 708
 709     update_old_control_values();
 710     FG_LOG( FG_COCKPIT, FG_INFO, " set_AltitudeMode():" );
 711 }
 712
 713
 714 #if 0
 715 static inline double get_aoa( void ) {
 716     return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
 717 }
 718
 719 static inline double fgAPget_latitude( void ) {
 720     return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
 721 }
 722
 723 static inline double fgAPget_longitude( void ) {
 724     return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
 725 }
 726
 727 static inline double fgAPget_roll( void ) {
 728     return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
 729 }
 730
 731 static inline double get_pitch( void ) {
 732     return( cur_fdm_state->get_Theta() );
 733 }
 734
 735 double fgAPget_heading( void ) {
 736     return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
 737 }
 738
 739 static inline double fgAPget_altitude( void ) {
 740     return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
 741 }
 742
 743 static inline double fgAPget_climb( void ) {
 744     // return in meters per minute
 745     return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
 746 }
 747
 748 static inline double get_sideslip( void ) {
 749     return( cur_fdm_state->get_Beta() );
 750 }
 751
 752 static inline double fgAPget_agl( void ) {
 753     double agl;
 754
 755     agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
 756         - scenery.cur_elev;
 757
 758     return( agl );
 759 }
 760 #endif
 761
 762
 763 void FGAutopilot::AltitudeSet( double new_altitude ) {
 764     double target_alt = new_altitude;
 765
 766     // cout << "new altitude = " << new_altitude << endl;
 767
 768     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 769         target_alt = new_altitude * FEET_TO_METER;
 770     }
 771
 772     if( target_alt < scenery.cur_elev ) {
 773         target_alt = scenery.cur_elev;
 774     }
 775
 776     TargetAltitude = target_alt;
 777     altitude_mode = FG_ALTITUDE_LOCK;
 778
 779     // cout << "TargetAltitude = " << TargetAltitude << endl;
 780
 781     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 782         target_alt *= METER_TO_FEET;
 783     }
 784     // ApAltitudeDialogInput->setValue((float)target_alt);
 785     MakeTargetAltitudeStr( target_alt );
 786
 787     update_old_control_values();
 788 }
 789
 790
 791 void FGAutopilot::AltitudeAdjust( double inc )
 792 {
 793     double target_alt, target_agl;
 794
 795     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 796         target_alt = TargetAltitude * METER_TO_FEET;
 797         target_agl = TargetAGL * METER_TO_FEET;
 798     } else {
 799         target_alt = TargetAltitude;
 800         target_agl = TargetAGL;
 801     }
 802
 803     // cout << "target_agl = " << target_agl << endl;
 804     // cout << "target_agl / inc = " << target_agl / inc << endl;
 805     // cout << "(int)(target_agl / inc) = " << (int)(target_agl / inc) << endl;
 806
 807     if ( fabs((int)(target_alt / inc) * inc - target_alt) < FG_EPSILON ) {
 808         target_alt += inc;
 809     } else {
 810         target_alt = ( int ) ( target_alt / inc ) * inc + inc;
 811     }
 812
 813     if ( fabs((int)(target_agl / inc) * inc - target_agl) < FG_EPSILON ) {
 814         target_agl += inc;
 815     } else {
 816         target_agl = ( int ) ( target_agl / inc ) * inc + inc;
 817     }
 818
 819     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
 820         target_alt *= FEET_TO_METER;
 821         target_agl *= FEET_TO_METER;
 822     }
 823
 824     TargetAltitude = target_alt;
 825     TargetAGL = target_agl;
 826
 827     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 828         target_alt *= METER_TO_FEET;
 829     if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
 830         target_agl *= METER_TO_FEET;
 831
 832     if ( altitude_mode == FG_ALTITUDE_LOCK ) {
 833         MakeTargetAltitudeStr( target_alt );
 834     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
 835         MakeTargetAltitudeStr( target_agl );
 836     }
 837
 838     update_old_control_values();
 839 }
 840
 841
 842 void FGAutopilot::HeadingAdjust( double inc ) {
 843     heading_mode = FG_HEADING_LOCK;
 844
 845     double target = ( int ) ( TargetHeading / inc ) * inc + inc;
 846
 847     TargetHeading = NormalizeDegrees( target );
 848     // following cast needed ambiguous plib
 849     // ApHeadingDialogInput -> setValue ((float)TargetHeading );
 850     MakeTargetHeadingStr( TargetHeading );
 851     update_old_control_values();
 852 }
 853
 854
 855 void FGAutopilot::HeadingSet( double new_heading ) {
 856     heading_mode = FG_HEADING_LOCK;
 857
 858     new_heading = NormalizeDegrees( new_heading );
 859     TargetHeading = new_heading;
 860     // following cast needed ambiguous plib
 861     // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
 862     MakeTargetHeadingStr( TargetHeading );
 863     update_old_control_values();
 864 }
 865
 866 void FGAutopilot::AutoThrottleAdjust( double inc ) {
 867     double target = ( int ) ( TargetSpeed / inc ) * inc + inc;
 868
 869     TargetSpeed = target;
 870 }
 871
 872
 873 void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
 874     auto_throttle = value;
 875
 876     if ( auto_throttle == true ) {
 877         TargetSpeed = FGBFI::getAirspeed();
 878         speed_error_accum = 0.0;
 879     }
 880
 881     update_old_control_values();
 882     FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
 883             << auto_throttle << ") " << TargetSpeed );
 884 }