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 )
55 f = current_aircraft.flight;
56 return( FG_V_equiv_kts ); // Make an explicit function call.
59 static double get_aoa( void )
63 f = current_aircraft.flight;
64 return( FG_Gamma_vert_rad * RAD_TO_DEG );
67 static double fgAPget_roll( void )
71 f = current_aircraft.flight;
72 return( FG_Phi * RAD_TO_DEG );
75 static double get_pitch( void )
79 f = current_aircraft.flight;
83 double fgAPget_heading( void )
87 f = current_aircraft.flight;
88 return( FG_Psi * RAD_TO_DEG );
91 static double fgAPget_altitude( void )
95 f = current_aircraft.flight;
97 return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
100 static double fgAPget_climb( void )
104 f = current_aircraft.flight;
106 // return in meters per minute
107 return( FG_Climb_Rate * FEET_TO_METER * 60 );
110 static double get_sideslip( void )
114 f = current_aircraft.flight;
119 static double fgAPget_agl( void )
124 f = current_aircraft.flight;
125 agl = FG_Altitude * FEET_TO_METER - scenery.cur_elev;
130 // End of copied section. ( thanks for the wheel :-)
132 // Local Prototype section
134 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
135 double NormalizeDegrees( double Input);
137 // End Local ProtoTypes
139 fgAPDataPtr APDataGlobal; // global variable holding the AP info
140 // I want this gone. Data should be in aircraft structure
144 void fgAPInit( fgAIRCRAFT *current_aircraft )
148 FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
150 APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
152 if (APData == NULL) {
153 // I couldn't get the mem. Dying
154 FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
158 APData->heading_hold = 0 ; // turn the heading hold off
159 APData->altitude_hold = 0 ; // turn the altitude hold off
161 APData->TargetHeading = 0.0; // default direction, due north
162 APData->TargetAltitude = 3000; // default altitude in meters
163 APData->alt_error_accum = 0.0;
165 // These eventually need to be read from current_aircaft somehow.
167 APData->MaxRoll = 7; // the maximum roll, in Deg
168 APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg
169 APData->MaxAileron= .1; // how far can I move the aleron from center.
170 APData->RollOutSmooth = 10; // Smoothing distance for alerion control
172 //Remove at a later date
173 APDataGlobal = APData;
179 // Remove the following lines when the calling funcitons start
180 // passing in the data pointer
184 APData = APDataGlobal;
187 // heading hold enabled?
188 if ( APData->heading_hold == 1 ) {
195 NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
196 // figure out how far off we are from desired heading
198 // Now it is time to deterime how far we should be rolled.
199 FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
202 // Check if we are further from heading than the roll out point
203 if ( fabs(RelHeading) > APData->RollOut ) {
204 // set Target Roll to Max in desired direction
205 if (RelHeading < 0 ) {
206 TargetRoll = 0-APData->MaxRoll;
208 TargetRoll = APData->MaxRoll;
211 // We have to calculate the Target roll
213 // This calculation engine thinks that the Target roll
214 // should be a line from (RollOut,MaxRoll) to (-RollOut,
215 // -MaxRoll) I hope this works well. If I get ambitious
216 // some day this might become a fancier curve or
219 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
220 -APData->MaxRoll, APData->RollOut,
224 // Target Roll has now been Found.
226 // Compare Target roll to Current Roll, Generate Rel Roll
228 FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
230 RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
232 // Check if we are further from heading than the roll out smooth point
233 if ( fabs(RelRoll) > APData->RollOutSmooth ) {
234 // set Target Roll to Max in desired direction
236 AileronSet = 0-APData->MaxAileron;
238 AileronSet = APData->MaxAileron;
241 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
243 APData->RollOutSmooth,
244 APData->MaxAileron );
247 controls.set_aileron( AileronSet );
248 controls.set_rudder( 0.0 );
251 // altitude hold or terrain follow enabled?
252 if ( (APData->altitude_hold == 1) || (APData->terrain_follow == 1) ) {
253 double speed, max_climb, error;
254 double prop_error, int_error;
255 double prop_adj, int_adj, total_adj;
257 if (APData->altitude_hold == 1) {
258 // normal altitude hold
259 APData->TargetClimbRate =
260 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
261 } else if (APData->terrain_follow == 1) {
262 // brain dead ground hugging with no look ahead
263 APData->TargetClimbRate =
264 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
266 // just try to zero out rate of climb ...
267 APData->TargetClimbRate = 0.0;
272 if ( speed < 90.0 ) {
274 } else if ( speed < 100.0 ) {
275 max_climb = (speed - 90.0) * 20;
277 max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
280 if ( APData->TargetClimbRate > max_climb ) {
281 APData->TargetClimbRate = max_climb;
284 if ( APData->TargetClimbRate < -400.0 ) {
285 APData->TargetClimbRate = -400.0;
288 error = fgAPget_climb() - APData->TargetClimbRate;
290 // accumulate the error under the curve ... this really should
292 APData->alt_error_accum += error;
294 // calculate integral error, and adjustment amount
295 int_error = APData->alt_error_accum;
296 // printf("error = %.2f int_error = %.2f\n", error, int_error);
297 int_adj = int_error / 8000.0;
299 // caclulate proportional error
301 prop_adj = prop_error / 2000.0;
303 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
304 if ( total_adj > 0.6 ) { total_adj = 0.6; }
305 if ( total_adj < -0.2 ) { total_adj = -0.2; }
307 controls.set_elevator( total_adj );
310 // auto throttle enabled?
311 if ( APData->auto_throttle == 1 ) {
313 double prop_error, int_error;
314 double prop_adj, int_adj, total_adj;
316 error = APData->TargetSpeed - get_speed();
318 // accumulate the error under the curve ... this really should
320 APData->speed_error_accum += error;
321 if ( APData->speed_error_accum > 2000.0 ) {
322 APData->speed_error_accum = 2000.0;
324 if ( APData->speed_error_accum < -2000.0 ) {
325 APData->speed_error_accum = -2000.0;
328 // calculate integral error, and adjustment amount
329 int_error = APData->speed_error_accum;
331 // printf("error = %.2f int_error = %.2f\n", error, int_error);
332 int_adj = int_error / 200.0;
334 // caclulate proportional error
336 prop_adj = 0.5 + prop_error / 50.0;
338 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
339 if ( total_adj > 1.0 ) { total_adj = 1.0; }
340 if ( total_adj < 0.0 ) { total_adj = 0.0; }
342 controls.set_throttle( fgCONTROLS::FG_ALL_ENGINES, total_adj );
346 if (APData->Mode == 2) // Glide slope hold
351 // First, calculate Relative slope and normalize it
352 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
354 // Now calculate the elevator offset from current angle
355 if ( abs(RelSlope) > APData->SlopeSmooth )
357 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
358 RelElevator = -APData->MaxElevator;
360 RelElevator = APData->MaxElevator;
364 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
367 fgElevMove(RelElevator);
378 void fgAPSetMode( int mode)
380 //Remove the following line when the calling funcitons start passing in the data pointer
383 APData = APDataGlobal;
386 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
388 APData->Mode = mode; // set the new mode
392 void fgAPToggleHeading( void )
394 // Remove at a later date
397 APData = APDataGlobal;
400 if ( APData->heading_hold ) {
401 // turn off heading hold
402 APData->heading_hold = 0;
404 // turn on heading hold, lock at current heading
405 APData->heading_hold = 1;
406 APData->TargetHeading = fgAPget_heading();
409 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
410 << APData->heading_hold << ") " << APData->TargetHeading );
414 void fgAPToggleAltitude( void )
416 // Remove at a later date
419 APData = APDataGlobal;
422 if ( APData->altitude_hold ) {
423 // turn off altitude hold
424 APData->altitude_hold = 0;
426 // turn on altitude hold, lock at current altitude
427 APData->altitude_hold = 1;
428 APData->terrain_follow = 0;
429 APData->TargetAltitude = fgAPget_altitude();
430 APData->alt_error_accum = 0.0;
431 // alt_error_queue.erase( alt_error_queue.begin(),
432 // alt_error_queue.end() );
435 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
436 << APData->altitude_hold << ") " << APData->TargetAltitude );
440 void fgAPToggleAutoThrottle ( void )
442 // Remove at a later date
445 APData = APDataGlobal;
448 if ( APData->auto_throttle ) {
449 // turn off altitude hold
450 APData->auto_throttle = 0;
452 // turn on terrain follow, lock at current agl
453 APData->auto_throttle = 1;
454 APData->TargetSpeed = get_speed();
455 APData->speed_error_accum = 0.0;
458 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
459 << APData->auto_throttle << ") " << APData->TargetSpeed );
462 void fgAPToggleTerrainFollow( void )
464 // Remove at a later date
467 APData = APDataGlobal;
470 if ( APData->terrain_follow ) {
471 // turn off altitude hold
472 APData->terrain_follow = 0;
474 // turn on terrain follow, lock at current agl
475 APData->terrain_follow = 1;
476 APData->altitude_hold = 0;
477 APData->TargetAGL = fgAPget_agl();
478 APData->alt_error_accum = 0.0;
481 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
482 << APData->terrain_follow << ") " << APData->TargetAGL );
485 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
487 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
488 //assert(x1 != x2); // Divide by zero error. Cold abort for now
490 double m, b, y; // the constants to find in y=mx+b
492 m=(y2-y1)/(x2-x1); // calculate the m
494 b= y1- m * x1; // calculate the b
496 y = m * x + b; // the final calculation
502 double NormalizeDegrees(double Input)
504 // normalize the input to the range (-180,180]
505 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.