//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
+FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
N=Nsub=0;
max_iterations=60;
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;
+ xlo=xhi=alo=ahi=0.0;
targetNlf=1.0;
debug_axis=tAll;
- 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;
- 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,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,tVdot,tBeta ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
- TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
- 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;
-
+ SetMode(tt);
if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
}
iAxes++;
}
TrimAxes.clear();
- cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
+ //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
cout << "nlf done" << endl;
} else if (mode == tTurn) {
setupTurn();
- TrimAxes[0]->SetStateTarget(targetNlf);
+ //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;
<< fgic->GetVtrueFpsIC() << endl;
q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
cout << targetNlf << ", " << q << endl;
- vPQR.InitMatrix();
- vPQR(2)=q;
- cout << vPQR << endl;
- fdmex->GetRotation()->SetPQR(vPQR);
+ fdmex->GetRotation()->SetPQR(0,q,0);
cout << "setPitchRateInPullup() complete" << endl;
}
-
+
+//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
void FGTrim::setupTurn(void){
- FGColumnVector3 vPQR;
- float g,q,r,phi, psidot;
+ double g,phi;
phi = fgic->GetRollAngleRadIC();
- if( fabs(phi) > 0.01 && fabs(phi) < 1.56 ) {
+ if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
targetNlf = 1 / cos(phi);
g = fdmex->GetInertial()->gravity();
- psidot = g*tan(phi) / fgic->GetVtrueFpsIC();
- q = psidot*sin(phi);
- r = psidot*cos(phi);
- vPQR(1)=0;vPQR(2)=q;vPQR(3)=r;
- fdmex->GetRotation()->SetPQR(vPQR);
- cout << targetNlf << ", " << vPQR*57.29577 << endl;
+ 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 ||
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.