bool FGTrim::DoTrim(void) {
bool trim_failed=false;
unsigned int N = 0;
unsigned int axis_count = 0;
FGFCS *FCS = fdmex->GetFCS();
vector<double> throttle0 = FCS->GetThrottleCmd();
double elevator0 = FCS->GetDeCmd();
double aileron0 = FCS->GetDaCmd();
double rudder0 = FCS->GetDrCmd();
double PitchTrim0 = FCS->GetPitchTrimCmd();
fgic = *fdmex->GetIC();
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
}
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
fgic.SetPRadpsIC(0.0);
fgic.SetQRadpsIC(0.0);
fgic.SetRRadpsIC(0.0);
if (mode == tGround) {
trimOnGround();
double theta = fgic.GetThetaRadIC();
double phi = fgic.GetPhiRadIC();
// Take opportunity of the first approx. found by trimOnGround() to
// refine the control limits.
TrimAxes[0].SetControlLimits(0., fgic.GetAltitudeAGLFtIC());
TrimAxes[1].SetControlLimits(theta - 5.0 * degtorad, theta + 5.0 * degtorad);
TrimAxes[2].SetControlLimits(phi - 30.0 * degtorad, phi + 30.0 * degtorad);
}
//clear the sub iterations counts & zero out the controls
for(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
//cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
//<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
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(unsigned int current_axis=0;current_axis<TrimAxes.size();current_axis++) {
setDebug(TrimAxes[current_axis]);
updateRates();
Nsub=0;
if(!solution[current_axis]) {
if(checkLimits(TrimAxes[current_axis])) {
solution[current_axis]=true;
solve(TrimAxes[current_axis]);
}
} else if(findInterval(TrimAxes[current_axis])) {
solve(TrimAxes[current_axis]);
} else {
solution[current_axis]=false;
}
sub_iterations[current_axis]+=Nsub;
}
for(unsigned int 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(unsigned int 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(TrimAxes[current_axis])) {
// 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();
TrimAxes[current_axis]=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++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else { // The trim has failed
total_its=N;
// Restore the aircraft parameters to their initial values
fgic = *fdmex->GetIC();
FCS->SetDeCmd(elevator0);
FCS->SetDaCmd(aileron0);
FCS->SetDrCmd(rudder0);
FCS->SetPitchTrimCmd(PitchTrim0);
for (unsigned int i=0; i < throttle0.size(); i++)
FCS->SetThrottleCmd(i, throttle0[i]);
fdmex->Initialize(&fgic);
fdmex->Run();
// If WOW is true we must make sure there are no gears into the ground.
if (fdmex->GetGroundReactions()->GetWOW())
trimOnGround();
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
fdmex->ResumeIntegration();
fdmex->SetTrimStatus(false);
for(int i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
return !trim_failed;
}
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();
fdmex->SetTrimStatus(true);
fdmex->SuspendIntegration();
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++;
if(N > max_iterations)
trim_failed=true;
} while((axis_count < TrimAxes.size()) && (!trim_failed));
if((!trim_failed) && (axis_count >= TrimAxes.size())) {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim successful" << endl;
} else {
total_its=N;
if (debug_lvl > 0)
cout << endl << " Trim failed" << endl;
}
for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
}
fdmex->SetTrimStatus(false);
fdmex->ResumeIntegration();
fdmex->EnableOutput();
return !trim_failed;
}
