#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGAerodynamics.h"
+#include "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,FGInitialCondition *FGIC, TrimMode tt ) {
+FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
N=Nsub=0;
max_iterations=60;
Tolerance=1E-3;
A_Tolerance = Tolerance / 10;
- Debug=0;
+ Debug=0;DebugLevel=0;
fdmex=FDMExec;
- fgic=FGIC;
+ fgic=fdmex->GetIC();
total_its=0;
trimudot=true;
gamma_fallback=true;
axis_count=0;
mode=tt;
- xlo=xhi=alo=ahi;
- switch(mode) {
- case tFull:
- 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,tVdot,tPhi ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
- break;
- case tLongitudinal:
- 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:
- cout << " Ground Trim" << endl;
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
- //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
- break;
- }
- //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
- sub_iterations=new float[TrimAxes.size()];
- successful=new float[TrimAxes.size()];
- solution=new bool[TrimAxes.size()];
- current_axis=0;
-
+ xlo=xhi=alo=ahi=0.0;
+ targetNlf=1.0;
+ debug_axis=tAll;
+ SetMode(tt);
if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
}
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]/float(total_its),
+ sub_iterations[current_axis]/double(total_its),
successful[current_axis],
TrimAxes[current_axis]->GetAvgStability() );
cout << out;
iAxes++;
}
TrimAxes.clear();
- cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+ //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
delete[] sub_iterations;
delete[] successful;
delete[] solution;
- sub_iterations=new float[TrimAxes.size()];
- successful=new float[TrimAxes.size()];
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
solution=new bool[TrimAxes.size()];
}
return result;
delete[] sub_iterations;
delete[] successful;
delete[] solution;
- sub_iterations=new float[TrimAxes.size()];
- successful=new float[TrimAxes.size()];
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
solution=new bool[TrimAxes.size()];
}
return result;
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()) {
bool FGTrim::solve(void) {
- float x1,x2,x3,f1,f2,f3,d,d0;
- const float relax =0.9;
- float eps=TrimAxes[current_axis]->GetSolverEps();
+ double x1,x2,x3,f1,f2,f3,d,d0;
+ const double relax =0.9;
+ double eps=TrimAxes[current_axis]->GetSolverEps();
x1=x2=x3=0;
d=1;
*/
bool FGTrim::findInterval(void) {
bool found=false;
- float step;
- float current_control=TrimAxes[current_axis]->GetControl();
- float current_accel=TrimAxes[current_axis]->GetState();;
- float xmin=TrimAxes[current_axis]->GetControlMin();
- float xmax=TrimAxes[current_axis]->GetControlMax();
- float lastxlo,lastxhi,lastalo,lastahi;
+ double step;
+ double current_control=TrimAxes[current_axis]->GetControl();
+ double current_accel=TrimAxes[current_axis]->GetState();;
+ 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;
bool FGTrim::checkLimits(void) {
bool solutionExists;
- float current_control=TrimAxes[current_axis]->GetControl();
- float current_accel=TrimAxes[current_axis]->GetState();
+ double current_control=TrimAxes[current_axis]->GetControl();
+ double current_accel=TrimAxes[current_axis]->GetState();
xlo=TrimAxes[current_axis]->GetControlMin();
xhi=TrimAxes[current_axis]->GetControlMax();
return solutionExists;
}
-//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::setupPullup() {
+ float g,q,cgamma;
+ FGColumnVector3 vPQR;
+ g=fdmex->GetInertial()->gravity();
+ cgamma=cos(fgic->GetFlightPathAngleRadIC());
+ cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
+ << fgic->GetVtrueFpsIC() << endl;
+ q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+ cout << targetNlf << ", " << q << endl;
+ fdmex->GetRotation()->SetPQR(0,q,0);
+ cout << "setPitchRateInPullup() complete" << endl;
+
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::setupTurn(void){
+ double g,phi;
+ phi = fgic->GetRollAngleRadIC();
+ if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+ targetNlf = 1 / cos(phi);
+ 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 p,q,r,theta;
+ if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
+ theta=fgic->GetPitchAngleRadIC();
+ phi=fgic->GetRollAngleRadIC();
+ 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;
+ }
+ fdmex->GetRotation()->SetPQR(p,q,r);
+ } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
+ float g,q,cgamma;
+ FGColumnVector3 vPQR;
+ g=fdmex->GetInertial()->gravity();
+ cgamma=cos(fgic->GetFlightPathAngleRadIC());
+ q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
+ fdmex->GetRotation()->SetPQR(0,q,0);
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::setDebug(void) {
+ if(debug_axis == tAll ||
+ TrimAxes[current_axis]->GetStateType() == debug_axis ) {
+ Debug=DebugLevel;
+ return;
+ } else {
+ Debug=0;
+ return;
+ }
+}
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+void FGTrim::SetMode(TrimMode tt) {
+ ClearStates();
+ switch(tt) {
+ case tFull:
+ 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,tVdot,tPhi ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+ break;
+ case tLongitudinal:
+ 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:
+ cout << " Ground Trim" << endl;
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
+ //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
+ break;
+ case tPullup:
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,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,tVdot,tPhi ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+ break;
+ case tTurn:
+ 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,tVdot,tBeta ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
+ TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
+ break;
+ case tCustom:
+ case tNone:
+ break;
+ }
+ //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+ sub_iterations=new double[TrimAxes.size()];
+ successful=new double[TrimAxes.size()];
+ solution=new bool[TrimAxes.size()];
+ current_axis=0;
+}
+//YOU WERE WARNED, BUT YOU DID IT ANYWAY.
+}