1 // newauto.hxx -- autopilot defines and prototypes (very alpha)
3 // Started April 1998 Copyright (C) 1998
5 // Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
6 // Norman Vine <nhv@cape.com>
7 // Curtis Olson <curt@flightgear.org>
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.
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.
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.
30 #include <simgear/misc/props.hxx>
31 #include <simgear/route/waypoint.hxx>
33 #include <Main/fg_props.hxx>
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
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
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
64 fgAutoHeadingMode heading_mode;
65 fgAutoAltitudeMode altitude_mode;
67 SGWayPoint waypoint; // the waypoint the AP should steer to.
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
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
82 double TargetSlope; // the glide slope hold value
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
92 // following for testing disengagement of autopilot upon pilot
93 // interaction with controls
96 double old_elevator_trim;
99 // manual controls override beyond this value
100 double disengage_threshold;
102 // For future cross track error adjust
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];
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;
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)
145 // Initialize autopilot system
148 // Reset the autopilot system
151 // run an interation of the autopilot (updates control positions)
154 void AltitudeSet( double new_altitude );
155 void AltitudeAdjust( double inc );
156 void HeadingAdjust( double inc );
157 void AutoThrottleAdjust( double inc );
159 void HeadingSet( double value );
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 );
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 );
171 inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
172 void set_AutoThrottleEnabled( bool value );
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" );
178 inline double get_TargetLatitude() const {
179 return waypoint.get_target_lat();
181 inline double get_TargetLongitude() const {
182 return waypoint.get_target_lon();
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();
199 inline void set_TargetClimbRate( double val ) {
200 fgSetFloat( "/autopilot/config/target-climb-rate-fpm", val);
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; }
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();
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; }
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; }
233 extern FGAutopilot *current_autopilot;
235 #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
236 // #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
240 * static functions for autopilot properties
244 * Get the autopilot altitude lock (true=on).
249 return (current_autopilot->get_AltitudeEnabled() &&
250 current_autopilot->get_AltitudeMode()
251 == FGAutopilot::FG_ALTITUDE_LOCK);
256 * Set the autopilot altitude lock (true=on).
259 setAPAltitudeLock (bool 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);
269 * Get the autopilot target altitude in feet.
274 return current_autopilot->get_TargetAltitude() * SG_METER_TO_FEET;
279 * Set the autopilot target altitude in feet.
282 setAPAltitude (double altitude)
284 current_autopilot->set_TargetAltitude( altitude * SG_FEET_TO_METER );
288 * Get the autopilot altitude lock (true=on).
293 return (current_autopilot->get_AltitudeEnabled() &&
294 (current_autopilot->get_AltitudeMode()
295 == FGAutopilot::FG_ALTITUDE_GS1));
300 * Set the autopilot altitude lock (true=on).
303 setAPGSLock (bool 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);
313 * Get the autopilot terrain lock (true=on).
318 return (current_autopilot->get_AltitudeEnabled() &&
319 (current_autopilot->get_AltitudeMode()
320 == FGAutopilot::FG_ALTITUDE_TERRAIN));
325 * Set the autopilot terrain lock (true=on).
328 setAPTerrainLock (bool lock)
331 current_autopilot->set_AltitudeMode(FGAutopilot::FG_ALTITUDE_TERRAIN);
333 ->set_TargetAGL(fgGetFloat("/position/altitude-agl-ft") *
335 cout << "Target AGL = "
336 << fgGetFloat("/position/altitude-agl-ft") * SG_FEET_TO_METER
339 if (current_autopilot->get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_TERRAIN)
340 current_autopilot->set_AltitudeEnabled(lock);
345 * Get the autopilot target altitude in feet.
350 return current_autopilot->get_TargetClimbRate() * SG_METER_TO_FEET;
355 * Set the autopilot target altitude in feet.
358 setAPClimb (double rate)
360 current_autopilot->set_TargetClimbRate( rate * SG_FEET_TO_METER );
365 * Get the autopilot heading lock (true=on).
371 (current_autopilot->get_HeadingEnabled() &&
372 current_autopilot->get_HeadingMode() == DEFAULT_AP_HEADING_LOCK);
377 * Set the autopilot heading lock (true=on).
380 setAPHeadingLock (bool 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);
390 * Get the autopilot heading bug in degrees.
395 return current_autopilot->get_DGTargetHeading();
400 * Set the autopilot heading bug in degrees.
403 setAPHeadingBug (double heading)
405 current_autopilot->set_DGTargetHeading( heading );
410 * Get the autopilot wing leveler lock (true=on).
416 (current_autopilot->get_HeadingEnabled() &&
417 current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK);
422 * Set the autopilot wing leveler lock (true=on).
425 setAPWingLeveler (bool 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);
434 * Return true if the autopilot is locked to NAV1.
440 (current_autopilot->get_HeadingEnabled() &&
441 current_autopilot->get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1);
446 * Set the autopilot NAV1 lock.
449 setAPNAV1Lock (bool 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);
458 * Get the autopilot autothrottle lock.
461 getAPAutoThrottleLock ()
463 return current_autopilot->get_AutoThrottleEnabled();
468 * Set the autothrottle lock.
471 setAPAutoThrottleLock (bool lock)
473 current_autopilot->set_AutoThrottleEnabled(lock);
479 getAPRudderControl ()
481 if (getAPHeadingLock())
482 return current_autopilot->get_TargetHeading();
484 return globals->get_controls()->get_rudder();
489 setAPRudderControl (double value)
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);
496 globals->get_controls()->set_rudder(value);
502 getAPElevatorControl ()
504 if (getAPAltitudeLock())
505 return current_autopilot->get_TargetAltitude();
507 return globals->get_controls()->get_elevator();
512 setAPElevatorControl (double value)
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);
519 globals->get_controls()->set_elevator(value);
525 getAPThrottleControl ()
527 if (getAPAutoThrottleLock())
528 return 0.0; // always resets
530 return globals->get_controls()->get_throttle(0);
535 setAPThrottleControl (double value)
537 if (getAPAutoThrottleLock())
538 current_autopilot->AutoThrottleAdjust(value < 0.0 ? -0.01 : 0.01);
540 globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
543 #endif // _NEWAUTO_HXX