1 // autopilot.cxx -- autopilot subsystem
3 // Written by Jeff Goeke-Smith, started April 1998.
5 // Copyright (C) 1998 Jeff Goeke-Smith, jgoeke@voyager.net
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
22 // (Log is kept at end of this file)
32 #include <Scenery/scenery.hxx>
34 #include "autopilot.hxx"
36 #include <Include/fg_constants.h>
37 #include <Debug/logstream.hxx>
38 #include <Main/options.hxx>
41 // The below routines were copied right from hud.c ( I hate reinventing
42 // the wheel more than necessary)
44 // The following routines obtain information concerntin the aircraft's
45 // current state and return it to calling instrument display routines.
46 // They should eventually be member functions of the aircraft.
50 static double get_speed( void )
52 return( current_aircraft.fdm_state->get_V_equiv_kts() );
55 static double get_aoa( void )
57 return( current_aircraft.fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
60 static double fgAPget_roll( void )
62 return( current_aircraft.fdm_state->get_Phi() * RAD_TO_DEG );
65 static double get_pitch( void )
67 return( current_aircraft.fdm_state->get_Theta() );
70 double fgAPget_heading( void )
72 return( current_aircraft.fdm_state->get_Psi() * RAD_TO_DEG );
75 static double fgAPget_altitude( void )
77 return( current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER );
80 static double fgAPget_climb( void )
82 // return in meters per minute
83 return( current_aircraft.fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
86 static double get_sideslip( void )
88 return( current_aircraft.fdm_state->get_Beta() );
91 static double fgAPget_agl( void )
95 agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER
101 // End of copied section. ( thanks for the wheel :-)
103 // Local Prototype section
105 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
106 double NormalizeDegrees( double Input);
108 // End Local ProtoTypes
110 fgAPDataPtr APDataGlobal; // global variable holding the AP info
111 // I want this gone. Data should be in aircraft structure
114 bool fgAPHeadingEnabled( void )
118 APData = APDataGlobal;
120 // heading hold enabled?
121 return APData->heading_hold;
124 bool fgAPAltitudeEnabled( void )
128 APData = APDataGlobal;
130 // altitude hold or terrain follow enabled?
131 return APData->altitude_hold || APData->terrain_follow ;
134 bool fgAPAutoThrottleEnabled( void )
138 APData = APDataGlobal;
140 // autothrottle enabled?
141 return APData->auto_throttle;
144 void fgAPAltitudeAdjust( double inc )
146 // Remove at a later date
148 APData = APDataGlobal;
151 double target_alt, target_agl;
153 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
154 target_alt = APData->TargetAltitude * METER_TO_FEET;
155 target_agl = APData->TargetAGL * METER_TO_FEET;
157 target_alt = APData->TargetAltitude;
158 target_agl = APData->TargetAGL;
161 target_alt = (int)(target_alt / inc) * inc + inc;
162 target_agl = (int)(target_agl / inc) * inc + inc;
164 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
165 target_alt *= FEET_TO_METER;
166 target_agl *= FEET_TO_METER;
169 APData->TargetAltitude = target_alt;
170 APData->TargetAGL = target_agl;
173 void fgAPHeadingAdjust( double inc )
176 APData = APDataGlobal;
178 double target = (int)(APData->TargetHeading / inc) * inc + inc;
180 APData->TargetHeading = NormalizeDegrees(target);
183 void fgAPAutoThrottleAdjust( double inc )
186 APData = APDataGlobal;
188 double target = (int)(APData->TargetSpeed / inc) * inc + inc;
190 APData->TargetSpeed = target;
193 void fgAPInit( fgAIRCRAFT *current_aircraft )
197 FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
199 APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
201 if (APData == NULL) {
202 // I couldn't get the mem. Dying
203 FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying.");
207 APData->heading_hold = false ; // turn the heading hold off
208 APData->altitude_hold = false ; // turn the altitude hold off
210 APData->TargetHeading = 0.0; // default direction, due north
211 APData->TargetAltitude = 3000; // default altitude in meters
212 APData->alt_error_accum = 0.0;
214 // These eventually need to be read from current_aircaft somehow.
218 // the maximum roll, in Deg
220 // the deg from heading to start rolling out at, in Deg
221 APData->RollOut = 30;
222 // how far can I move the aleron from center.
223 APData->MaxAileron= .1;
224 // Smoothing distance for alerion control
225 APData->RollOutSmooth = 10;
228 // the maximum roll, in Deg
229 APData->MaxRoll = 20;
231 // the deg from heading to start rolling out at, in Deg
232 APData->RollOut = 20;
234 // how far can I move the aleron from center.
235 APData->MaxAileron= .2;
237 // Smoothing distance for alerion control
238 APData->RollOutSmooth = 10;
240 //Remove at a later date
241 APDataGlobal = APData;
247 // Remove the following lines when the calling funcitons start
248 // passing in the data pointer
252 APData = APDataGlobal;
255 // heading hold enabled?
256 if ( APData->heading_hold == true ) {
263 NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
264 // figure out how far off we are from desired heading
266 // Now it is time to deterime how far we should be rolled.
267 FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
270 // Check if we are further from heading than the roll out point
271 if ( fabs(RelHeading) > APData->RollOut ) {
272 // set Target Roll to Max in desired direction
273 if (RelHeading < 0 ) {
274 TargetRoll = 0-APData->MaxRoll;
276 TargetRoll = APData->MaxRoll;
279 // We have to calculate the Target roll
281 // This calculation engine thinks that the Target roll
282 // should be a line from (RollOut,MaxRoll) to (-RollOut,
283 // -MaxRoll) I hope this works well. If I get ambitious
284 // some day this might become a fancier curve or
287 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
288 -APData->MaxRoll, APData->RollOut,
292 // Target Roll has now been Found.
294 // Compare Target roll to Current Roll, Generate Rel Roll
296 FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
298 RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
300 // Check if we are further from heading than the roll out smooth point
301 if ( fabs(RelRoll) > APData->RollOutSmooth ) {
302 // set Target Roll to Max in desired direction
304 AileronSet = 0-APData->MaxAileron;
306 AileronSet = APData->MaxAileron;
309 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
311 APData->RollOutSmooth,
312 APData->MaxAileron );
315 controls.set_aileron( AileronSet );
316 controls.set_rudder( 0.0 );
319 // altitude hold or terrain follow enabled?
320 if ( APData->altitude_hold || APData->terrain_follow ) {
321 double speed, max_climb, error;
322 double prop_error, int_error;
323 double prop_adj, int_adj, total_adj;
325 if ( APData->altitude_hold ) {
326 // normal altitude hold
327 APData->TargetClimbRate =
328 (APData->TargetAltitude - fgAPget_altitude()) * 8.0;
329 } else if ( APData->terrain_follow ) {
330 // brain dead ground hugging with no look ahead
331 APData->TargetClimbRate =
332 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
334 // just try to zero out rate of climb ...
335 APData->TargetClimbRate = 0.0;
340 if ( speed < 90.0 ) {
342 } else if ( speed < 100.0 ) {
343 max_climb = (speed - 90.0) * 20;
345 max_climb = ( speed - 100.0 ) * 4.0 + 200.0;
348 if ( APData->TargetClimbRate > max_climb ) {
349 APData->TargetClimbRate = max_climb;
352 if ( APData->TargetClimbRate < -400.0 ) {
353 APData->TargetClimbRate = -400.0;
356 error = fgAPget_climb() - APData->TargetClimbRate;
358 // accumulate the error under the curve ... this really should
360 APData->alt_error_accum += error;
362 // calculate integral error, and adjustment amount
363 int_error = APData->alt_error_accum;
364 // printf("error = %.2f int_error = %.2f\n", error, int_error);
365 int_adj = int_error / 8000.0;
367 // caclulate proportional error
369 prop_adj = prop_error / 2000.0;
371 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
372 if ( total_adj > 0.6 ) { total_adj = 0.6; }
373 if ( total_adj < -0.2 ) { total_adj = -0.2; }
375 controls.set_elevator( total_adj );
378 // auto throttle enabled?
379 if ( APData->auto_throttle ) {
381 double prop_error, int_error;
382 double prop_adj, int_adj, total_adj;
384 error = APData->TargetSpeed - get_speed();
386 // accumulate the error under the curve ... this really should
388 APData->speed_error_accum += error;
389 if ( APData->speed_error_accum > 2000.0 ) {
390 APData->speed_error_accum = 2000.0;
392 if ( APData->speed_error_accum < -2000.0 ) {
393 APData->speed_error_accum = -2000.0;
396 // calculate integral error, and adjustment amount
397 int_error = APData->speed_error_accum;
399 // printf("error = %.2f int_error = %.2f\n", error, int_error);
400 int_adj = int_error / 200.0;
402 // caclulate proportional error
404 prop_adj = 0.5 + prop_error / 50.0;
406 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
407 if ( total_adj > 1.0 ) { total_adj = 1.0; }
408 if ( total_adj < 0.0 ) { total_adj = 0.0; }
410 controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
414 if (APData->Mode == 2) // Glide slope hold
419 // First, calculate Relative slope and normalize it
420 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
422 // Now calculate the elevator offset from current angle
423 if ( abs(RelSlope) > APData->SlopeSmooth )
425 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
426 RelElevator = -APData->MaxElevator;
428 RelElevator = APData->MaxElevator;
432 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
435 fgElevMove(RelElevator);
446 void fgAPSetMode( int mode)
448 //Remove the following line when the calling funcitons start passing in the data pointer
451 APData = APDataGlobal;
454 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
456 APData->Mode = mode; // set the new mode
460 void fgAPToggleHeading( void )
462 // Remove at a later date
465 APData = APDataGlobal;
468 if ( APData->heading_hold ) {
469 // turn off heading hold
470 APData->heading_hold = false;
472 // turn on heading hold, lock at current heading
473 APData->heading_hold = true;
474 APData->TargetHeading = fgAPget_heading();
477 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
478 << APData->heading_hold << ") " << APData->TargetHeading );
482 void fgAPToggleAltitude( void )
484 // Remove at a later date
487 APData = APDataGlobal;
490 if ( APData->altitude_hold ) {
491 // turn off altitude hold
492 APData->altitude_hold = false;
494 // turn on altitude hold, lock at current altitude
495 APData->altitude_hold = true;
496 APData->terrain_follow = false;
497 APData->TargetAltitude = fgAPget_altitude();
498 APData->alt_error_accum = 0.0;
499 // alt_error_queue.erase( alt_error_queue.begin(),
500 // alt_error_queue.end() );
503 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
504 << APData->altitude_hold << ") " << APData->TargetAltitude );
508 void fgAPToggleAutoThrottle ( void )
510 // Remove at a later date
513 APData = APDataGlobal;
516 if ( APData->auto_throttle ) {
517 // turn off altitude hold
518 APData->auto_throttle = false;
520 // turn on terrain follow, lock at current agl
521 APData->auto_throttle = true;
522 APData->TargetSpeed = get_speed();
523 APData->speed_error_accum = 0.0;
526 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
527 << APData->auto_throttle << ") " << APData->TargetSpeed );
530 void fgAPToggleTerrainFollow( void )
532 // Remove at a later date
535 APData = APDataGlobal;
538 if ( APData->terrain_follow ) {
539 // turn off altitude hold
540 APData->terrain_follow = false;
542 // turn on terrain follow, lock at current agl
543 APData->terrain_follow = true;
544 APData->altitude_hold = false;
545 APData->TargetAGL = fgAPget_agl();
546 APData->alt_error_accum = 0.0;
549 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
550 << APData->terrain_follow << ") " << APData->TargetAGL );
553 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
555 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
556 //assert(x1 != x2); // Divide by zero error. Cold abort for now
558 double m, b, y; // the constants to find in y=mx+b
560 m=(y2-y1)/(x2-x1); // calculate the m
562 b= y1- m * x1; // calculate the b
564 y = m * x + b; // the final calculation
570 double NormalizeDegrees(double Input)
572 // normalize the input to the range (-180,180]
573 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.
584 // Revision 1.15 1999/02/12 23:22:35 curt
585 // Allow auto-throttle adjustment while active.
587 // Revision 1.14 1999/02/12 22:17:14 curt
588 // Changes contributed by Norman Vine to allow adjustment of the autopilot
589 // while it is activated.