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 // push current error onto queue and add into accumulator
337 // alt_error_queue.push_back(error);
338 APData->alt_error_accum += error;
340 // if queue size larger than 60 ... pop front and subtract
342 // size = alt_error_queue.size();
343 // if ( size > 300 ) {
344 // APData->alt_error_accum -= alt_error_queue.front();
345 // alt_error_queue.pop_front();
349 // calculate integral error, and adjustment amount
350 int_error = APData->alt_error_accum;
351 // printf("error = %.2f int_error = %.2f\n", error, int_error);
352 int_adj = int_error / 8000.0;
354 // caclulate proportional error
356 prop_adj = prop_error / 2000.0;
358 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
359 if ( total_adj > 0.6 ) { total_adj = 0.6; }
360 if ( total_adj < -0.2 ) { total_adj = -0.2; }
362 fgElevSet( total_adj );
366 if (APData->Mode == 2) // Glide slope hold
371 // First, calculate Relative slope and normalize it
372 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
374 // Now calculate the elevator offset from current angle
375 if ( abs(RelSlope) > APData->SlopeSmooth )
377 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
378 RelElevator = -APData->MaxElevator;
380 RelElevator = APData->MaxElevator;
384 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
387 fgElevMove(RelElevator);
398 void fgAPSetMode( int mode)
400 //Remove the following line when the calling funcitons start passing in the data pointer
403 APData = APDataGlobal;
406 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
408 APData->Mode = mode; // set the new mode
412 void fgAPToggleHeading( void )
414 // Remove at a later date
417 APData = APDataGlobal;
420 if ( APData->heading_hold ) {
421 // turn off heading hold
422 APData->heading_hold = 0;
424 // turn on heading hold, lock at current heading
425 APData->heading_hold = 1;
426 APData->TargetHeading = fgAPget_heading();
429 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
430 APData->heading_hold,
431 APData->TargetHeading);
435 void fgAPToggleAltitude( void )
437 // Remove at a later date
440 APData = APDataGlobal;
443 if ( APData->altitude_hold ) {
444 // turn off altitude hold
445 APData->altitude_hold = 0;
447 // turn on altitude hold, lock at current altitude
448 APData->altitude_hold = 1;
449 APData->terrain_follow = 0;
450 APData->TargetAltitude = fgAPget_altitude();
451 APData->alt_error_accum = 0.0;
452 // alt_error_queue.erase( alt_error_queue.begin(),
453 // alt_error_queue.end() );
456 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
457 APData->altitude_hold,
458 APData->TargetAltitude);
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 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: (%d) %.2f\n",
482 APData->terrain_follow,
486 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
488 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
489 //assert(x1 != x2); // Divide by zero error. Cold abort for now
491 double m, b, y; // the constants to find in y=mx+b
493 m=(y2-y1)/(x2-x1); // calculate the m
495 b= y1- m * x1; // calculate the b
497 y = m * x + b; // the final calculation
503 double NormalizeDegrees(double Input)
505 // normalize the input to the range (-180,180]
506 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.