fgtrim.cpp

6 DOF Missle Simulation

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

 Header:       FGTrim.cpp
 Author:       Tony Peden
 Date started: 9/8/99

 --------- Copyright (C) 1999  Anthony K. Peden ([e-mail]) ---------

 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
--------------------------------------------------------------------------------
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. */

//  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!

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

#include <cstdlib>
#include "FGTrim.h"
#include <models/FGAtmosphere.h>
#include "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>

#if _MSC_VER
#pragma warning (disable : 4786 4788)
#endif

namespace JSBSim {

static const char *IdSrc = "$Id$";
static const char *IdHdr = ID_TRIM;

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {

  N=Nsub=0;
  max_iterations=60;
  max_sub_iterations=100;
  Tolerance=1E-3;
  A_Tolerance = Tolerance / 10;

  Debug=0;DebugLevel=0;
  fdmex=FDMExec;
  fgic=fdmex->GetIC();
  total_its=0;
  trimudot=true;
  gamma_fallback=false;
  axis_count=0;
  mode=tt;
  xlo=xhi=alo=ahi=0.0;
  targetNlf=1.0;
  debug_axis=tAll;
  SetMode(tt);
  if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

FGTrim::~FGTrim(void) {
  for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
    delete TrimAxes[current_axis];
  }
  delete[] sub_iterations;
  delete[] successful;
  delete[] solution;
  if (debug_lvl & 2) cout << "Destroyed:    FGTrim" << endl;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGTrim::TrimStats() {
  char out[80];
  int run_sum=0;
  cout << endl << "  Trim Statistics: " << endl;
  cout << "    Total Iterations: " << total_its << endl;
  if(total_its > 0) {
    cout << "    Sub-iterations:" << endl;
    for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
      run_sum+=TrimAxes[current_axis]->GetRunCount();
      snprintf(out,80,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
                  TrimAxes[current_axis]->GetStateName().c_str(),
                  sub_iterations[current_axis],
                  sub_iterations[current_axis]/double(total_its),
                  successful[current_axis],
                  TrimAxes[current_axis]->GetAvgStability() );
      cout << out;
    }
    cout << "    Run Count: " << run_sum << endl;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGTrim::Report(void) {
  cout << "  Trim Results: " << endl;
  for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
    TrimAxes[current_axis]->AxisReport();

}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGTrim::ClearStates(void) {
    FGTrimAxis* ta;

    mode=tCustom;
    vector<FGTrimAxis*>::iterator iAxes;
    iAxes = TrimAxes.begin();
    while (iAxes != TrimAxes.end()) {
      ta=*iAxes;
      delete ta;
      iAxes++;
    }
    TrimAxes.clear();
    //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGTrim::AddState( State state, Control control ) {
  FGTrimAxis* ta;
  bool result=true;

  mode = tCustom;
  vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
  while (iAxes != TrimAxes.end()) {
      ta=*iAxes;
      if( ta->GetStateType() == state )
        result=false;
      iAxes++;
  }
  if(result) {
    TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
    delete[] sub_iterations;
    delete[] successful;
    delete[] solution;
    sub_iterations=new double[TrimAxes.size()];
    successful=new double[TrimAxes.size()];
    solution=new bool[TrimAxes.size()];
  }
  return result;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGTrim::RemoveState( State state ) {
  FGTrimAxis* ta;
  bool result=false;

  mode = tCustom;
  vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
  while (iAxes != TrimAxes.end()) {
      ta=*iAxes;
      if( ta->GetStateType() == state ) {
        delete ta;
        TrimAxes.erase(iAxes);
        result=true;
        continue;
      }
      iAxes++;
  }
  if(result) {
    delete[] sub_iterations;
    delete[] successful;
    delete[] solution;
    sub_iterations=new double[TrimAxes.size()];
    successful=new double[TrimAxes.size()];
    solution=new bool[TrimAxes.size()];
  }
  return result;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGTrim::EditState( State state, Control new_control ){
  FGTrimAxis* ta;
  bool result=false;

  mode = tCustom;
  vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
  while (iAxes != TrimAxes.end()) {
      ta=*iAxes;
      if( ta->GetStateType() == state ) {
        TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
        delete ta;
        TrimAxes.erase(iAxes+1);
        result=true;
        break;
      }
      iAxes++;
  }
  return result;
}


//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

bool FGTrim::DoTrim(void) {

  trim_failed=false;
  int i;

  for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
    fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
  }

  fdmex->DisableOutput();

  fgic->SetPRadpsIC(0.0);
  fgic->SetQRadpsIC(0.0);
  fgic->SetRRadpsIC(0.0);

  //clear the sub iterations counts & zero out the controls
  for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
    //cout << current_axis << "  " << TrimAxes[current_axis]->GetStateName()
    //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
    if(TrimAxes[current_axis]->GetStateType() == tQdot) {
      if(mode == tGround) {
    	  TrimAxes[current_axis]->initTheta();
      }
    }
    xlo=TrimAxes[current_axis]->GetControlMin();
    xhi=TrimAxes[current_axis]->GetControlMax();
    TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
    TrimAxes[current_axis]->Run();
    //TrimAxes[current_axis]->AxisReport();
    sub_iterations[current_axis]=0;
    successful[current_axis]=0;
    solution[current_axis]=false;
  }


  if(mode == tPullup ) {
    cout << "Setting pitch rate and nlf... " << endl;
    setupPullup();
    cout << "pitch rate done ... " << endl;
    TrimAxes[0]->SetStateTarget(targetNlf);
    cout << "nlf done" << endl;
  } else if (mode == tTurn) {
    setupTurn();
    //TrimAxes[0]->SetStateTarget(targetNlf);
  }

  do {
    axis_count=0;
    for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
      setDebug();
      updateRates();
      Nsub=0;
      if(!solution[current_axis]) {
        if(checkLimits()) {
          solution[current_axis]=true;
          solve();
        }
      } else if(findInterval()) {
        solve();
      } else {
        solution[current_axis]=false;
      }
      sub_iterations[current_axis]+=Nsub;
    }
    for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
      //these checks need to be done after all the axes have run
      if(Debug > 0) TrimAxes[current_axis]->AxisReport();
      if(TrimAxes[current_axis]->InTolerance()) {
        axis_count++;
        successful[current_axis]++;
      }
    }


    if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
      //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
      //At this point we can check the input limits of the failed axis
      //and declare the trim failed if there is no sign change. If there
      //is, keep going until success or max iteration count

      //Oh, well: two out of three ain't bad
      for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
        //these checks need to be done after all the axes have run
        if(!TrimAxes[current_axis]->InTolerance()) {
          if(!checkLimits()) {
            // special case this for now -- if other cases arise proper
            // support can be added to FGTrimAxis
            if( (gamma_fallback) &&
                (TrimAxes[current_axis]->GetStateType() == tUdot) &&
                (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
              cout << "  Can't trim udot with throttle, trying flight"
              << " path angle. (" << N << ")" << endl;
              if(TrimAxes[current_axis]->GetState() > 0)
                TrimAxes[current_axis]->SetControlToMin();
              else
                TrimAxes[current_axis]->SetControlToMax();
              TrimAxes[current_axis]->Run();
              delete TrimAxes[current_axis];
              TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
                                                    tGamma );
            } else {
              cout << "  Sorry, " << TrimAxes[current_axis]->GetStateName()
              << " doesn't appear to be trimmable" << endl;
              //total_its=k;
              trim_failed=true; //force the trim to fail
            } //gamma_fallback
          }
        } //solution check
      } //for loop
    } //all-but-one check
    N++;