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.
32 #include <simgear/props/props.hxx>
33 #include <simgear/structure/subsystem_mgr.hxx>
34 #include <simgear/route/waypoint.hxx>
36 #include <Main/fg_props.hxx>
39 class FGAutopilot : public SGSubsystem
44 enum fgAutoHeadingMode {
45 FG_DG_HEADING_LOCK = 0, // follow bug on directional gryo (vacuum)
46 FG_TC_HEADING_LOCK = 1, // keep turn coordinator zero'd
47 FG_TRUE_HEADING_LOCK = 2, // lock to true heading (i.e. a perfect world)
48 FG_HEADING_NAV1 = 3, // follow nav1 radial
49 FG_HEADING_NAV2 = 4, // follow nav2 radial
50 FG_HEADING_WAYPOINT = 5 // track next waypoint
53 enum fgAutoAltitudeMode {
54 FG_ALTITUDE_LOCK = 0, // lock to a specific altitude (indicated)
55 FG_ALTITUDE_TERRAIN = 1, // try to maintain a specific AGL
56 FG_ALTITUDE_GS1 = 2, // follow glide slope 1
57 FG_ALTITUDE_GS2 = 3, // follow glide slope 2
58 FG_ALTITUDE_ARM = 4, // ascend to selected altitude
59 FG_TRUE_ALTITUDE_LOCK = 5, // lock to a specific true altitude
60 // (i.e. a perfect world)
61 FG_VERT_SPEED = 6 // ascend or descend at selected fpm
66 bool heading_hold; // the current state of the heading hold
67 bool altitude_hold; // the current state of the altitude hold
68 bool auto_throttle; // the current state of the auto throttle
70 fgAutoHeadingMode heading_mode;
71 fgAutoAltitudeMode altitude_mode;
73 SGWayPoint waypoint; // the waypoint the AP should steer to.
75 // double TargetLatitude; // the latitude the AP should steer to.
76 // double TargetLongitude; // the longitude the AP should steer to.
77 double TargetDistance; // the distance to Target.
78 double DGTargetHeading; // the apparent DG heading to steer towards.
79 double TargetHeading; // the true heading the AP should steer to.
80 double TargetAltitude; // altitude to hold
81 double TargetAGL; // the terrain separation
83 double TargetVertSpeed; // vertical speed to shoot for
85 double TargetSpeed; // speed to shoot for
86 double alt_error_accum; // altitude error accumulator
87 double climb_error_accum; // climb error accumulator (for GS)
88 double speed_error_accum; // speed error accumulator
90 double current_ap_throttle; // current ap stored throttle setting used to set all engines
91 double previous_speed; // used to detect acceleration rate
93 double TargetSlope; // the glide slope hold value
95 double MaxRoll ; // the max the plane can roll for the turn
96 double RollOut; // when the plane should roll out
97 // measured from Heading
98 double MaxAileron; // how far to move the aleroin from center
99 double RollOutSmooth; // deg to use for smoothing Aileron Control
100 double MaxElevator; // the maximum elevator allowed
101 double SlopeSmooth; // smoothing angle for elevator
103 // following for testing disengagement of autopilot upon pilot
104 // interaction with controls
107 double old_elevator_trim;
110 // manual controls override beyond this value
111 double disengage_threshold;
113 // For future cross track error adjust
117 // keeping these locally to save work inside main loop
118 char TargetLatitudeStr[64];
119 char TargetLongitudeStr[64];
120 char TargetLatLonStr[64];
121 char TargetWP1Str[64];
122 char TargetWP2Str[64];
123 char TargetWP3Str[64];
124 char TargetHeadingStr[64];
125 char TargetAltitudeStr[64];
128 SGPropertyNode *latitude_node;
129 SGPropertyNode *longitude_node;
130 SGPropertyNode *altitude_node;
131 SGPropertyNode *altitude_agl_node;
132 SGPropertyNode *vertical_speed_node;
133 SGPropertyNode *heading_node;
134 SGPropertyNode *indicated_heading_node;
135 SGPropertyNode *roll_node;
136 SGPropertyNode *pitch_node;
137 SGPropertyNode *airspeed_node;
139 SGPropertyNode *min_climb; // minimum climb speed
140 SGPropertyNode *best_climb; // best climb speed
141 SGPropertyNode *elevator_adj_factor; // factor to optimize altitude hold adjustments
142 SGPropertyNode *integral_contrib; // amount of contribution of the integral
143 // component of the pid
144 SGPropertyNode *zero_pitch_throttle; // amount of throttle at which the aircraft does not pitch up
145 SGPropertyNode *zero_pitch_trim_full_throttle; // amount of trim required to level at full throttle
146 SGPropertyNode *max_aileron_node; // maximum aileron setting range -1 ~ 1
147 SGPropertyNode *max_roll_node; // maximum roll setting in degrees
148 SGPropertyNode *roll_out_node; // start rollout offset from desired heading in degrees
149 SGPropertyNode *roll_out_smooth_node; // rollout smoothing offset in degrees
150 SGPropertyNode *throttle_adj_factor; // factor to optimize autothrottle adjustments
151 SGPropertyNode *throttle_integral; // amount of contribution of the integral
152 // component of the pid
153 SGPropertyNode *speed_change_node; // anticipated speed change
155 SGPropertyNode *TargetClimbRate; // target climb rate
156 SGPropertyNode *TargetDescentRate; // target decent rate
157 SGPropertyNode *current_throttle; // current throttle (engine 0)
158 SGPropertyNode *terrain_follow_factor; // modifies the climb rate to
159 // permit more control when using terrain following mode
170 ////////////////////////////////////////////////////////////////////
171 // Implementation of SGSubsystem.
172 ////////////////////////////////////////////////////////////////////
177 void update (double dt);
179 // Reset the autopilot system
182 void AltitudeSet( double new_altitude );
183 void AltitudeAdjust( double inc );
184 void HeadingAdjust( double inc );
185 void AutoThrottleAdjust( double inc );
187 void HeadingSet( double value );
189 inline bool get_HeadingEnabled() const { return heading_hold; }
190 inline void set_HeadingEnabled( bool value ) { heading_hold = value; }
191 inline fgAutoHeadingMode get_HeadingMode() const { return heading_mode; }
192 void set_HeadingMode( fgAutoHeadingMode mode );
194 inline bool get_AltitudeEnabled() const { return altitude_hold; }
195 inline void set_AltitudeEnabled( bool value ) { altitude_hold = value; }
196 inline fgAutoAltitudeMode get_AltitudeMode() const { return altitude_mode;}
197 void set_AltitudeMode( fgAutoAltitudeMode mode );
199 inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
200 void set_AutoThrottleEnabled( bool value );
202 /* inline void set_WayPoint( const double lon, const double lat,
203 const double alt, const string s ) {
204 waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
206 inline double get_TargetLatitude() const {
207 return waypoint.get_target_lat();
209 inline double get_TargetLongitude() const {
210 return waypoint.get_target_lon();
212 inline void set_old_lat( double val ) { old_lat = val; }
213 inline void set_old_lon( double val ) { old_lon = val; }
214 inline double get_TargetHeading() const { return TargetHeading; }
215 inline void set_TargetHeading( double val ) { TargetHeading = val; }
216 inline double get_DGTargetHeading() const { return DGTargetHeading; }
217 inline void set_DGTargetHeading( double val ) { DGTargetHeading = val; }
218 inline double get_TargetDistance() const { return TargetDistance; }
219 inline void set_TargetDistance( double val ) { TargetDistance = val; }
220 inline double get_TargetAltitude() const { return TargetAltitude; }
221 inline void set_TargetAltitude( double val ) { TargetAltitude = val; }
222 inline double get_TargetSpeed() const { return TargetSpeed; }
223 inline void set_TargetSpeed( double val ) { TargetSpeed = val; }
224 inline double get_TargetAGL() const { return TargetAGL; }
225 inline void set_TargetAGL( double val ) { TargetAGL = val; }
226 inline double get_TargetClimbRate() const {
227 return TargetClimbRate->getFloatValue();
229 inline void set_TargetClimbRate( double val ) {
230 fgSetFloat( "/autopilot/config/target-climb-rate-fpm", val);
233 inline char *get_TargetLatitudeStr() { return TargetLatitudeStr; }
234 inline char *get_TargetLongitudeStr() { return TargetLongitudeStr; }
235 inline char *get_TargetWP1Str() { return TargetWP1Str; }
236 inline char *get_TargetWP2Str() { return TargetWP2Str; }
237 inline char *get_TargetWP3Str() { return TargetWP3Str; }
238 inline char *get_TargetHeadingStr() { return TargetHeadingStr; }
239 inline char *get_TargetAltitudeStr() { return TargetAltitudeStr; }
240 inline char *get_TargetLatLonStr() { return TargetLatLonStr; }
243 void MakeTargetLatLonStr( double lat, double lon );
244 void MakeTargetAltitudeStr( double altitude );
245 void MakeTargetHeadingStr( double bearing );
246 void MakeTargetWPStr( double distance );
247 void update_old_control_values();
250 inline double get_MaxRoll() {
251 return fgGetFloat( "/autopilot/config/max-roll-deg" );
253 inline double get_RollOut() {
254 return fgGetFloat( "/autopilot/config/roll-out-deg" );
256 inline double get_MaxAileron() {
257 return fgGetFloat( "/autopilot/config/max-aileron" );
259 inline double get_RollOutSmooth() {
260 return fgGetFloat( "/autopilot/config/roll-out-smooth-deg" );
262 inline void set_MaxRoll( double val ) {
263 fgSetFloat( "/autopilot/config/max-roll-deg", val );
265 inline void set_RollOut( double val ) {
266 fgSetFloat( "/autopilot/config/roll-out-deg", val );
268 inline void set_MaxAileron( double val ) {
269 fgSetFloat( "/autopilot/config/max-aileron", val );
271 inline void set_RollOutSmooth( double val ) {
272 fgSetFloat( "/autopilot/config/roll-out-smooth-deg", val );
277 bool getAPAltitudeLock () const;
278 void setAPAltitudeLock (bool lock);
279 double getAPAltitude () const;
280 void setAPAltitude (double altitude);
281 bool getAPGSLock () const;
282 void setAPGSLock (bool lock);
283 bool getAPVertSpeedLock () const;
284 void setAPVertSpeedLock (bool lock);
285 bool getAPTerrainLock () const;
286 void setAPTerrainLock (bool lock);
287 double getAPClimb () const;
288 void setAPClimb (double rate);
289 bool getAPHeadingLock () const;
290 void setAPHeadingLock (bool lock);
291 double getAPHeadingBug () const;
292 void setAPHeadingBug (double heading);
293 const char * getAPwaypoint () const;
294 void setAPwaypoint (const char * apt);
295 bool getAPWingLeveler () const;
296 void setAPWingLeveler (bool lock);
297 bool getAPNAV1Lock () const;
298 void setAPNAV1Lock (bool lock);
299 bool getAPAutoThrottleLock () const;
300 void setAPAutoThrottleLock (bool lock);
301 double getAPAutoThrottle () const;
302 void setAPAutoThrottle (double altitude);
303 double getAPRudderControl () const;
304 void setAPRudderControl (double value);
305 double getAPElevatorControl () const;
306 void setAPElevatorControl (double value);
307 double getAPThrottleControl () const;
308 void setAPThrottleControl (double value);
309 double getAPVertSpeed () const;
310 void setAPVertSpeed (double speed);
315 #define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_DG_HEADING_LOCK
316 #define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_ALTITUDE_LOCK
317 //#define DEFAULT_AP_HEADING_LOCK FGAutopilot::FG_TRUE_HEADING_LOCK
318 //#define DEFAULT_AP_ALTITUDE_LOCK FGAutopilot::FG_TRUE_ALTITUDE_LOCK
321 * static functions for autopilot properties
324 #endif // _NEWAUTO_HXX