flightgear/src/FDM/JSBSim/FGTrim.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
 Header:       FGTrim.h
 Author:       Tony Peden
 Date started: 7/1/99
 
 ------------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) -------------
 
 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU General Public License as published by the Free Software
 Foundation; either version 2 of the License, or (at your option) any later
 version.
 
 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the GNU General Public License for more
 details.
 
 You should have received a copy of the GNU General Public License along with
 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
 Place - Suite 330, Boston, MA  02111-1307, USA.
 
 Further information about the GNU General Public License can also be found on
 the world wide web at http://www.gnu.org.
 
 
 HISTORY
--------------------------------------------------------------------------------
9/8/99   TP   Created
 
 
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
 
This class takes the given set of IC's and finds the angle of attack, elevator,
and throttle setting required to fly steady level. This is currently for in-air
conditions only.  It is implemented using an iterative, one-axis-at-a-time 
scheme.  
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGTRIM_H
#define FGTRIM_H

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include "FGFDMExec.h"
#include "FGJSBBase.h"
#include "FGRotation.h"
#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFCS.h"
#include "FGAircraft.h"
#include "FGTranslation.h"
#include "FGPosition.h"
#include "FGAuxiliary.h"
#include "FGOutput.h"
#include "FGTrim.h"
#include "FGTrimAxis.h"

#include <vector>

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#define ID_TRIM "$Id$"

typedef enum { tLongitudinal, tFull, tGround, tCustom, tNone } TrimMode;

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
FORWARD DECLARATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DOCUMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

/** FGTrim -- the trimming routine for JSBSim.
    FGTrim finds the aircraft attitude and control settings needed to maintain
    the steady state described by the FGInitialCondition object .  It does this
    iteratively by assigning a control to each state and adjusting that control
    until the state is within a specified tolerance of zero. States include the
    recti-linear accelerations udot, vdot, and wdot, the angular accelerations 
    qdot, pdot, and rdot, and the difference between heading and ground track.
    Controls include the usual flight deck controls available to the pilot plus
    angle of attack (alpha), sideslip angle(beta), flight path angle (gamma), 
    pitch attitude(theta), roll attitude(phi), and altitude above ground.  The
    last three are used for on-ground trimming. The state-control pairs used in
    a given trim are completely user configurable and several pre-defined modes
    are provided as well. They are:
    <ul>
    <li> tLongitudinal: Trim wdot with alpha, udot with thrust, qdot with elevator</li>
    <li> tFull: tLongitudinal + vdot with phi, pdot with aileron, rdot with rudder
                and heading minus ground track (hmgt) with beta</li>
    <li> tGround: wdot with altitude, qdot with theta, and pdot with phi</li>
    The remaining modes include <b>tCustom</b>, which is completely user defined and
    <b>tNone</b>.
    </ul>
    Currently, this class cannot trim a non-1g condition and is limited to 
    trimming for constant true airspeed in climbs and descents.
    
    Note that trims can (and do) fail for reasons that are completely outside
    the control of the trimming routine itself. The most common problem is the 
    initial conditions: is the model capable of steady state flight
    at those conditions?  Check the speed, altitude, configuration (flaps,
    gear, etc.), weight, cg, and anything else that may be relevant.
    
    Example usage:
    FGFDMExec* FDMExec = new FGFDMExec();
    .
    .
    .
    FGInitialCondition* fgic = new FGInitialCondition(FDMExec);
    FGTrim *fgt(FDMExec,fgic,tFull);
    fgic->SetVcaibratedKtsIC(100);
    fgic->SetAltitudeFtIC(1000);
    fgic->SetClimbRate(500);
    if( !fgt->DoTrim() ) {
      cout << "Trim Failed" << endl;
    }
    fgt->ReportState();  
    @author Tony Peden
    @version $Id$
*/       
  
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGTrim : public FGJSBBase
{
private:

  vector<FGTrimAxis*> TrimAxes;
  int current_axis;
  int N, Nsub;
  TrimMode mode;
  int Debug;
  float Tolerance, A_Tolerance;
  float wdot,udot,qdot;
  float dth;
  float *sub_iterations;
  float *successful;
  bool *solution;
  int max_sub_iterations;
  int max_iterations;
  int total_its;
  bool trimudot;
  bool gamma_fallback;
  bool trim_failed;
  int axis_count;
  int solutionDomain;
  float xlo,xhi,alo,ahi;

  FGFDMExec* fdmex;
  FGInitialCondition* fgic;
   
  bool solve(void);
  
  /** @return false if there is no change in the current axis accel
      between accel(control_min) and accel(control_max). If there is a
      change, sets solutionDomain to:
      0 for no sign change,
     -1 if sign change between accel(control_min) and accel(0)
      1 if sign between accel(0) and accel(control_max)
  */
  bool findInterval(void);

  bool checkLimits(void);

public:
  /** Initializes the trimming class
      @param FDMExec pointer to a JSBSim executive object.
      @param FGIC pointer to a FGInitialCondition object
      @param TrimMode the set of axes to trim. Can be:
             tLongitudinal, tFull, tGround, tCustom, or tNone
  */
  FGTrim(FGFDMExec *FDMExec, FGInitialCondition *FGIC, TrimMode tt);

    ~FGTrim(void);

  /** Execute the trim
  */
  bool DoTrim(void);

  /** Print the results of the trim. For each axis trimmed, this 
      includes the final state value, control value, and tolerance
      used.
      @return true if trim succeeds
  */     
  void Report(void);
  
  /** Prints a summary of simulator state (speed, altitude, 
      configuration, etc.)
  */
  void ReportState(void);
  
  /** Iteration statistics
  */
  void TrimStats();

  /** Clear all state-control pairs from the current configuration.
      The trimming routine must have at least one state-control pair
      configured to be useful
  */    
  void ClearStates(void);

  /** Add a state-control pair to the current configuration. See the enums
      State and Control in FGTrimAxis.h for the available options.
      Will fail if the given state is already configured.
      @param state the accel or other condition to zero 
      @param control the control used to zero the state
      @return true if add is successful
  */    
  bool AddState( State state, Control control );
  
  /** Remove a specific state-control pair from the current configuration
      @param state the state to remove
      @return true if removal is successful
  */    
  bool RemoveState( State state );
  
  /** Change the control used to zero a state previously configured
      @param state the accel or other condition to zero 
      @param control the control used to zero the state
  */
  bool EditState( State state, Control new_control );

  /** automatically switch to trimming longitudinal acceleration with
      flight path angle (gamma) once it becomes apparent that there
      is not enough/too much thrust.
      @param gamma_fallback true to enable fallback
  */     
  inline void SetGammaFallback(bool bb) { gamma_fallback=true; }
  
  /** query the fallback state
      @return true if fallback is enabled.
  */
  inline bool GetGammaFallback(void) { return gamma_fallback; }

  /** Set the iteration limit. DoTrim() will return false if limit
      iterations are reached before trim is achieved.  The default
      is 60.  This does not ordinarily need to be changed.
      @param ii integer iteration limit 
  */
  inline void SetMaxCycles(int ii) { max_iterations = ii; }
  
  /** Set the per-axis iteration limit.  Attempt to zero each state
      by iterating limit times before moving on to the next. The
      default limit is 100 and also does not ordinarily need to
      be changed.
      @param ii integer iteration limit 
  */    
  inline void SetMaxCyclesPerAxis(int ii) { max_sub_iterations = ii; }
  
  /** Set the tolerance for declaring a state trimmed. Angular accels are
      held to a tolerance of 1/10th of the given.  The default is 
      0.001 for the recti-linear accelerations and 0.0001 for the angular.
  */         
  inline void SetTolerance(float tt) {
    Tolerance = tt;
    A_Tolerance = tt / 10;
  }
  
  //Debug level 1 shows results of each top-level iteration
  //Debug level 2 shows level 1 & results of each per-axis iteration
  inline void SetDebug(int level) { Debug = level; }
  inline void ClearDebug(void) { Debug = 0; }

};


#endif