src/FDM/JSBSim/initialization/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 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 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=0, tFull, tGround, tPullup,
  76                tCustom, tTurn, tNone } TrimMode;
  77
  78 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  79 CLASS DOCUMENTATION
  80 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
  81
  82 /** 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
  99                with SetTargetNlf()
 100     - tGround: wdot with altitude, qdot with theta, and pdot with phi
 101
 102     The remaining modes include <b>tCustom</b>, which is completely user defined and
 103     <b>tNone</b>.
 104
 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.
 110
 111     Example usage:
 112     @code
 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.Report();
 124     @endcode
 125
 126     @author Tony Peden
 127     @version "$Id$"
 128 */
 129
 130 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 131 CLASS DECLARATION
 132 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
 133
 134 class FGTrim : public FGJSBBase
 135 {
 136 private:
 137
 138   vector<FGTrimAxis*> TrimAxes;
 139   unsigned int current_axis;
 140   int N, Nsub;
 141   TrimMode mode;
 142   int DebugLevel, Debug;
 143   double Tolerance, A_Tolerance;
 144   double wdot,udot,qdot;
 145   double dth;
 146   double *sub_iterations;
 147   double *successful;
 148   bool *solution;
 149   int max_sub_iterations;
 150   int max_iterations;
 151   int total_its;
 152   bool trimudot;
 153   bool gamma_fallback;
 154   bool trim_failed;
 155   unsigned int axis_count;
 156   int solutionDomain;
 157   double xlo,xhi,alo,ahi;
 158   double targetNlf;
 159   int debug_axis;
 160
 161   double psidot,thetadot;
 162
 163   FGFDMExec* fdmex;
 164   FGInitialCondition* fgic;
 165
 166   bool solve(void);
 167
 168   /** @return false if there is no change in the current axis accel
 169       between accel(control_min) and accel(control_max). If there is a
 170       change, sets solutionDomain to:
 171       0 for no sign change,
 172      -1 if sign change between accel(control_min) and accel(0)
 173       1 if sign between accel(0) and accel(control_max)
 174   */
 175   bool findInterval(void);
 176
 177   bool checkLimits(void);
 178
 179   void setupPullup(void);
 180   void setupTurn(void);
 181
 182   void updateRates(void);
 183
 184   void setDebug(void);
 185
 186 public:
 187   /** Initializes the trimming class
 188       @param FDMExec pointer to a JSBSim executive object.
 189       @param tm trim mode
 190   */
 191   FGTrim(FGFDMExec *FDMExec, TrimMode tm=tGround );
 192
 193   ~FGTrim(void);
 194
 195   /** Execute the trim
 196   */
 197   bool DoTrim(void);
 198
 199   /** Print the results of the trim. For each axis trimmed, this
 200       includes the final state value, control value, and tolerance
 201       used.
 202       @return true if trim succeeds
 203   */
 204   void Report(void);
 205
 206   /** Iteration statistics
 207   */
 208   void TrimStats();
 209
 210   /** Clear all state-control pairs and set a predefined trim mode
 211       @param tm the set of axes to trim. Can be:
 212              tLongitudinal, tFull, tGround, tCustom, or tNone
 213   */
 214   void SetMode(TrimMode tm);
 215
 216   /** Clear all state-control pairs from the current configuration.
 217       The trimming routine must have at least one state-control pair
 218       configured to be useful
 219   */
 220   void ClearStates(void);
 221
 222   /** Add a state-control pair to the current configuration. See the enums
 223       State and Control in FGTrimAxis.h for the available options.
 224       Will fail if the given state is already configured.
 225       @param state the accel or other condition to zero
 226       @param control the control used to zero the state
 227       @return true if add is successful
 228   */
 229   bool AddState( State state, Control control );
 230
 231   /** Remove a specific state-control pair from the current configuration
 232       @param state the state to remove
 233       @return true if removal is successful
 234   */
 235   bool RemoveState( State state );
 236
 237   /** Change the control used to zero a state previously configured
 238       @param state the accel or other condition to zero
 239       @param new_control the control used to zero the state
 240   */
 241   bool EditState( State state, Control new_control );
 242
 243   /** automatically switch to trimming longitudinal acceleration with
 244       flight path angle (gamma) once it becomes apparent that there
 245       is not enough/too much thrust.
 246       @param bb true to enable fallback
 247   */
 248   inline void SetGammaFallback(bool bb) { gamma_fallback=bb; }
 249
 250   /** query the fallback state
 251       @return true if fallback is enabled.
 252   */
 253   inline bool GetGammaFallback(void) { return gamma_fallback; }
 254
 255   /** Set the iteration limit. DoTrim() will return false if limit
 256       iterations are reached before trim is achieved.  The default
 257       is 60.  This does not ordinarily need to be changed.
 258       @param ii integer iteration limit
 259   */
 260   inline void SetMaxCycles(int ii) { max_iterations = ii; }
 261
 262   /** Set the per-axis iteration limit.  Attempt to zero each state
 263       by iterating limit times before moving on to the next. The
 264       default limit is 100 and also does not ordinarily need to
 265       be changed.
 266       @param ii integer iteration limit
 267   */
 268   inline void SetMaxCyclesPerAxis(int ii) { max_sub_iterations = ii; }
 269
 270   /** Set the tolerance for declaring a state trimmed. Angular accels are
 271       held to a tolerance of 1/10th of the given.  The default is
 272       0.001 for the recti-linear accelerations and 0.0001 for the angular.
 273   */
 274   inline void SetTolerance(double tt) {
 275     Tolerance = tt;
 276     A_Tolerance = tt / 10;
 277   }
 278
 279   /**
 280     Debug level 1 shows results of each top-level iteration
 281     Debug level 2 shows level 1 & results of each per-axis iteration
 282   */
 283   inline void SetDebug(int level) { DebugLevel = level; }
 284   inline void ClearDebug(void) { DebugLevel = 0; }
 285
 286   /**
 287     Output debug data for one of the axes
 288     The State enum is defined in FGTrimAxis.h
 289   */
 290   inline void DebugState(State state) { debug_axis=state; }
 291
 292   inline void SetTargetNlf(double nlf) { targetNlf=nlf; }
 293   inline double GetTargetNlf(void) { return targetNlf; }
 294
 295 };
 296 }
 297
 298 #endif