src/Autopilot/newauto.hxx

   1 // newauto.hxx -- 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 #ifndef _NEWAUTO_HXX
  27 #define _NEWAUTO_HXX
  28
  29
  30 #include <simgear/misc/props.hxx>
  31 #include <simgear/route/waypoint.hxx>
  32
  33 #include <Main/fg_props.hxx>
  34
  35 // Structures
  36 class FGAutopilot {
  37
  38 public:
  39
  40     enum fgAutoHeadingMode {
  41         FG_DG_HEADING_LOCK = 0,   // follow bug on directional gryo
  42         FG_TC_HEADING_LOCK = 1,   // keep turn coordinator zero'd
  43         FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
  44         FG_HEADING_NAV1 = 3,      // follow nav1 radial
  45         FG_HEADING_NAV2 = 4,      // follow nav2 radial
  46         FG_HEADING_WAYPOINT = 5   // track next waypoint
  47     };
  48
  49     enum fgAutoAltitudeMode {
  50         FG_ALTITUDE_LOCK = 0,     // lock to a specific altitude
  51         FG_ALTITUDE_TERRAIN = 1,  // try to maintain a specific AGL
  52         FG_ALTITUDE_GS1 = 2,      // follow glide slope 1
  53         FG_ALTITUDE_GS2 = 3,      // follow glide slope 2
  54         FG_ALTITUDE_ARM = 4       // ascend to selected altitude
  55     };
  56
  57
  58 private:
  59
  60     bool heading_hold;          // the current state of the heading hold
  61     bool altitude_hold;         // the current state of the altitude hold
  62     bool auto_throttle;         // the current state of the auto throttle
  63
  64     fgAutoHeadingMode heading_mode;
  65     fgAutoAltitudeMode altitude_mode;
  66
  67     SGWayPoint waypoint;        // the waypoint the AP should steer to.
  68
  69     // double TargetLatitude;   // the latitude the AP should steer to.
  70     // double TargetLongitude;  // the longitude the AP should steer to.
  71     double TargetDistance;      // the distance to Target.
  72     double DGTargetHeading;     // the apparent DG heading to steer towards.
  73     double TargetHeading;       // the true heading the AP should steer to.
  74     double TargetAltitude;      // altitude to hold
  75     double TargetAGL;           // the terrain separation
  76
  77     double TargetSpeed;         // speed to shoot for
  78     double alt_error_accum;     // altitude error accumulator
  79     double climb_error_accum;   // climb error accumulator (for GS)
  80     double speed_error_accum;   // speed error accumulator
  81
  82     double TargetSlope;         // the glide slope hold value
  83
  84     double MaxRoll ;            // the max the plane can roll for the turn
  85     double RollOut;             // when the plane should roll out
  86                                 // measured from Heading
  87     double MaxAileron;          // how far to move the aleroin from center
  88     double RollOutSmooth;       // deg to use for smoothing Aileron Control
  89     double MaxElevator;         // the maximum elevator allowed
  90     double SlopeSmooth;         // smoothing angle for elevator
  91
  92     // following for testing disengagement of autopilot upon pilot
  93     // interaction with controls
  94     double old_aileron;
  95     double old_elevator;
  96     double old_elevator_trim;
  97     double old_rudder;
  98
  99     // manual controls override beyond this value
 100     double disengage_threshold;
 101
 102     // For future cross track error adjust
 103     double old_lat;
 104     double old_lon;
 105
 106     // keeping these locally to save work inside main loop
 107     char TargetLatitudeStr[64];
 108     char TargetLongitudeStr[64];
 109     char TargetLatLonStr[64];
 110     char TargetWP1Str[64];
 111     char TargetWP2Str[64];
 112     char TargetWP3Str[64];
 113     char TargetHeadingStr[64];
 114     char TargetAltitudeStr[64];
 115
 116     // property nodes
 117     SGPropertyNode *latitude_node;
 118     SGPropertyNode *longitude_node;
 119     SGPropertyNode *altitude_node;
 120     SGPropertyNode *altitude_agl_node;
 121     SGPropertyNode *vertical_speed_node;
 122     SGPropertyNode *heading_node;
 123     SGPropertyNode *roll_node;
 124     SGPropertyNode *pitch_node;
 125
 126     SGPropertyNode *min_climb;           // minimum climb speed
 127     SGPropertyNode *best_climb;          // best climb speed
 128     SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
 129     SGPropertyNode *integral_contrib;    // amount of contribution of the integral
 130                                 // component of the pid
 131     SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
 132     SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
 133     SGPropertyNode *TargetClimbRate;    // target climb rate
 134     SGPropertyNode *TargetDescentRate;  // target decent rate
 135     SGPropertyNode *current_throttle; // current throttle (engine 0)
 136
 137 public:
 138
 139     // constructor
 140     FGAutopilot();
 141
 142     // destructor
 143     ~FGAutopilot();
 144
 145     // Initialize autopilot system
 146     void init();
 147
 148     // Reset the autopilot system
 149     void reset(void);
 150
 151     // run an interation of the autopilot (updates control positions)
 152     int run();
 153
 154     void AltitudeSet( double new_altitude );
 155     void AltitudeAdjust( double inc );
 156     void HeadingAdjust( double inc );
 157     void AutoThrottleAdjust( double inc );
 158
 159     void HeadingSet( double value );
 160
 161     inline bool get_HeadingEnabled() const { return heading_hold; }
 162     inline void set_HeadingEnabled( bool value ) { heading_hold = value; }
 163     inline fgAutoHeadingMode get_HeadingMode() const { return heading_mode; }
 164     void set_HeadingMode( fgAutoHeadingMode mode );
 165
 166     inline bool get_AltitudeEnabled() const { return altitude_hold; }
 167     inline void set_AltitudeEnabled( bool value ) { altitude_hold = value; }
 168     inline fgAutoAltitudeMode get_AltitudeMode() const { return altitude_mode;}
 169     void set_AltitudeMode( fgAutoAltitudeMode mode );
 170
 171     inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
 172     void set_AutoThrottleEnabled( bool value );
 173
 174     /* inline void set_WayPoint( const double lon, const double lat,
 175                               const double alt, const string s ) {
 176         waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
 177     } */
 178     inline double get_TargetLatitude() const {
 179         return waypoint.get_target_lat();
 180     }
 181     inline double get_TargetLongitude() const {
 182         return waypoint.get_target_lon();
 183     }
 184     inline void set_old_lat( double val ) { old_lat = val; }
 185     inline void set_old_lon( double val ) { old_lon = val; }
 186     inline double get_TargetHeading() const { return TargetHeading; }
 187     inline void set_TargetHeading( double val ) { TargetHeading = val; }
 188     inline double get_DGTargetHeading() const { return DGTargetHeading; }
 189     inline void set_DGTargetHeading( double val ) { DGTargetHeading = val; }
 190     inline double get_TargetDistance() const { return TargetDistance; }
 191     inline void set_TargetDistance( double val ) { TargetDistance = val; }
 192     inline double get_TargetAltitude() const { return TargetAltitude; }
 193     inline void set_TargetAltitude( double val ) { TargetAltitude = val; }
 194     inline double get_TargetAGL() const { return TargetAGL; }
 195     inline void set_TargetAGL( double val ) { TargetAGL = val; }
 196     inline double get_TargetClimbRate() const {
 197         return TargetClimbRate->getFloatValue();
 198     }
 199     inline void set_TargetClimbRate( double val ) {
 200         fgSetFloat( "/autopilot/config/target-climb-rate-fpm",  val);
 201     }
 202
 203     inline char *get_TargetLatitudeStr() { return TargetLatitudeStr; }
 204     inline char *get_TargetLongitudeStr() { return TargetLongitudeStr; }
 205     inline char *get_TargetWP1Str() { return TargetWP1Str; }
 206     inline char *get_TargetWP2Str() { return TargetWP2Str; }
 207     inline char *get_TargetWP3Str() { return TargetWP3Str; }
 208     inline char *get_TargetHeadingStr() { return TargetHeadingStr; }
 209     inline char *get_TargetAltitudeStr() { return TargetAltitudeStr; }
 210     inline char *get_TargetLatLonStr() { return TargetLatLonStr; }
 211
 212     // utility functions
 213     void MakeTargetLatLonStr( double lat, double lon );
 214     void MakeTargetAltitudeStr( double altitude );
 215     void MakeTargetHeadingStr( double bearing );
 216     void MakeTargetWPStr( double distance );
 217     void update_old_control_values();
 218
 219     // accessors
 220     inline double get_MaxRoll() const { return MaxRoll; }
 221     inline void set_MaxRoll( double val ) { MaxRoll = val; }
 222     inline double get_RollOut() const { return RollOut; }
 223     inline void set_RollOut( double val ) { RollOut = val; }
 224
 225     inline double get_MaxAileron() const { return MaxAileron; }
 226     inline void set_MaxAileron( double val ) { MaxAileron = val; }
 227     inline double get_RollOutSmooth() const { return RollOutSmooth; }
 228     inline void set_RollOutSmooth( double val ) { RollOutSmooth = val; }
 229
 230 };
 231
 232
 233 extern FGAutopilot *current_autopilot;
 234
 235 #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
 236 // #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
 237
 238
 239 /**
 240  * static functions for autopilot properties
 241  */
 242
 243 /**
 244  * Get the autopilot altitude lock (true=on).
 245  */
 246 static bool
 247 getAPAltitudeLock ()
 248 {
 249     return (current_autopilot->get_AltitudeEnabled() &&
 250             current_autopilot->get_AltitudeMode()
 251             == FGAutopilot::FG_ALTITUDE_LOCK);
 252 }
 253
 254
 255 /**
 256  * Set the autopilot altitude lock (true=on).
 257  */
 258 static void
 259 setAPAltitudeLock (bool lock)
 260 {
 261   if (lock)
 262     current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_LOCK);
 263   if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_LOCK)
 264     current_autopilot->set_AltitudeEnabled(lock);
 265 }
 266
 267
 268 /**
 269  * Get the autopilot target altitude in feet.
 270  */
 271 static double
 272 getAPAltitude ()
 273 {
 274   return current_autopilot->get_TargetAltitude() * SG_METER_TO_FEET;
 275 }
 276
 277
 278 /**
 279  * Set the autopilot target altitude in feet.
 280  */
 281 static void
 282 setAPAltitude (double altitude)
 283 {
 284   current_autopilot->set_TargetAltitude( altitude * SG_FEET_TO_METER );
 285 }
 286
 287 /**
 288  * Get the autopilot altitude lock (true=on).
 289  */
 290 static bool
 291 getAPGSLock ()
 292 {
 293     return (current_autopilot->get_AltitudeEnabled() &&
 294             (current_autopilot->get_AltitudeMode()
 295              == FGAutopilot::FG_ALTITUDE_GS1));
 296 }
 297
 298
 299 /**
 300  * Set the autopilot altitude lock (true=on).
 301  */
 302 static void
 303 setAPGSLock (bool lock)
 304 {
 305   if (lock)
 306     current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_GS1);
 307   if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_GS1)
 308     current_autopilot->set_AltitudeEnabled(lock);
 309 }
 310
 311
 312 /**
 313  * Get the autopilot terrain lock (true=on).
 314  */
 315 static bool
 316 getAPTerrainLock ()
 317 {
 318     return (current_autopilot->get_AltitudeEnabled() &&
 319             (current_autopilot->get_AltitudeMode()
 320              == FGAutopilot::FG_ALTITUDE_TERRAIN));
 321 }
 322
 323
 324 /**
 325  * Set the autopilot terrain lock (true=on).
 326  */
 327 static void
 328 setAPTerrainLock (bool lock)
 329 {
 330   if (lock) {
 331     current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_TERRAIN);
 332     current_autopilot
 333       ->set_TargetAGL(fgGetFloat("/position/altitude-agl-ft") *
 334                       SG_FEET_TO_METER);
 335     cout << "Target AGL = "
 336          << fgGetFloat("/position/altitude-agl-ft") * SG_FEET_TO_METER
 337          << endl;
 338   }
 339   if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_TERRAIN)
 340     current_autopilot->set_AltitudeEnabled(lock);
 341 }
 342
 343
 344 /**
 345  * Get the autopilot target altitude in feet.
 346  */
 347 static double
 348 getAPClimb ()
 349 {
 350   return current_autopilot->get_TargetClimbRate() * SG_METER_TO_FEET;
 351 }
 352
 353
 354 /**
 355  * Set the autopilot target altitude in feet.
 356  */
 357 static void
 358 setAPClimb (double rate)
 359 {
 360     current_autopilot->set_TargetClimbRate( rate * SG_FEET_TO_METER );
 361 }
 362
 363
 364 /**
 365  * Get the autopilot heading lock (true=on).
 366  */
 367 static bool
 368 getAPHeadingLock ()
 369 {
 370     return
 371       (current_autopilot->get_HeadingEnabled() &&
 372        current_autopilot->get_HeadingMode() == DEFAULT_AP_HEADING_LOCK);
 373 }
 374
 375
 376 /**
 377  * Set the autopilot heading lock (true=on).
 378  */
 379 static void
 380 setAPHeadingLock (bool lock)
 381 {
 382   if (lock)
 383     current_autopilot->set_HeadingMode(DEFAULT_AP_HEADING_LOCK);
 384   if (current_autopilot->get_HeadingMode() == DEFAULT_AP_HEADING_LOCK)
 385     current_autopilot->set_HeadingEnabled(lock);
 386 }
 387
 388
 389 /**
 390  * Get the autopilot heading bug in degrees.
 391  */
 392 static double
 393 getAPHeadingBug ()
 394 {
 395   return current_autopilot->get_DGTargetHeading();
 396 }
 397
 398
 399 /**
 400  * Set the autopilot heading bug in degrees.
 401  */
 402 static void
 403 setAPHeadingBug (double heading)
 404 {
 405   current_autopilot->set_DGTargetHeading( heading );
 406 }
 407
 408
 409 /**
 410  * Get the autopilot wing leveler lock (true=on).
 411  */
 412 static bool
 413 getAPWingLeveler ()
 414 {
 415     return
 416       (current_autopilot->get_HeadingEnabled() &&
 417        current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK);
 418 }
 419
 420
 421 /**
 422  * Set the autopilot wing leveler lock (true=on).
 423  */
 424 static void
 425 setAPWingLeveler (bool lock)
 426 {
 427     if (lock)
 428         current_autopilot->set_HeadingMode(FGAutopilot::FG_TC_HEADING_LOCK);
 429     if (current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK)
 430       current_autopilot->set_HeadingEnabled(lock);
 431 }
 432
 433 /**
 434  * Return true if the autopilot is locked to NAV1.
 435  */
 436 static bool
 437 getAPNAV1Lock ()
 438 {
 439   return
 440     (current_autopilot->get_HeadingEnabled() &&
 441      current_autopilot->get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1);
 442 }
 443
 444
 445 /**
 446  * Set the autopilot NAV1 lock.
 447  */
 448 static void
 449 setAPNAV1Lock (bool lock)
 450 {
 451   if (lock)
 452     current_autopilot->set_HeadingMode(FGAutopilot::FG_HEADING_NAV1);
 453   if (current_autopilot->get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1)
 454     current_autopilot->set_HeadingEnabled(lock);
 455 }
 456
 457 /**
 458  * Get the autopilot autothrottle lock.
 459  */
 460 static bool
 461 getAPAutoThrottleLock ()
 462 {
 463   return current_autopilot->get_AutoThrottleEnabled();
 464 }
 465
 466
 467 /**
 468  * Set the autothrottle lock.
 469  */
 470 static void
 471 setAPAutoThrottleLock (bool lock)
 472 {
 473   current_autopilot->set_AutoThrottleEnabled(lock);
 474 }
 475
 476
 477 // kludge
 478 static double
 479 getAPRudderControl ()
 480 {
 481     if (getAPHeadingLock())
 482         return current_autopilot->get_TargetHeading();
 483     else
 484         return globals->get_controls()->get_rudder();
 485 }
 486
 487 // kludge
 488 static void
 489 setAPRudderControl (double value)
 490 {
 491     if (getAPHeadingLock()) {
 492         SG_LOG(SG_GENERAL, SG_DEBUG, "setAPRudderControl " << value );
 493         value -= current_autopilot->get_TargetHeading();
 494         current_autopilot->HeadingAdjust(value < 0.0 ? -1.0 : 1.0);
 495     } else {
 496         globals->get_controls()->set_rudder(value);
 497     }
 498 }
 499
 500 // kludge
 501 static double
 502 getAPElevatorControl ()
 503 {
 504   if (getAPAltitudeLock())
 505       return current_autopilot->get_TargetAltitude();
 506   else
 507     return globals->get_controls()->get_elevator();
 508 }
 509
 510 // kludge
 511 static void
 512 setAPElevatorControl (double value)
 513 {
 514     if (value != 0 && getAPAltitudeLock()) {
 515         SG_LOG(SG_GENERAL, SG_DEBUG, "setAPElevatorControl " << value );
 516         value -= current_autopilot->get_TargetAltitude();
 517         current_autopilot->AltitudeAdjust(value < 0.0 ? 100.0 : -100.0);
 518     } else {
 519         globals->get_controls()->set_elevator(value);
 520     }
 521 }
 522
 523 // kludge
 524 static double
 525 getAPThrottleControl ()
 526 {
 527   if (getAPAutoThrottleLock())
 528     return 0.0;                 // always resets
 529   else
 530     return globals->get_controls()->get_throttle(0);
 531 }
 532
 533 // kludge
 534 static void
 535 setAPThrottleControl (double value)
 536 {
 537   if (getAPAutoThrottleLock())
 538     current_autopilot->AutoThrottleAdjust(value < 0.0 ? -0.01 : 0.01);
 539   else
 540     globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
 541 }
 542
 543 #endif // _NEWAUTO_HXX