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