1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
19 You should have received a copy of the GNU General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU General Public License can also be found on
24 the world wide web at http://www.gnu.org.
28 --------------------------------------------------------------------------------
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
35 This class takes the given set of IC's and finds the aircraft state required to
36 maintain a specified flight condition. This flight condition can be
37 steady-level with non-zero sideslip, a steady turn, a pull-up or pushover.
38 On-ground conditions can be trimmed as well, but this is currently limited to
39 adjusting altitude and pitch angle only. It is implemented using an iterative,
40 one-axis-at-a-time scheme.
42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
44 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
53 #include "FGFDMExec.h"
54 #include "FGJSBBase.h"
55 #include "FGTrimAxis.h"
59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
63 #define ID_TRIM "$Id$"
65 #if defined(_WIN32) && !defined(__CYGWIN__)
66 #define snprintf _snprintf
69 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
75 typedef enum { tLongitudinal=0, tFull, tGround, tPullup,
76 tCustom, tTurn, tNone } TrimMode;
78 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
80 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
82 /** FGTrim -- the trimming routine for JSBSim.
83 FGTrim finds the aircraft attitude and control settings needed to maintain
84 the steady state described by the FGInitialCondition object . It does this
85 iteratively by assigning a control to each state and adjusting that control
86 until the state is within a specified tolerance of zero. States include the
87 recti-linear accelerations udot, vdot, and wdot, the angular accelerations
88 qdot, pdot, and rdot, and the difference between heading and ground track.
89 Controls include the usual flight deck controls available to the pilot plus
90 angle of attack (alpha), sideslip angle(beta), flight path angle (gamma),
91 pitch attitude(theta), roll attitude(phi), and altitude above ground. The
92 last three are used for on-ground trimming. The state-control pairs used in
93 a given trim are completely user configurable and several pre-defined modes
94 are provided as well. They are:
95 - tLongitudinal: Trim wdot with alpha, udot with thrust, qdot with elevator
96 - tFull: tLongitudinal + vdot with phi, pdot with aileron, rdot with rudder
97 and heading minus ground track (hmgt) with beta
98 - tPullup: tLongitudinal but adjust alpha to achieve load factor input
100 - tGround: wdot with altitude, qdot with theta, and pdot with phi
102 The remaining modes include <b>tCustom</b>, which is completely user defined and
105 Note that trims can (and do) fail for reasons that are completely outside
106 the control of the trimming routine itself. The most common problem is the
107 initial conditions: is the model capable of steady state flight
108 at those conditions? Check the speed, altitude, configuration (flaps,
109 gear, etc.), weight, cg, and anything else that may be relevant.
112 FGFDMExec* FDMExec = new FGFDMExec();
114 FGInitialCondition* fgic = new FGInitialCondition(FDMExec);
115 FGTrim fgt(FDMExec, fgic, tFull);
116 fgic->SetVcaibratedKtsIC(100);
117 fgic->SetAltitudeFtIC(1000);
118 fgic->SetClimbRate(500);
119 if( !fgt.DoTrim() ) {
120 cout << "Trim Failed" << endl;
127 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
131 class FGTrim : public FGJSBBase
135 vector<FGTrimAxis*> TrimAxes;
136 unsigned int current_axis;
139 int DebugLevel, Debug;
140 double Tolerance, A_Tolerance;
141 double wdot,udot,qdot;
143 double *sub_iterations;
146 int max_sub_iterations;
152 unsigned int axis_count;
154 double xlo,xhi,alo,ahi;
158 double psidot,thetadot;
161 FGInitialCondition* fgic;
165 /** @return false if there is no change in the current axis accel
166 between accel(control_min) and accel(control_max). If there is a
167 change, sets solutionDomain to:
168 0 for no sign change,
169 -1 if sign change between accel(control_min) and accel(0)
170 1 if sign between accel(0) and accel(control_max)
172 bool findInterval(void);
174 bool checkLimits(void);
176 void setupPullup(void);
177 void setupTurn(void);
179 void updateRates(void);
184 /** Initializes the trimming class
185 @param FDMExec pointer to a JSBSim executive object.
188 FGTrim(FGFDMExec *FDMExec, TrimMode tm=tGround );
196 /** Print the results of the trim. For each axis trimmed, this
197 includes the final state value, control value, and tolerance
199 @return true if trim succeeds
203 /** Iteration statistics
207 /** Clear all state-control pairs and set a predefined trim mode
208 @param tm the set of axes to trim. Can be:
209 tLongitudinal, tFull, tGround, tCustom, or tNone
211 void SetMode(TrimMode tm);
213 /** Clear all state-control pairs from the current configuration.
214 The trimming routine must have at least one state-control pair
215 configured to be useful
217 void ClearStates(void);
219 /** Add a state-control pair to the current configuration. See the enums
220 State and Control in FGTrimAxis.h for the available options.
221 Will fail if the given state is already configured.
222 @param state the accel or other condition to zero
223 @param control the control used to zero the state
224 @return true if add is successful
226 bool AddState( State state, Control control );
228 /** Remove a specific state-control pair from the current configuration
229 @param state the state to remove
230 @return true if removal is successful
232 bool RemoveState( State state );
234 /** Change the control used to zero a state previously configured
235 @param state the accel or other condition to zero
236 @param new_control the control used to zero the state
238 bool EditState( State state, Control new_control );
240 /** automatically switch to trimming longitudinal acceleration with
241 flight path angle (gamma) once it becomes apparent that there
242 is not enough/too much thrust.
243 @param bb true to enable fallback
245 inline void SetGammaFallback(bool bb) { gamma_fallback=bb; }
247 /** query the fallback state
248 @return true if fallback is enabled.
250 inline bool GetGammaFallback(void) { return gamma_fallback; }
252 /** Set the iteration limit. DoTrim() will return false if limit
253 iterations are reached before trim is achieved. The default
254 is 60. This does not ordinarily need to be changed.
255 @param ii integer iteration limit
257 inline void SetMaxCycles(int ii) { max_iterations = ii; }
259 /** Set the per-axis iteration limit. Attempt to zero each state
260 by iterating limit times before moving on to the next. The
261 default limit is 100 and also does not ordinarily need to
263 @param ii integer iteration limit
265 inline void SetMaxCyclesPerAxis(int ii) { max_sub_iterations = ii; }
267 /** Set the tolerance for declaring a state trimmed. Angular accels are
268 held to a tolerance of 1/10th of the given. The default is
269 0.001 for the recti-linear accelerations and 0.0001 for the angular.
271 inline void SetTolerance(double tt) {
273 A_Tolerance = tt / 10;
277 Debug level 1 shows results of each top-level iteration
278 Debug level 2 shows level 1 & results of each per-axis iteration
280 inline void SetDebug(int level) { DebugLevel = level; }
281 inline void ClearDebug(void) { DebugLevel = 0; }
284 Output debug data for one of the axes
285 The State enum is defined in FGTrimAxis.h
287 inline void DebugState(State state) { debug_axis=state; }
289 inline void SetTargetNlf(double nlf) { targetNlf=nlf; }
290 inline double GetTargetNlf(void) { return targetNlf; }