/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
+
Header: FGTrim.cpp
Author: Tony Peden
Date started: 9/8/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
+ 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 General Public License for more
+ FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
details.
-
- You should have received a copy of the GNU General Public License along with
+
+ 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 General Public License can also be found on
+
+ 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
INCLUDES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
-#include <stdlib.h>
-
-#include <FGFDMExec.h>
-#include <models/FGAtmosphere.h>
-#include "FGInitialCondition.h"
+#include <cstdlib>
+#include <iomanip>
#include "FGTrim.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/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 "models/FGPropulsion.h"
+#include "models/propulsion/FGEngine.h"
+#include "math/FGColumnVector3.h"
#if _MSC_VER
#pragma warning (disable : 4786 4788)
#endif
+using namespace std;
+
namespace JSBSim {
-static const char *IdSrc = "$Id$";
+static const char *IdSrc = "$Id: FGTrim.cpp,v 1.13 2010/04/23 17:23:40 dpculp Exp $";
static const char *IdHdr = ID_TRIM;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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=true;
+ gamma_fallback=false;
axis_count=0;
mode=tt;
xlo=xhi=alo=ahi=0.0;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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) {
+ 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;
+ for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
+ run_sum += TrimAxes[current_axis]->GetRunCount();
+ cout << " " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
+ << ": " << setprecision(3) << sub_iterations[current_axis]
+ << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
+ << " successful: " << setprecision(3) << successful[current_axis]
+ << " stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
+ << endl;
}
cout << " Run Count: " << run_sum << endl;
}
void FGTrim::ClearStates(void) {
FGTrimAxis* ta;
-
+
mode=tCustom;
vector<FGTrimAxis*>::iterator iAxes;
iAxes = TrimAxes.begin();
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()) {
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);
+ iAxes = TrimAxes.erase(iAxes);
result=true;
continue;
}
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 ){
+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()) {
iAxes++;
}
return result;
-}
-
+}
+
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGTrim::DoTrim(void) {
-
+
trim_failed=false;
int i;
}
fdmex->DisableOutput();
-
+
+ setEngineTrimMode(true);
+
fgic->SetPRadpsIC(0.0);
fgic->SetQRadpsIC(0.0);
fgic->SetRRadpsIC(0.0);
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);
successful[current_axis]=0;
solution[current_axis]=false;
}
-
-
+
+
if(mode == tPullup ) {
cout << "Setting pitch rate and nlf... " << endl;
setupPullup();
} else if (mode == tTurn) {
setupTurn();
//TrimAxes[0]->SetStateTarget(targetNlf);
- }
-
+ }
+
do {
axis_count=0;
for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
updateRates();
Nsub=0;
if(!solution[current_axis]) {
- if(checkLimits()) {
+ 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;
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
+ setEngineTrimMode(false);
fdmex->EnableOutput();
return !trim_failed;
}
}
//cout << i << endl;
-
+
}//end while
if(Nsub < max_sub_iterations) success=true;
- }
+ }
return success;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
/*
- produces an interval (xlo..xhi) on one side or the other of the current
- control value in which a solution exists. This domain is, hopefully,
- smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
- to find the solution. This is, hopefully, more efficient than having the
+ produces an interval (xlo..xhi) on one side or the other of the current
+ control value in which a solution exists. This domain is, hopefully,
+ smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
+ to find the solution. This is, hopefully, more efficient than having the
solver start from scratch every time. Maybe it isn't though...
This tries to take advantage of the idea that the changes from iteration to
iteration will be small after the first one or two top-level iterations.
assumes that changing the control will a produce significant change in the
accel i.e. checkLimits() has already been called.
- if a solution is found above the current control, the function returns true
- and xlo is set to the current control, xhi to the interval max it found, and
+ if a solution is found above the current control, the function returns true
+ and xlo is set to the current control, xhi to the interval max it found, and
solutionDomain is set to 1.
- if the solution lies below the current control, then the function returns
- true and xlo is set to the interval min it found and xmax to the current
+ if the solution lies below the current control, then the function returns
+ true and xlo is set to the interval min it found and xmax to the current
control. if no solution is found, then the function returns false.
-
-
+
+
in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
- no assumptions about the state of the sim after this function has run
+ no assumptions about the state of the sim after this function has run
can be made.
*/
bool FGTrim::findInterval(void) {
double xmin=TrimAxes[current_axis]->GetControlMin();
double xmax=TrimAxes[current_axis]->GetControlMax();
double lastxlo,lastxhi,lastalo,lastahi;
-
+
step=0.025*fabs(xmax);
xlo=xhi=current_control;
alo=ahi=current_accel;
lastxlo=xlo;lastxhi=xhi;
lastalo=alo;lastahi=ahi;
do {
-
+
Nsub++;
step*=2;
xlo-=step;
alo=lastahi;
//xlo=current_control;
//alo=current_accel;
- }
+ }
}
lastxlo=xlo;lastxhi=xhi;
lastalo=alo;lastahi=ahi;
// 1 if solution is between the current and max
// -1 if solution is between the min and current
// 0 if there is no solution
-//
+//
//if changing the control produces no significant change in the accel then
//solutionDomain is set to zero and the function returns false
//if a solution is found, then xlo and xhi are set so that they bracket
//the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
//if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
-//xhi=xmax and ahi=accel(xmax)
+//xhi=xmax and ahi=accel(xmax)
//in all cases the sim is left such that the control=xmax and accel=ahi
bool FGTrim::checkLimits(void) {
solutionExists=true;
solutionDomain=1;
xlo=current_control;
- alo=current_accel;
+ alo=current_accel;
}
- }
+ }
TrimAxes[current_axis]->SetControl(current_control);
TrimAxes[current_axis]->Run();
return solutionExists;
cout << targetNlf << ", " << q << endl;
fgic->SetQRadpsIC(q);
cout << "setPitchRateInPullup() complete" << endl;
-
-}
+
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::setupTurn(void){
double g,phi;
- phi = fgic->GetRollAngleRadIC();
+ phi = fgic->GetPhiRadIC();
if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
targetNlf = 1 / cos(phi);
- g = fdmex->GetInertial()->gravity();
+ g = fdmex->GetInertial()->gravity();
psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
cout << targetNlf << ", " << psidot << endl;
}
-
-}
+
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::updateRates(void){
if( mode == tTurn ) {
- double phi = fgic->GetRollAngleRadIC();
- double g = fdmex->GetInertial()->gravity();
+ double phi = fgic->GetPhiRadIC();
+ double g = fdmex->GetInertial()->gravity();
double p,q,r,theta;
if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
- theta=fgic->GetPitchAngleRadIC();
- phi=fgic->GetRollAngleRadIC();
+ theta=fgic->GetThetaRadIC();
+ phi=fgic->GetPhiRadIC();
psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
p=-psidot*sin(theta);
q=psidot*cos(theta)*sin(phi);
r=psidot*cos(theta)*cos(phi);
} else {
p=q=r=0;
- }
+ }
fgic->SetPRadpsIC(p);
fgic->SetQRadpsIC(q);
fgic->SetRRadpsIC(r);
cgamma=cos(fgic->GetFlightPathAngleRadIC());
q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
fgic->SetQRadpsIC(q);
- }
-}
+ }
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGTrim::setDebug(void) {
if(debug_axis == tAll ||
TrimAxes[current_axis]->GetStateType() == debug_axis ) {
- Debug=DebugLevel;
+ Debug=DebugLevel;
return;
} else {
Debug=0;
return;
}
-}
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::setEngineTrimMode(bool mode) {
+ FGPropulsion* prop = fdmex->GetPropulsion();
+ for (unsigned int i=0; i<prop->GetNumEngines(); i++) {
+ prop->GetEngine(i)->SetTrimMode(mode);
+ }
+}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
mode=tt;
switch(tt) {
case tFull:
- if (debug_lvl > 0)
+ if (debug_lvl > 0)
cout << " Full Trim" << endl;
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
+ //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
break;
case tLongitudinal:
- if (debug_lvl > 0)
+ if (debug_lvl > 0)
cout << " Longitudinal Trim" << endl;
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
break;
case tGround:
- if (debug_lvl > 0)
+ if (debug_lvl > 0)
cout << " Ground Trim" << endl;
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
successful=new double[TrimAxes.size()];
solution=new bool[TrimAxes.size()];
current_axis=0;
-}
+}
//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
}
projects / flightgear.git / blobdiff