1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 3 Header: FGTrim.h 4 Author: Tony Peden 5 Date started: 7/1/99 6 7 ------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) ------------- 8 9 This program is free software; you can redistribute it and/or modify it under 10 the terms of the GNU Lesser General Public License as published by the Free Software 11 Foundation; either version 2 of the License, or (at your option) any later 12 version. 13 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 Lesser General Public License for more 17 details. 18 19 You should have received a copy of the GNU Lesser 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. 22 23 Further information about the GNU Lesser General Public License can also be found on 24 the world wide web at http://www.gnu.org. 25 26 27 HISTORY 28 -------------------------------------------------------------------------------- 29 9/8/99 TP Created 30 31 32 FUNCTIONAL DESCRIPTION 33 -------------------------------------------------------------------------------- 34 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. 41 42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 43 SENTRY 44 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 45 46 #ifndef FGTRIM_H 47 #define FGTRIM_H 48 49 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 50 INCLUDES 51 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 52 53 #include "FGFDMExec.h" 54 #include "FGJSBBase.h" 55 #include "FGTrimAxis.h" 56 57 #include <vector> 58 59 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 60 FORWARD DECLARATIONS 61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 62 63 namespace JSBSim { 64 65 typedef enum { tLongitudinal=0, tFull, tGround, tPullup, 66 tCustom, tTurn, tNone } TrimMode; 67 68 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 69 CLASS DOCUMENTATION 70 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 71 72 /** The trimming routine for JSBSim. 73 FGTrim finds the aircraft attitude and control settings needed to maintain 74 the steady state described by the FGInitialCondition object . It does this 75 iteratively by assigning a control to each state and adjusting that control 76 until the state is within a specified tolerance of zero. States include the 77 recti-linear accelerations udot, vdot, and wdot, the angular accelerations 78 qdot, pdot, and rdot, and the difference between heading and ground track. 79 Controls include the usual flight deck controls available to the pilot plus 80 angle of attack (alpha), sideslip angle(beta), flight path angle (gamma), 81 pitch attitude(theta), roll attitude(phi), and altitude above ground. The 82 last three are used for on-ground trimming. The state-control pairs used in 83 a given trim are completely user configurable and several pre-defined modes 84 are provided as well. They are: 85 - tLongitudinal: Trim wdot with alpha, udot with thrust, qdot with elevator 86 - tFull: tLongitudinal + vdot with phi, pdot with aileron, rdot with rudder 87 and heading minus ground track (hmgt) with beta 88 - tPullup: tLongitudinal but adjust alpha to achieve load factor input 89 with SetTargetNlf() 90 - tGround: wdot with altitude, qdot with theta, and pdot with phi 91 92 The remaining modes include <b>tCustom</b>, which is completely user defined and 93 <b>tNone</b>. 94 95 Note that trims can (and do) fail for reasons that are completely outside 96 the control of the trimming routine itself. The most common problem is the 97 initial conditions: is the model capable of steady state flight 98 at those conditions? Check the speed, altitude, configuration (flaps, 99 gear, etc.), weight, cg, and anything else that may be relevant. 100 101 Example usage: 102 @code 103 FGFDMExec* FDMExec = new FGFDMExec(); 104 105 FGInitialCondition* fgic = new FGInitialCondition(FDMExec); 106 FGTrim fgt(FDMExec, fgic, tFull); 107 fgic->SetVcaibratedKtsIC(100); 108 fgic->SetAltitudeFtIC(1000); 109 fgic->SetClimbRate(500); 110 if( !fgt.DoTrim() ) { 111 cout << "Trim Failed" << endl; 112 } 113 fgt.Report(); 114 @endcode 115 116 @author Tony Peden 117 */ 118 119 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 120 CLASS DECLARATION 121 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ 122 123 class FGTrim : public FGJSBBase 124 { 125 private: 126 127 std::vector<FGTrimAxis> TrimAxes; 128 unsigned int Nsub; 129 TrimMode mode; 130 int DebugLevel, Debug; 131 double Tolerance, A_Tolerance; 132 std::vector<double> sub_iterations, successful; 133 std::vector<bool> solution; 134 unsigned int max_sub_iterations; 135 unsigned int max_iterations; 136 unsigned int total_its; 137 bool gamma_fallback; 138 int solutionDomain; 139 double xlo,xhi,alo,ahi; 140 double targetNlf; 141 int debug_axis; 142 143 double psidot; 144 145 FGFDMExec* fdmex; 146 FGInitialCondition fgic; 147 148 bool solve(FGTrimAxis& axis); 149 150 /** @return false if there is no change in the current axis accel 151 between accel(control_min) and accel(control_max). If there is a 152 change, sets solutionDomain to: 153 0 for no sign change, 154 -1 if sign change between accel(control_min) and accel(0) 155 1 if sign between accel(0) and accel(control_max) 156 */ 157 bool findInterval(FGTrimAxis& axis); 158 159 bool checkLimits(FGTrimAxis& axis); 160 161 void setupPullup(void); 162 void setupTurn(void); 163 164 void updateRates(void); 165 void setDebug(FGTrimAxis& axis); 166 167 struct ContactPoints { 168 FGColumnVector3 location; 169 FGColumnVector3 normal; 170 }; 171 172 struct RotationParameters { 173 double angleMin; 174 std::vector<ContactPoints>::iterator contactRef; 175 }; 176 177 void trimOnGround(void); 178 RotationParameters calcRotation(std::vector<ContactPoints>& contacts, 179 const FGColumnVector3& rotationAxis, 180 const FGColumnVector3& contact0); 181 182 public: 183 /** Initializes the trimming class 184 @param FDMExec pointer to a JSBSim executive object. 185 @param tm trim mode 186 */ 187 FGTrim(FGFDMExec *FDMExec, TrimMode tm=tGround ); 188 189 ~FGTrim(void); 190 191 /** Execute the trim 192 */ 193 bool DoTrim(void); 194 195 /** Print the results of the trim. For each axis trimmed, this 196 includes the final state value, control value, and tolerance 197 used. 198 @return true if trim succeeds 199 */ 200 void Report(void); 201 202 /** Iteration statistics 203 */ 204 void TrimStats(); 205 206 /** Clear all state-control pairs and set a predefined trim mode 207 @param tm the set of axes to trim. Can be: 208 tLongitudinal, tFull, tGround, tCustom, or tNone 209 */ 210 void SetMode(TrimMode tm); 211 212 /** Clear all state-control pairs from the current configuration. 213 The trimming routine must have at least one state-control pair 214 configured to be useful 215 */ 216 void ClearStates(void); 217 218 /** Add a state-control pair to the current configuration. See the enums 219 State and Control in FGTrimAxis.h for the available options. 220 Will fail if the given state is already configured. 221 @param state the accel or other condition to zero 222 @param control the control used to zero the state 223 @return true if add is successful 224 */ 225 bool AddState( State state, Control control ); 226 227 /** Remove a specific state-control pair from the current configuration 228 @param state the state to remove 229 @return true if removal is successful 230 */ 231 bool RemoveState( State state ); 232 233 /** Change the control used to zero a state previously configured 234 @param state the accel or other condition to zero 235 @param new_control the control used to zero the state 236 */ 237 bool EditState( State state, Control new_control ); 238 239 /** automatically switch to trimming longitudinal acceleration with 240 flight path angle (gamma) once it becomes apparent that there 241 is not enough/too much thrust. 242 @param bb true to enable fallback 243 */ SetGammaFallback(bool bb)244 inline void SetGammaFallback(bool bb) { gamma_fallback=bb; } 245 246 /** query the fallback state 247 @return true if fallback is enabled. 248 */ GetGammaFallback(void)249 inline bool GetGammaFallback(void) { return gamma_fallback; } 250 251 /** Set the iteration limit. DoTrim() will return false if limit 252 iterations are reached before trim is achieved. The default 253 is 60. This does not ordinarily need to be changed. 254 @param ii integer iteration limit 255 */ SetMaxCycles(int ii)256 inline void SetMaxCycles(int ii) { max_iterations = ii; } 257 258 /** Set the per-axis iteration limit. Attempt to zero each state 259 by iterating limit times before moving on to the next. The 260 default limit is 100 and also does not ordinarily need to 261 be changed. 262 @param ii integer iteration limit 263 */ SetMaxCyclesPerAxis(int ii)264 inline void SetMaxCyclesPerAxis(int ii) { max_sub_iterations = ii; } 265 266 /** Set the tolerance for declaring a state trimmed. Angular accels are 267 held to a tolerance of 1/10th of the given. The default is 268 0.001 for the recti-linear accelerations and 0.0001 for the angular. 269 */ SetTolerance(double tt)270 inline void SetTolerance(double tt) { 271 Tolerance = tt; 272 A_Tolerance = tt / 10; 273 } 274 275 /** 276 Debug level 1 shows results of each top-level iteration 277 Debug level 2 shows level 1 & results of each per-axis iteration 278 */ SetDebug(int level)279 inline void SetDebug(int level) { DebugLevel = level; } ClearDebug(void)280 inline void ClearDebug(void) { DebugLevel = 0; } 281 282 /** 283 Output debug data for one of the axes 284 The State enum is defined in FGTrimAxis.h 285 */ DebugState(State state)286 inline void DebugState(State state) { debug_axis=state; } 287 SetTargetNlf(double nlf)288 inline void SetTargetNlf(double nlf) { targetNlf=nlf; } GetTargetNlf(void)289 inline double GetTargetNlf(void) { return targetNlf; } 290 291 }; 292 } 293 294 #endif 295