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 #ifdef NEEDNAMESPACESTD
41 #include "autopilot.hxx"
43 #include <Include/fg_constants.h>
44 #include <Debug/fg_debug.h>
47 static list < double > alt_error_queue;
50 // The below routines were copied right from hud.c ( I hate reinventing
51 // the wheel more than necessary)
53 // The following routines obtain information concerntin the aircraft's
54 // current state and return it to calling instrument display routines.
55 // They should eventually be member functions of the aircraft.
58 static double get_throttleval( void )
60 fgCONTROLS *pcontrols;
62 pcontrols = current_aircraft.controls;
63 return pcontrols->throttle[0]; // Hack limiting to one engine
66 static double get_aileronval( void )
68 fgCONTROLS *pcontrols;
70 pcontrols = current_aircraft.controls;
71 return pcontrols->aileron;
74 static double get_elevatorval( void )
76 fgCONTROLS *pcontrols;
78 pcontrols = current_aircraft.controls;
79 return pcontrols->elevator;
82 static double get_elev_trimval( void )
84 fgCONTROLS *pcontrols;
86 pcontrols = current_aircraft.controls;
87 return pcontrols->elevator_trim;
90 static double get_rudderval( void )
92 fgCONTROLS *pcontrols;
94 pcontrols = current_aircraft.controls;
95 return pcontrols->rudder;
98 static double get_speed( void )
102 f = current_aircraft.flight;
103 return( FG_V_equiv_kts ); // Make an explicit function call.
106 static double get_aoa( void )
110 f = current_aircraft.flight;
111 return( FG_Gamma_vert_rad * RAD_TO_DEG );
114 static double fgAPget_roll( void )
118 f = current_aircraft.flight;
119 return( FG_Phi * RAD_TO_DEG );
122 static double get_pitch( void )
126 f = current_aircraft.flight;
130 double fgAPget_heading( void )
134 f = current_aircraft.flight;
135 return( FG_Psi * RAD_TO_DEG );
138 static double fgAPget_altitude( void )
142 f = current_aircraft.flight;
144 return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
147 static double fgAPget_climb( void )
151 f = current_aircraft.flight;
153 // return in meters per minute
154 return( FG_Climb_Rate * FEET_TO_METER * 60 );
157 static double get_sideslip( void )
161 f = current_aircraft.flight;
166 // End of copied section. ( thanks for the wheel :-)
168 // Local Prototype section
170 double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
171 double NormalizeDegrees( double Input);
173 // End Local ProtoTypes
175 fgAPDataPtr APDataGlobal; // global variable holding the AP info
176 // I want this gone. Data should be in aircraft structure
180 void fgAPInit( fgAIRCRAFT *current_aircraft )
184 fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" );
186 APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1);
188 if (APData == NULL) // I couldn't get the mem. Dying
189 fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n");
191 APData->heading_hold = 0 ; // turn the heading hold off
192 APData->altitude_hold = 0 ; // turn the altitude hold off
194 APData->TargetHeading = 0.0; // default direction, due north
195 APData->TargetAltitude = 3000; // default altitude in meters
196 APData->alt_error_accum = 0.0;
198 // These eventually need to be read from current_aircaft somehow.
200 APData->MaxRoll = 7; // the maximum roll, in Deg
201 APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg
202 APData->MaxAileron= .1; // how far can I move the aleron from center.
203 APData->RollOutSmooth = 10; // Smoothing distance for alerion control
205 //Remove at a later date
206 APDataGlobal = APData;
212 // Remove the following lines when the calling funcitons start
213 // passing in the data pointer
217 APData = APDataGlobal;
220 // heading hold enabled?
221 if ( APData->heading_hold == 1 ) {
228 NormalizeDegrees( APData->TargetHeading - fgAPget_heading());
229 // figure out how far off we are from desired heading
231 // Now it is time to deterime how far we should be rolled.
232 fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading);
235 // Check if we are further from heading than the roll out point
236 if ( fabs(RelHeading) > APData->RollOut ) {
237 // set Target Roll to Max in desired direction
238 if (RelHeading < 0 ) {
239 TargetRoll = 0-APData->MaxRoll;
241 TargetRoll = APData->MaxRoll;
244 // We have to calculate the Target roll
246 // This calculation engine thinks that the Target roll
247 // should be a line from (RollOut,MaxRoll) to (-RollOut,
248 // -MaxRoll) I hope this works well. If I get ambitious
249 // some day this might become a fancier curve or
252 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
253 -APData->MaxRoll, APData->RollOut,
257 // Target Roll has now been Found.
259 // Compare Target roll to Current Roll, Generate Rel Roll
261 fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll);
263 RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll());
265 // Check if we are further from heading than the roll out smooth point
266 if ( fabs(RelRoll) > APData->RollOutSmooth ) {
267 // set Target Roll to Max in desired direction
269 AileronSet = 0-APData->MaxAileron;
271 AileronSet = APData->MaxAileron;
274 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
276 APData->RollOutSmooth,
277 APData->MaxAileron );
280 fgAileronSet(AileronSet);
284 // altitude hold enabled?
285 if ( APData->altitude_hold == 1 ) {
287 double prop_error, int_error;
288 double prop_adj, int_adj;
290 error = fgAPget_climb();
292 // push current error onto queue and add into accumulator
293 alt_error_queue.push_back(error);
294 APData->alt_error_accum += error;
296 // if queue size larger than 20 ... pop front and subtract
298 size = alt_error_queue.size();
300 APData->alt_error_accum -= alt_error_queue.front();
301 alt_error_queue.pop_front();
305 // calculate integral error, and adjustment amount
306 int_error = APData->alt_error_accum / size;
307 printf("int_error = %.2f\n", int_error);
308 int_adj = int_error / 2000.0;
310 // caclulate proportional error
311 prop_error = fgAPget_climb();
312 prop_adj = prop_error / 2000.0;
314 fgElevSet( int_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 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (%d) %.2f\n",
382 APData->heading_hold,
383 APData->TargetHeading);
387 void fgAPToggleAltitude( void )
389 // Remove at a later date
392 APData = APDataGlobal;
395 if ( APData->altitude_hold ) {
396 // turn off altitude hold
397 APData->altitude_hold = 0;
399 // turn on altitude hold, lock at current altitude
400 APData->altitude_hold = 1;
401 APData->TargetAltitude = fgAPget_altitude();
404 fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",
405 APData->altitude_hold,
406 APData->TargetAltitude);
410 double LinearExtrapolate( double x,double x1,double y1,double x2,double y2)
412 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
413 //assert(x1 != x2); // Divide by zero error. Cold abort for now
415 double m, b, y; // the constants to find in y=mx+b
417 m=(y2-y1)/(x2-x1); // calculate the m
419 b= y1- m * x1; // calculate the b
421 y = m * x + b; // the final calculation
427 double NormalizeDegrees(double Input)
429 // normalize the input to the range (-180,180]
430 // Input should not be greater than -360 to 360. Current rules send the output to an undefined state.