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 **************************************************************************/
35 // #include <Include/fg_stl_config.h>
37 #include <Scenery/scenery.hxx>
39 // #ifdef NEEDNAMESPACESTD
40 // using namespace std;
43 #include "autopilot.hxx"
45 #include <Include/fg_constants.h>
46 #include <Debug/fg_debug.h>
49 // static list < double > alt_error_queue;
52 // The below routines were copied right from hud.c ( I hate reinventing
53 // the wheel more than necessary)
55 // The following routines obtain information concerntin the aircraft's
56 // current state and return it to calling instrument display routines.
57 // They should eventually be member functions of the aircraft.
60 static double get_throttleval( void )
62 fgCONTROLS *pcontrols;
64 pcontrols = current_aircraft.controls;
65 return pcontrols->throttle[0]; // Hack limiting to one engine
68 static double get_aileronval( void )
70 fgCONTROLS *pcontrols;
72 pcontrols = current_aircraft.controls;
73 return pcontrols->aileron;
76 static double get_elevatorval( void )
78 fgCONTROLS *pcontrols;
80 pcontrols = current_aircraft.controls;
81 return pcontrols->elevator;
84 static double get_elev_trimval( void )
86 fgCONTROLS *pcontrols;
88 pcontrols = current_aircraft.controls;
89 return pcontrols->elevator_trim;
92 static double get_rudderval( void )
94 fgCONTROLS *pcontrols;
96 pcontrols = current_aircraft.controls;
97 return pcontrols->rudder;
100 static double get_speed( void )
104 f = current_aircraft.flight;
105 return( FG_V_equiv_kts ); // Make an explicit function call.
108 static double get_aoa( void )
112 f = current_aircraft.flight;
113 return( FG_Gamma_vert_rad * RAD_TO_DEG );
116 static double fgAPget_roll( void )
120 f = current_aircraft.flight;
121 return( FG_Phi * RAD_TO_DEG );
124 static double get_pitch( void )
128 f = current_aircraft.flight;
132 double fgAPget_heading( void )
136 f = current_aircraft.flight;
137 return( FG_Psi * RAD_TO_DEG );
140 static double fgAPget_altitude( void )
144 f = current_aircraft.flight;
146 return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
149 static double fgAPget_climb( void )
153 f = current_aircraft.flight;
155 // return in meters per minute
156 return( FG_Climb_Rate * FEET_TO_METER * 60 );
159 static double get_sideslip( void )
163 f = current_aircraft.flight;
168 static double fgAPget_agl( void )
173 f = current_aircraft.flight;
174 agl = FG_Altitude * FEET_TO_METER - scenery.cur_elev;
179 // End of copied section. ( thanks for the wheel :-)
181 // Local Prototype section
183 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
184 double NormalizeDegrees( double Input);
186 // End Local ProtoTypes
188 fgAPDataPtr APDataGlobal; // global variable holding the AP info
189 // I want this gone. Data should be in aircraft structure
193 void fgAPInit( fgAIRCRAFT *current_aircraft )
197 fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
199 APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
201 if (APData == NULL) // I couldn't get the mem. Dying
202 fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
204 APData->heading_hold = 0 ; // turn the heading hold off
205 APData->altitude_hold = 0 ; // turn the altitude hold off
207 APData->TargetHeading = 0.0; // default direction, due north
208 APData->TargetAltitude = 3000; // default altitude in meters
209 APData->alt_error_accum = 0.0;
211 // These eventually need to be read from current_aircaft somehow.
213 APData->MaxRoll = 7; // the maximum roll, in Deg
214 APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg
215 APData->MaxAileron= .1; // how far can I move the aleron from center.
216 APData->RollOutSmooth = 10; // Smoothing distance for alerion control
218 //Remove at a later date
219 APDataGlobal = APData;
225 // Remove the following lines when the calling funcitons start
226 // passing in the data pointer
230 APData = APDataGlobal;
233 // heading hold enabled?
234 if ( APData->heading_hold == 1 ) {
241 NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
242 // figure out how far off we are from desired heading
244 // Now it is time to deterime how far we should be rolled.
245 fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
248 // Check if we are further from heading than the roll out point
249 if ( fabs(RelHeading) > APData->RollOut ) {
250 // set Target Roll to Max in desired direction
251 if (RelHeading < 0 ) {
252 TargetRoll = 0-APData->MaxRoll;
254 TargetRoll = APData->MaxRoll;
257 // We have to calculate the Target roll
259 // This calculation engine thinks that the Target roll
260 // should be a line from (RollOut,MaxRoll) to (-RollOut,
261 // -MaxRoll) I hope this works well. If I get ambitious
262 // some day this might become a fancier curve or
265 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
266 -APData->MaxRoll, APData->RollOut,
270 // Target Roll has now been Found.
272 // Compare Target roll to Current Roll, Generate Rel Roll
274 fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
276 RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
278 // Check if we are further from heading than the roll out smooth point
279 if ( fabs(RelRoll) > APData->RollOutSmooth ) {
280 // set Target Roll to Max in desired direction
282 AileronSet = 0-APData->MaxAileron;
284 AileronSet = APData->MaxAileron;
287 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
289 APData->RollOutSmooth,
290 APData->MaxAileron );
293 fgAileronSet(AileronSet);
297 // altitude hold or terrain follow enabled?
298 if ( (APData->altitude_hold == 1) || (APData->terrain_follow == 1) ) {
299 double speed, max_climb, error;
300 double prop_error, int_error;
301 double prop_adj, int_adj, total_adj;
303 if (APData->altitude_hold == 1) {
304 // normal altitude hold
305 APData->TargetClimbRate =
306 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
307 } else if (APData->terrain_follow == 1) {
308 // brain dead ground hugging with no look ahead
309 APData->TargetClimbRate =
310 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
312 // just try to zero out rate of climb ...
313 APData->TargetClimbRate = 0.0;
318 if ( speed < 90.0 ) {
320 } else if ( speed < 100.0 ) {
321 max_climb = (speed - 90.0) * 20;
323 max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
326 if ( APData->TargetClimbRate > max_climb ) {
327 APData->TargetClimbRate = max_climb;
330 if ( APData->TargetClimbRate < -400.0 ) {
331 APData->TargetClimbRate = -400.0;
334 error = fgAPget_climb() - APData->TargetClimbRate;
336 // accumulate the error under the curve ... this really should
338 APData->alt_error_accum += error;
340 // calculate integral error, and adjustment amount
341 int_error = APData->alt_error_accum;
342 // printf("error = %.2f int_error = %.2f\n", error, int_error);
343 int_adj = int_error / 8000.0;
345 // caclulate proportional error
347 prop_adj = prop_error / 2000.0;
349 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
350 if ( total_adj > 0.6 ) { total_adj = 0.6; }
351 if ( total_adj < -0.2 ) { total_adj = -0.2; }
353 fgElevSet( total_adj );
356 // auto throttle enabled?
357 if ( APData->auto_throttle == 1 ) {
359 double prop_error, int_error;
360 double prop_adj, int_adj, total_adj;
362 error = APData->TargetSpeed - get_speed();
364 // accumulate the error under the curve ... this really should
366 APData->speed_error_accum += error;
367 if ( APData->speed_error_accum > 2000.0 ) {
368 APData->speed_error_accum = 2000.0;
370 if ( APData->speed_error_accum < -2000.0 ) {
371 APData->speed_error_accum = -2000.0;
374 // calculate integral error, and adjustment amount
375 int_error = APData->speed_error_accum;
377 // printf("error = %.2f int_error = %.2f\n", error, int_error);
378 int_adj = int_error / 200.0;
380 // caclulate proportional error
382 prop_adj = 0.5 + prop_error / 50.0;
384 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
385 if ( total_adj > 1.0 ) { total_adj = 1.0; }
386 if ( total_adj < 0.0 ) { total_adj = 0.0; }
388 fgThrottleSet( 0, total_adj );
392 if (APData->Mode == 2) // Glide slope hold
397 // First, calculate Relative slope and normalize it
398 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
400 // Now calculate the elevator offset from current angle
401 if ( abs(RelSlope) > APData->SlopeSmooth )
403 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
404 RelElevator = -APData->MaxElevator;
406 RelElevator = APData->MaxElevator;
410 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
413 fgElevMove(RelElevator);
424 void fgAPSetMode( int mode)
426 //Remove the following line when the calling funcitons start passing in the data pointer
429 APData = APDataGlobal;
432 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
434 APData->Mode = mode; // set the new mode
438 void fgAPToggleHeading( void )
440 // Remove at a later date
443 APData = APDataGlobal;
446 if ( APData->heading_hold ) {
447 // turn off heading hold
448 APData->heading_hold = 0;
450 // turn on heading hold, lock at current heading
451 APData->heading_hold = 1;
452 APData->TargetHeading = fgAPget_heading();
455 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
456 APData->heading_hold,
457 APData->TargetHeading);
461 void fgAPToggleAltitude( void )
463 // Remove at a later date
466 APData = APDataGlobal;
469 if ( APData->altitude_hold ) {
470 // turn off altitude hold
471 APData->altitude_hold = 0;
473 // turn on altitude hold, lock at current altitude
474 APData->altitude_hold = 1;
475 APData->terrain_follow = 0;
476 APData->TargetAltitude = fgAPget_altitude();
477 APData->alt_error_accum = 0.0;
478 // alt_error_queue.erase( alt_error_queue.begin(),
479 // alt_error_queue.end() );
482 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
483 APData->altitude_hold,
484 APData->TargetAltitude);
488 void fgAPToggleAutoThrottle ( void )
490 // Remove at a later date
493 APData = APDataGlobal;
496 if ( APData->auto_throttle ) {
497 // turn off altitude hold
498 APData->auto_throttle = 0;
500 // turn on terrain follow, lock at current agl
501 APData->auto_throttle = 1;
502 APData->TargetSpeed = get_speed();
503 APData->speed_error_accum = 0.0;
506 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: (%d) %.2f\n",
507 APData->auto_throttle,
508 APData->TargetSpeed);
511 void fgAPToggleTerrainFollow( void )
513 // Remove at a later date
516 APData = APDataGlobal;
519 if ( APData->terrain_follow ) {
520 // turn off altitude hold
521 APData->terrain_follow = 0;
523 // turn on terrain follow, lock at current agl
524 APData->terrain_follow = 1;
525 APData->altitude_hold = 0;
526 APData->TargetAGL = fgAPget_agl();
527 APData->alt_error_accum = 0.0;
530 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: (%d) %.2f\n",
531 APData->terrain_follow,
535 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
537 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
538 //assert(x1 != x2); // Divide by zero error. Cold abort for now
540 double m, b, y; // the constants to find in y=mx+b
542 m=(y2-y1)/(x2-x1); // calculate the m
544 b= y1- m * x1; // calculate the b
546 y = m * x + b; // the final calculation
552 double NormalizeDegrees(double Input)
554 // normalize the input to the range (-180,180]
555 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.