#include "FGAircraft.h"
#include "FGMassBalance.h"
#include "FGAerodynamics.h"
+#include "FGColumnVector3.h"
#if _MSC_VER
#pragma warning (disable : 4786 4788)
#endif
Tolerance=1E-3;
A_Tolerance = Tolerance / 10;
- Debug=0;
+ Debug=0;DebugLevel=0;
fdmex=FDMExec;
fgic=FGIC;
total_its=0;
axis_count=0;
mode=tt;
xlo=xhi=alo=ahi;
+ targetNlf=1.0;
+ debug_axis=tAll;
switch(mode) {
case tFull:
cout << " Full Trim" << endl;
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()];
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()) {
return solutionExists;
}
+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.01 && fabs(phi) < 1.56 ) {
+ targetNlf = 1 / cos(phi);
+ g = fdmex->GetInertial()->gravity();
+ psidot = g*tan(phi) / fgic->GetVtrueFpsIC();
+ cout << targetNlf << ", " << psidot << endl;
+ }
+}
+
+void FGTrim::updateRates(void){
+ if(mode == tTurn) {
+ double p,q,r,theta,phi;
+ theta=fgic->GetPitchAngleRadIC();
+ phi=fgic->GetRollAngleRadIC();
+ p=-psidot*sin(theta);
+ q=psidot*cos(theta)*sin(phi);
+ r=psidot*cos(theta)*cos(phi);
+ fdmex->GetRotation()->SetPQR(p,q,r);
+ }
+}
+
+void FGTrim::setDebug(void) {
+ if(debug_axis == tAll ||
+ TrimAxes[current_axis]->GetStateType() == debug_axis ) {
+ Debug=DebugLevel;
+ return;
+ } else {
+ Debug=0;
+ return;
+ }
+}
+
//YOU WERE WARNED, BUT YOU DID IT ANYWAY.