1 /**************************************************************************
2 * autopilot.cxx -- autopilot subsystem
4 * Written by Jeff Goeke-Smith, started April 1998.
6 * Copyright (C) 1998 Jeff Goeke-Smith, jgoeke@voyager.net
8 * This program is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU General Public License as
10 * published by the Free Software Foundation; either version 2 of the
11 * License, or (at your option) any later version.
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 * General Public License for more details.
18 * You should have received a copy of the GNU General Public License
19 * along with this program; if not, write to the Free Software
20 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
24 **************************************************************************/
34 #include <Scenery/scenery.hxx>
36 #include "autopilot.hxx"
38 #include <Include/fg_constants.h>
39 #include <Debug/logstream.hxx>
42 // The below routines were copied right from hud.c ( I hate reinventing
43 // the wheel more than necessary)
45 // The following routines obtain information concerntin the aircraft's
46 // current state and return it to calling instrument display routines.
47 // They should eventually be member functions of the aircraft.
51 static double get_speed( void )
53 return( current_aircraft.fdm_state->get_V_equiv_kts() );
56 static double get_aoa( void )
58 return( current_aircraft.fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
61 static double fgAPget_roll( void )
63 return( current_aircraft.fdm_state->get_Phi() * RAD_TO_DEG );
66 static double get_pitch( void )
68 return( current_aircraft.fdm_state->get_Theta() );
71 double fgAPget_heading( void )
73 return( current_aircraft.fdm_state->get_Psi() * RAD_TO_DEG );
76 static double fgAPget_altitude( void )
78 return( current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER );
81 static double fgAPget_climb( void )
83 // return in meters per minute
84 return( current_aircraft.fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
87 static double get_sideslip( void )
89 return( current_aircraft.fdm_state->get_Beta() );
92 static double fgAPget_agl( void )
96 agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER
102 // End of copied section. ( thanks for the wheel :-)
104 // Local Prototype section
106 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
107 double NormalizeDegrees( double Input);
109 // End Local ProtoTypes
111 fgAPDataPtr APDataGlobal; // global variable holding the AP info
112 // I want this gone. Data should be in aircraft structure
116 void fgAPInit( fgAIRCRAFT *current_aircraft )
120 FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
122 APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
124 if (APData == NULL) {
125 // I couldn't get the mem. Dying
126 FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
130 APData->heading_hold = 0 ; // turn the heading hold off
131 APData->altitude_hold = 0 ; // turn the altitude hold off
133 APData->TargetHeading = 0.0; // default direction, due north
134 APData->TargetAltitude = 3000; // default altitude in meters
135 APData->alt_error_accum = 0.0;
137 // These eventually need to be read from current_aircaft somehow.
139 APData->MaxRoll = 7; // the maximum roll, in Deg
140 APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg
141 APData->MaxAileron= .1; // how far can I move the aleron from center.
142 APData->RollOutSmooth = 10; // Smoothing distance for alerion control
144 //Remove at a later date
145 APDataGlobal = APData;
151 // Remove the following lines when the calling funcitons start
152 // passing in the data pointer
156 APData = APDataGlobal;
159 // heading hold enabled?
160 if ( APData->heading_hold == 1 ) {
167 NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
168 // figure out how far off we are from desired heading
170 // Now it is time to deterime how far we should be rolled.
171 FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
174 // Check if we are further from heading than the roll out point
175 if ( fabs(RelHeading) > APData->RollOut ) {
176 // set Target Roll to Max in desired direction
177 if (RelHeading < 0 ) {
178 TargetRoll = 0-APData->MaxRoll;
180 TargetRoll = APData->MaxRoll;
183 // We have to calculate the Target roll
185 // This calculation engine thinks that the Target roll
186 // should be a line from (RollOut,MaxRoll) to (-RollOut,
187 // -MaxRoll) I hope this works well. If I get ambitious
188 // some day this might become a fancier curve or
191 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
192 -APData->MaxRoll, APData->RollOut,
196 // Target Roll has now been Found.
198 // Compare Target roll to Current Roll, Generate Rel Roll
200 FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
202 RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
204 // Check if we are further from heading than the roll out smooth point
205 if ( fabs(RelRoll) > APData->RollOutSmooth ) {
206 // set Target Roll to Max in desired direction
208 AileronSet = 0-APData->MaxAileron;
210 AileronSet = APData->MaxAileron;
213 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
215 APData->RollOutSmooth,
216 APData->MaxAileron );
219 controls.set_aileron( AileronSet );
220 controls.set_rudder( 0.0 );
223 // altitude hold or terrain follow enabled?
224 if ( (APData->altitude_hold == 1) || (APData->terrain_follow == 1) ) {
225 double speed, max_climb, error;
226 double prop_error, int_error;
227 double prop_adj, int_adj, total_adj;
229 if (APData->altitude_hold == 1) {
230 // normal altitude hold
231 APData->TargetClimbRate =
232 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
233 } else if (APData->terrain_follow == 1) {
234 // brain dead ground hugging with no look ahead
235 APData->TargetClimbRate =
236 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
238 // just try to zero out rate of climb ...
239 APData->TargetClimbRate = 0.0;
244 if ( speed < 90.0 ) {
246 } else if ( speed < 100.0 ) {
247 max_climb = (speed - 90.0) * 20;
249 max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
252 if ( APData->TargetClimbRate > max_climb ) {
253 APData->TargetClimbRate = max_climb;
256 if ( APData->TargetClimbRate < -400.0 ) {
257 APData->TargetClimbRate = -400.0;
260 error = fgAPget_climb() - APData->TargetClimbRate;
262 // accumulate the error under the curve ... this really should
264 APData->alt_error_accum += error;
266 // calculate integral error, and adjustment amount
267 int_error = APData->alt_error_accum;
268 // printf("error = %.2f int_error = %.2f\n", error, int_error);
269 int_adj = int_error / 8000.0;
271 // caclulate proportional error
273 prop_adj = prop_error / 2000.0;
275 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
276 if ( total_adj > 0.6 ) { total_adj = 0.6; }
277 if ( total_adj < -0.2 ) { total_adj = -0.2; }
279 controls.set_elevator( total_adj );
282 // auto throttle enabled?
283 if ( APData->auto_throttle == 1 ) {
285 double prop_error, int_error;
286 double prop_adj, int_adj, total_adj;
288 error = APData->TargetSpeed - get_speed();
290 // accumulate the error under the curve ... this really should
292 APData->speed_error_accum += error;
293 if ( APData->speed_error_accum > 2000.0 ) {
294 APData->speed_error_accum = 2000.0;
296 if ( APData->speed_error_accum < -2000.0 ) {
297 APData->speed_error_accum = -2000.0;
300 // calculate integral error, and adjustment amount
301 int_error = APData->speed_error_accum;
303 // printf("error = %.2f int_error = %.2f\n", error, int_error);
304 int_adj = int_error / 200.0;
306 // caclulate proportional error
308 prop_adj = 0.5 + prop_error / 50.0;
310 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
311 if ( total_adj > 1.0 ) { total_adj = 1.0; }
312 if ( total_adj < 0.0 ) { total_adj = 0.0; }
314 controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
318 if (APData->Mode == 2) // Glide slope hold
323 // First, calculate Relative slope and normalize it
324 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
326 // Now calculate the elevator offset from current angle
327 if ( abs(RelSlope) > APData->SlopeSmooth )
329 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
330 RelElevator = -APData->MaxElevator;
332 RelElevator = APData->MaxElevator;
336 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
339 fgElevMove(RelElevator);
350 void fgAPSetMode( int mode)
352 //Remove the following line when the calling funcitons start passing in the data pointer
355 APData = APDataGlobal;
358 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
360 APData->Mode = mode; // set the new mode
364 void fgAPToggleHeading( void )
366 // Remove at a later date
369 APData = APDataGlobal;
372 if ( APData->heading_hold ) {
373 // turn off heading hold
374 APData->heading_hold = 0;
376 // turn on heading hold, lock at current heading
377 APData->heading_hold = 1;
378 APData->TargetHeading = fgAPget_heading();
381 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
382 << APData->heading_hold << ") " << APData->TargetHeading );
386 void fgAPToggleAltitude( void )
388 // Remove at a later date
391 APData = APDataGlobal;
394 if ( APData->altitude_hold ) {
395 // turn off altitude hold
396 APData->altitude_hold = 0;
398 // turn on altitude hold, lock at current altitude
399 APData->altitude_hold = 1;
400 APData->terrain_follow = 0;
401 APData->TargetAltitude = fgAPget_altitude();
402 APData->alt_error_accum = 0.0;
403 // alt_error_queue.erase( alt_error_queue.begin(),
404 // alt_error_queue.end() );
407 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
408 << APData->altitude_hold << ") " << APData->TargetAltitude );
412 void fgAPToggleAutoThrottle ( void )
414 // Remove at a later date
417 APData = APDataGlobal;
420 if ( APData->auto_throttle ) {
421 // turn off altitude hold
422 APData->auto_throttle = 0;
424 // turn on terrain follow, lock at current agl
425 APData->auto_throttle = 1;
426 APData->TargetSpeed = get_speed();
427 APData->speed_error_accum = 0.0;
430 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
431 << APData->auto_throttle << ") " << APData->TargetSpeed );
434 void fgAPToggleTerrainFollow( void )
436 // Remove at a later date
439 APData = APDataGlobal;
442 if ( APData->terrain_follow ) {
443 // turn off altitude hold
444 APData->terrain_follow = 0;
446 // turn on terrain follow, lock at current agl
447 APData->terrain_follow = 1;
448 APData->altitude_hold = 0;
449 APData->TargetAGL = fgAPget_agl();
450 APData->alt_error_accum = 0.0;
453 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
454 << APData->terrain_follow << ") " << APData->TargetAGL );
457 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
459 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
460 //assert(x1 != x2); // Divide by zero error. Cold abort for now
462 double m, b, y; // the constants to find in y=mx+b
464 m=(y2-y1)/(x2-x1); // calculate the m
466 b= y1- m * x1; // calculate the b
468 y = m * x + b; // the final calculation
474 double NormalizeDegrees(double Input)
476 // normalize the input to the range (-180,180]
477 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.