FGTrimAnalysis.h

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

 Header:       FGTrimAnalysis.h
 Author:       Agostino De Marco
 Date started: Dec/14/2006

 ------------- Copyright (C) 2006  Agostino De Marco (agodemar@unina.it) -------

 This program is free software; you can redistribute it and/or modify it under
 the terms of the GNU Lesser 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 Lesser General Public License for more
 details.

 You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
 the world wide web at http://www.gnu.org.


 HISTORY
--------------------------------------------------------------------------------
12/14/06   ADM   Created


FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------

This class takes the given set of IC's and analyzes the possible trim states
of the aircraft, i.e. finds the aircraft state required to
maintain a specified flight condition.  This flight condition can be
steady-level, a steady turn, a pull-up or pushover.
It is implemented using an iterative, direct search of a cost function minimum.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SENTRY
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#ifndef FGTRIMANAlYSIS_H
#define FGTRIMANAlYSIS_H

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

#include "FGFDMExec.h"
#include "FGJSBBase.h"
#include "FGTrimAnalysisControl.h"

#include "models/FGAtmosphere.h"
#include "initialization/FGInitialCondition.h"
#include "models/FGAircraft.h"
#include "models/FGMassBalance.h"
#include "models/FGGroundReactions.h"
#include "models/FGInertial.h"
#include "models/FGAerodynamics.h"
#include "math/FGColumnVector3.h"
#include "models/FGAuxiliary.h"
#include "models/FGPropulsion.h"

#include <vector>
#include <map>
#include <string>

#include "math/direct_search/vec.h"

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

#define ID_FGTRIMANALYSIS "$Id: FGTrimAnalysis.h,v 1.8 2009/10/02 10:30:09 jberndt Exp $"

#if defined(_WIN32) && !defined(__CYGWIN__)
  #define snprintf _snprintf
#endif

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

namespace JSBSim {

typedef enum { taLongitudinal=0, taFull, taFullWingsLevel, taTurn, taPullup, taTurnFull,
                taGround, taCustom, taNone } TrimAnalysisMode;

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

/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

class FGTrimAnalysis;

class Objective {
public:
    Objective(FGFDMExec* fdmex, FGTrimAnalysis* ta, double x);
    ~Objective() {}

    void CostFunctionFull                (long vars, Vector<double> &v, double & f);
    void CostFunctionFullWingsLevel      (long vars, Vector<double> &v, double & f);
    void CostFunctionLongitudinal        (long vars, Vector<double> &v, double & f);
    void CostFunctionFullTurn (long vars, Vector<double> &v, double & f);
    void CostFunctionFullCoordinatedTurn (long vars, Vector<double> &v, double & f);

    void CostFunctionPullUp              (long vars, Vector<double> &v, double & f);

    friend void find_CostFunctionFull(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    friend void find_CostFunctionFullWingsLevel(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    friend void find_CostFunctionLongitudinal(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    friend void find_CostFunctionFullCoordinatedTurn(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    friend void find_CostFunctionFullTurn(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    friend void find_CostFunctionPullUp(long vars, Vector<double> &v, double & f,
                    bool & success, void* t_ptr);
    /* function definiation moved outside of class declaration */

    void Set_x_val(double new_x);
    double Get_x_val() const {return _x;}

    typedef void (*PF)(long vars, Vector<double> &v, double & f, bool & success, void* t_ptr);

    map<TrimAnalysisMode,PF> mpCostFunctions; // primordial...

private:
    void calculateDottedStates(double delta_cmd_T, double delta_cmd_E, double delta_cmd_A, double delta_cmd_R,
                               double phi, double theta, double psi,
                               TrimAnalysisMode trimMode,
                               double& alpha, double& beta, double& gamma,
                               //FGColumnVector3& vUVW, FGColumnVector3& vPQR,
                               FGPropagate::VehicleState& VState,
                               FGColumnVector3& vUVWdot, FGColumnVector3& vPQRdot );

    double myCostFunctionFull                  (Vector<double> & vec1);
    double myCostFunctionFullWingsLevel        (Vector<double> & vec1);
    double myCostFunctionLongitudinal          (Vector<double> & vec1);
    double myCostFunctionFullCoordinatedTurn   (Vector<double> & vec1);
    double myCostFunctionFullTurn              (Vector<double> & vec1);
    double myCostFunctionPullUp                (Vector<double> & vec1);

    double _x;
    FGFDMExec* FDMExec;
    FGTrimAnalysis* TrimAnalysis;
};


class FGTrimAnalysis : public FGJSBBase, public FGXMLFileRead
{
private:
  vector<FGTrimAnalysisControl*> vTrimAnalysisControls;
  double cost_function_value;
  unsigned int current_ctrl;
  int N, Nsub;
  TrimAnalysisMode mode;
  int DebugLevel, Debug;

  double dth;
  bool trimudot;
  bool gamma_fallback;

  bool trim_failed;
  unsigned int ctrl_count;

  FGFDMExec* fdmex;
  FGInitialCondition * fgic; // IC;

  FGAuxiliary *        Auxiliary;
  FGAerodynamics *     Aerodynamics;
  FGPropulsion *       Propulsion;
  FGFCS *              FCS;

  vector<double> vAlphaDeg, vAlphaTrimDeg; // DEG !!!
  vector<double> vCL, vCD, vCm;
  vector<double> vCLE, vCDE, vCmE;
  vector<vector <double> > mCL, mCD, mCm, mTa;
  vector<double> vThrottleCmd, vElevatorCmd;
  vector<double> vVn, vTn, vTa, vVnE, vTE;
  vector<double> vCt, vCq, vRPM;
  vector<vector <double> > mCt, mCq, mRPM;
  vector<bool> hasThrustTrim, hasCmTrim;

  string trim_id;

  // direct search stuff
  string search_type;
  double sigma_nm, alpha_nm, beta_nm, gamma_nm;
  double initial_step;
  double tolerance;
  string stop_criterion;
  int max_iterations;
  int total_its;

  // results file
  mutable ofstream rf; // see Stroustrup 10.2.7.2; alternative: use a cache struct
  string rf_name;

  double _u,_v,_w;
  double _p,_q,_r;
  //double uw,vw,ww;
  //double vnorth,veast,vdown;
  //double wnorth,weast,wdown;
  double _alpha, _beta, _theta, _phi, _psi, _psiW, _gamma, _phiW;

  double _stheta, _sphi, _spsi;
  double _ctheta, _cphi, _cpsi;

  double _vtIC, _hIC, _gammaIC, _rocIC, _vdownIC, _psiIC, _psigtIC, _vgIC,
         _vnorthIC, _veastIC, wnorthIC, _weastIC, _wdownIC;

  double _udot, _vdot, _wdot, _pdot, _qdot, _rdot;
  double _targetNlf;
  double _psiWdot, _phiWdot, _gammadot, _psidot, _thetadot;

  double C1, C2, C3, _calpha, _salpha, _cbeta, _sbeta;

  void setupPullup(void);
  void setupTurn(void);
  void setupTurn(double phiW); // recalculate target Nlf
  void setupTurnPhi(double psi, double theta); // recalculate only phi

  void updateRates(void);

  void setDebug(void);

  bool ensureRunning(void);
  bool ensureRunning(unsigned int i);
  bool runForAWhile(int nruns);

  bool populateVecAlphaDeg(double vmin, double vmax, int n);
  bool populateVecThrottleCmd(double vmin, double vmax, int n);
  bool populateVecElevatorCmd(double vmin, double vmax, int n);

  bool calculateAerodynamics(
                           double dE_cmd,
                           double Vt,
                           double alpha_deg,
                           double altitude,
                           double rho,
                           double S, double mac, double bw,
                           double& CL, double& CD, double& Cm);


  bool getSteadyState(int nrepeat);
  bool InitializeTrimControl(double default_value, Element* el,
                                           string unit, TaControl type);

public:
  FGTrimAnalysis(FGFDMExec *FDMExec, TrimAnalysisMode tam=taFull );

  ~FGTrimAnalysis(void);

  friend class Objective;

  bool Load(string fname, bool useStoredPath = true );

  bool DoTrim(void);

  void Report(void);

  void TrimStats();

  bool SetResultsFile(string name);

  ofstream* GetResultsFile() const { if (rf.is_open()) return &rf; else return 0; } // const_cast<ofstream*>(&rf) (if rf is not mutable)

  inline void SetCostFunctionValue(double value){cost_function_value = value;}

  inline double GetCostFunctionValue() const { return cost_function_value;}

  void SetMode(TrimAnalysisMode tam);

  inline TrimAnalysisMode GetMode() const { return mode;};

  inline vector<FGTrimAnalysisControl*>* GetControls(){return &vTrimAnalysisControls;}

  void ClearControls(void);

  bool AddControl( TaControl control );

  bool RemoveControl( TaControl control );

  bool EditState( TaControl new_control, double new_initvalue, double new_step, double new_min, double new_max );


  inline double GetGamma() { return _gamma; }

  inline void SetGammaFallback(bool bb) { gamma_fallback=bb; }

  inline bool GetGammaFallback(void) { return gamma_fallback; }

  inline void SetMaxCycles(int ii) { max_iterations = ii; }

  inline void SetTolerance(double tt) {
    tolerance = tt;
  }
  inline double GetTolerance(void) {return tolerance; }

  inline void SetTrimFailed(bool tf) { trim_failed = tf; }
  inline bool GetTrimFailed(void) { return trim_failed; }
  inline void SetTrimSuccessfull() { trim_failed = false; }


  void SetState(double u0, double v0, double w0, double p0, double q0, double r0,
                double alpha0, double beta0, double phi0, double theta0, double psi0, double gamma0);

  void SetEulerAngles(double phi0, double theta0, double psi0);

  double GetPhiRad  (){ return _phi;}
  double GetThetaRad(){ return _theta;}
  double GetPsiRad  (){ return _psi;}

  double GetPhiWRad  (){ return _phiW;}
  double GetGammaRad (){ return _gamma;}
  double GetVtFps    (){ return _vtIC;}

  void CalculatePhiWFromTargetNlfTurn(double nlf);

  FGColumnVector3 UpdateRatesTurn(double psi, double theta, double phi, double phiW);

  FGColumnVector3 UpdateRatesPullup(void);

  void SetDottedValues(double udot, double vdot, double wdot, double pdot, double qdot, double rdot);

  inline void SetDebug(int level) { DebugLevel = level; }
  inline void ClearDebug(void) { DebugLevel = 0; }

  inline void SetTargetNlf(double nlf) { _targetNlf=nlf; }

  inline double GetTargetNlf(void) { return _targetNlf; }

  //void eom_costf (long vars, Vector<double> &x, double & f, bool& flag, void* an_obj);
  //inline FGFDMExec* GetFDMExec() const { return fdmex; }

};
}

#endif