src/FDM/JSBSim/FGTrim.h

   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 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 General Public License for more
  17  details.
  18
  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.
  22
  23  Further information about the GNU 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 DEFINITIONS
  61 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  62
  63 #define ID_TRIM "$Id$"
  64
  65 typedef enum { tLongitudinal, tFull, tGround, tPullup,
  66                tCustom, tNone, tTurn
  67              } TrimMode;
  68
  69 #ifdef _WIN32
  70 #define snprintf _snprintf
  71 #endif
  72
  73 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  74 FORWARD DECLARATIONS
  75 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  76
  77 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  78 COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
  79 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  80
  81 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  82 CLASS DOCUMENTATION
  83 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  84
  85 /** FGTrim -- the trimming routine for JSBSim.
  86     FGTrim finds the aircraft attitude and control settings needed to maintain
  87     the steady state described by the FGInitialCondition object .  It does this
  88     iteratively by assigning a control to each state and adjusting that control
  89     until the state is within a specified tolerance of zero. States include the
  90     recti-linear accelerations udot, vdot, and wdot, the angular accelerations
  91     qdot, pdot, and rdot, and the difference between heading and ground track.
  92     Controls include the usual flight deck controls available to the pilot plus
  93     angle of attack (alpha), sideslip angle(beta), flight path angle (gamma),
  94     pitch attitude(theta), roll attitude(phi), and altitude above ground.  The
  95     last three are used for on-ground trimming. The state-control pairs used in
  96     a given trim are completely user configurable and several pre-defined modes
  97     are provided as well. They are:
  98     <ul>
  99     <li> tLongitudinal: Trim wdot with alpha, udot with thrust, qdot with elevator</li>
 100     <li> tFull: tLongitudinal + vdot with phi, pdot with aileron, rdot with rudder
 101                 and heading minus ground track (hmgt) with beta</li>
 102     <li> tPullup: tLongitudinal but adjust alpha to achieve load factor input
 103          with SetTargetNlf()
 104
 105     <li> tGround: wdot with altitude, qdot with theta, and pdot with phi</li>
 106
 107     The remaining modes include <b>tCustom</b>, which is completely user defined and
 108     <b>tNone</b>.
 109     </ul>
 110
 111     Note that trims can (and do) fail for reasons that are completely outside
 112     the control of the trimming routine itself. The most common problem is the
 113     initial conditions: is the model capable of steady state flight
 114     at those conditions?  Check the speed, altitude, configuration (flaps,
 115     gear, etc.), weight, cg, and anything else that may be relevant.
 116
 117     Example usage:
 118     FGFDMExec* FDMExec = new FGFDMExec();
 119     .
 120     .
 121     .
 122     FGInitialCondition* fgic = new FGInitialCondition(FDMExec);
 123     FGTrim *fgt(FDMExec,fgic,tFull);
 124     fgic->SetVcaibratedKtsIC(100);
 125     fgic->SetAltitudeFtIC(1000);
 126     fgic->SetClimbRate(500);
 127     if( !fgt->DoTrim() ) {
 128       cout << "Trim Failed" << endl;
 129     }
 130     fgt->ReportState();
 131     @author Tony Peden
 132     @version $Id$
 133     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGTrim.h?rev=HEAD&content-type=text/vnd.viewcvs-markup">
 134          Header File </a>
 135     @see <a href="http://cvs.sourceforge.net/cgi-bin/viewcvs.cgi/jsbsim/JSBSim/FGTrim.cpp?rev=HEAD&content-type=text/vnd.viewcvs-markup">
 136          Source File </a>
 137 */
 138
 139 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 140 CLASS DECLARATION
 141 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 142
 143 class FGTrim : public FGJSBBase
 144 {
 145 private:
 146
 147   vector<FGTrimAxis*> TrimAxes;
 148   unsigned int current_axis;
 149   int N, Nsub;
 150   TrimMode mode;
 151   int DebugLevel, Debug;
 152   double Tolerance, A_Tolerance;
 153   double wdot,udot,qdot;
 154   double dth;
 155   double *sub_iterations;
 156   double *successful;
 157   bool *solution;
 158   int max_sub_iterations;
 159   int max_iterations;
 160   int total_its;
 161   bool trimudot;
 162   bool gamma_fallback;
 163   bool trim_failed;
 164   unsigned int axis_count;
 165   int solutionDomain;
 166   double xlo,xhi,alo,ahi;
 167   double targetNlf;
 168   int debug_axis;
 169
 170   double psidot,thetadot;
 171
 172   FGFDMExec* fdmex;
 173   FGInitialCondition* fgic;
 174
 175   bool solve(void);
 176
 177   /** @return false if there is no change in the current axis accel
 178       between accel(control_min) and accel(control_max). If there is a
 179       change, sets solutionDomain to:
 180       0 for no sign change,
 181      -1 if sign change between accel(control_min) and accel(0)
 182       1 if sign between accel(0) and accel(control_max)
 183   */
 184   bool findInterval(void);
 185
 186   bool checkLimits(void);
 187
 188   void setupPullup(void);
 189   void setupTurn(void);
 190
 191   void updateRates(void);
 192
 193   void setDebug(void);
 194
 195 public:
 196   /** Initializes the trimming class
 197       @param FDMExec pointer to a JSBSim executive object.
 198       @param FGIC pointer to a FGInitialCondition object
 199   */
 200   FGTrim(FGFDMExec *FDMExec, TrimMode tt=tGround );
 201
 202     ~FGTrim(void);
 203
 204   /** Execute the trim
 205   */
 206   bool DoTrim(void);
 207
 208   /** Print the results of the trim. For each axis trimmed, this
 209       includes the final state value, control value, and tolerance
 210       used.
 211       @return true if trim succeeds
 212   */
 213   void Report(void);
 214
 215   /** Iteration statistics
 216   */
 217   void TrimStats();
 218
 219   /** Clear all state-control pairs and set a predefined trim mode
 220       @param TrimMode the set of axes to trim. Can be:
 221              tLongitudinal, tFull, tGround, tCustom, or tNone
 222   */
 223   void SetMode(TrimMode tt);
 224
 225   /** Clear all state-control pairs from the current configuration.
 226       The trimming routine must have at least one state-control pair
 227       configured to be useful
 228   */
 229   void ClearStates(void);
 230
 231   /** Add a state-control pair to the current configuration. See the enums
 232       State and Control in FGTrimAxis.h for the available options.
 233       Will fail if the given state is already configured.
 234       @param state the accel or other condition to zero
 235       @param control the control used to zero the state
 236       @return true if add is successful
 237   */
 238   bool AddState( State state, Control control );
 239
 240   /** Remove a specific state-control pair from the current configuration
 241       @param state the state to remove
 242       @return true if removal is successful
 243   */
 244   bool RemoveState( State state );
 245
 246   /** Change the control used to zero a state previously configured
 247       @param state the accel or other condition to zero
 248       @param control the control used to zero the state
 249   */
 250   bool EditState( State state, Control new_control );
 251
 252   /** automatically switch to trimming longitudinal acceleration with
 253       flight path angle (gamma) once it becomes apparent that there
 254       is not enough/too much thrust.
 255       @param gamma_fallback true to enable fallback
 256   */
 257   inline void SetGammaFallback(bool bb) { gamma_fallback=bb; }
 258
 259   /** query the fallback state
 260       @return true if fallback is enabled.
 261   */
 262   inline bool GetGammaFallback(void) { return gamma_fallback; }
 263
 264   /** Set the iteration limit. DoTrim() will return false if limit
 265       iterations are reached before trim is achieved.  The default
 266       is 60.  This does not ordinarily need to be changed.
 267       @param ii integer iteration limit
 268   */
 269   inline void SetMaxCycles(int ii) { max_iterations = ii; }
 270
 271   /** Set the per-axis iteration limit.  Attempt to zero each state
 272       by iterating limit times before moving on to the next. The
 273       default limit is 100 and also does not ordinarily need to
 274       be changed.
 275       @param ii integer iteration limit
 276   */
 277   inline void SetMaxCyclesPerAxis(int ii) { max_sub_iterations = ii; }
 278
 279   /** Set the tolerance for declaring a state trimmed. Angular accels are
 280       held to a tolerance of 1/10th of the given.  The default is
 281       0.001 for the recti-linear accelerations and 0.0001 for the angular.
 282   */
 283   inline void SetTolerance(double tt) {
 284     Tolerance = tt;
 285     A_Tolerance = tt / 10;
 286   }
 287
 288   /**
 289     Debug level 1 shows results of each top-level iteration
 290     Debug level 2 shows level 1 & results of each per-axis iteration
 291   */
 292   inline void SetDebug(int level) { DebugLevel = level; }
 293   inline void ClearDebug(void) { DebugLevel = 0; }
 294
 295   /**
 296     Output debug data for one of the axes
 297     The State enum is defined in FGTrimAxis.h
 298   */
 299   inline void DebugState(State state) { debug_axis=state; }
 300
 301   inline void SetTargetNlf(float nlf) { targetNlf=nlf; }
 302   inline double GetTargetNlf(void) { return targetNlf; }
 303
 304 };
 305
 306
 307 #endif
 308
 309
 310
 311
 312
 313
 314
 315
 316