1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 --------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) ---------
9 This program is free software; you can redistribute it and/or modify it under
10 the terms of the GNU Lesser General Public License as published by the Free Software
11 Foundation; either version 2 of the License, or (at your option) any later
14 This program is distributed in the hope that it will be useful, but WITHOUT
15 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
16 FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
19 You should have received a copy of the GNU Lesser General Public License along with
20 this program; if not, write to the Free Software Foundation, Inc., 59 Temple
21 Place - Suite 330, Boston, MA 02111-1307, USA.
23 Further information about the GNU Lesser General Public License can also be found on
24 the world wide web at http://www.gnu.org.
27 --------------------------------------------------------------------------------
30 FUNCTIONAL DESCRIPTION
31 --------------------------------------------------------------------------------
33 This class takes the given set of IC's and finds the angle of attack, elevator,
34 and throttle setting required to fly steady level. This is currently for in-air
35 conditions only. It is implemented using an iterative, one-axis-at-a-time
38 // !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
40 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
42 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
46 #include "models/FGGroundReactions.h"
47 #include "models/FGInertial.h"
50 #pragma warning (disable : 4786 4788)
57 static const char *IdSrc = "$Id: FGTrim.cpp,v 1.15 2011/02/19 16:29:29 bcoconni Exp $";
58 static const char *IdHdr = ID_TRIM;
60 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
62 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
66 max_sub_iterations=100;
68 A_Tolerance = Tolerance / 10;
82 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
85 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
87 FGTrim::~FGTrim(void) {
88 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
89 delete TrimAxes[current_axis];
91 delete[] sub_iterations;
94 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
97 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
99 void FGTrim::TrimStats() {
101 cout << endl << " Trim Statistics: " << endl;
102 cout << " Total Iterations: " << total_its << endl;
104 cout << " Sub-iterations:" << endl;
105 for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
106 run_sum += TrimAxes[current_axis]->GetRunCount();
107 cout << " " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
108 << ": " << setprecision(3) << sub_iterations[current_axis]
109 << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
110 << " successful: " << setprecision(3) << successful[current_axis]
111 << " stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
114 cout << " Run Count: " << run_sum << endl;
118 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
120 void FGTrim::Report(void) {
121 cout << " Trim Results: " << endl;
122 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
123 TrimAxes[current_axis]->AxisReport();
127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 void FGTrim::ClearStates(void) {
133 vector<FGTrimAxis*>::iterator iAxes;
134 iAxes = TrimAxes.begin();
135 while (iAxes != TrimAxes.end()) {
141 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
144 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
146 bool FGTrim::AddState( State state, Control control ) {
151 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
152 while (iAxes != TrimAxes.end()) {
154 if( ta->GetStateType() == state )
159 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
160 delete[] sub_iterations;
163 sub_iterations=new double[TrimAxes.size()];
164 successful=new double[TrimAxes.size()];
165 solution=new bool[TrimAxes.size()];
170 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
172 bool FGTrim::RemoveState( State state ) {
177 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
178 while (iAxes != TrimAxes.end()) {
180 if( ta->GetStateType() == state ) {
182 iAxes = TrimAxes.erase(iAxes);
189 delete[] sub_iterations;
192 sub_iterations=new double[TrimAxes.size()];
193 successful=new double[TrimAxes.size()];
194 solution=new bool[TrimAxes.size()];
199 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
201 bool FGTrim::EditState( State state, Control new_control ){
206 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
207 while (iAxes != TrimAxes.end()) {
209 if( ta->GetStateType() == state ) {
210 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
212 TrimAxes.erase(iAxes+1);
222 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
224 bool FGTrim::DoTrim(void) {
229 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
230 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
233 fdmex->DisableOutput();
235 fdmex->SetTrimStatus(true);
237 fgic->SetPRadpsIC(0.0);
238 fgic->SetQRadpsIC(0.0);
239 fgic->SetRRadpsIC(0.0);
241 //clear the sub iterations counts & zero out the controls
242 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
243 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
244 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
245 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
246 if(mode == tGround) {
247 TrimAxes[current_axis]->initTheta();
250 xlo=TrimAxes[current_axis]->GetControlMin();
251 xhi=TrimAxes[current_axis]->GetControlMax();
252 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
253 TrimAxes[current_axis]->Run();
254 //TrimAxes[current_axis]->AxisReport();
255 sub_iterations[current_axis]=0;
256 successful[current_axis]=0;
257 solution[current_axis]=false;
261 if(mode == tPullup ) {
262 cout << "Setting pitch rate and nlf... " << endl;
264 cout << "pitch rate done ... " << endl;
265 TrimAxes[0]->SetStateTarget(targetNlf);
266 cout << "nlf done" << endl;
267 } else if (mode == tTurn) {
269 //TrimAxes[0]->SetStateTarget(targetNlf);
274 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
278 if(!solution[current_axis]) {
280 solution[current_axis]=true;
283 } else if(findInterval()) {
286 solution[current_axis]=false;
288 sub_iterations[current_axis]+=Nsub;
290 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
291 //these checks need to be done after all the axes have run
292 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
293 if(TrimAxes[current_axis]->InTolerance()) {
295 successful[current_axis]++;
300 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
301 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
302 //At this point we can check the input limits of the failed axis
303 //and declare the trim failed if there is no sign change. If there
304 //is, keep going until success or max iteration count
306 //Oh, well: two out of three ain't bad
307 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
308 //these checks need to be done after all the axes have run
309 if(!TrimAxes[current_axis]->InTolerance()) {
311 // special case this for now -- if other cases arise proper
312 // support can be added to FGTrimAxis
313 if( (gamma_fallback) &&
314 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
315 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
316 cout << " Can't trim udot with throttle, trying flight"
317 << " path angle. (" << N << ")" << endl;
318 if(TrimAxes[current_axis]->GetState() > 0)
319 TrimAxes[current_axis]->SetControlToMin();
321 TrimAxes[current_axis]->SetControlToMax();
322 TrimAxes[current_axis]->Run();
323 delete TrimAxes[current_axis];
324 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
327 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
328 << " doesn't appear to be trimmable" << endl;
330 trim_failed=true; //force the trim to fail
335 } //all-but-one check
337 if(N > max_iterations)
339 } while((axis_count < TrimAxes.size()) && (!trim_failed));
340 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
343 cout << endl << " Trim successful" << endl;
347 cout << endl << " Trim failed" << endl;
349 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
350 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
352 fdmex->SetTrimStatus(false);
353 fdmex->EnableOutput();
357 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
359 bool FGTrim::solve(void) {
361 double x1,x2,x3,f1,f2,f3,d,d0;
362 const double relax =0.9;
363 double eps=TrimAxes[current_axis]->GetSolverEps();
369 if( solutionDomain != 0) {
370 /* if(ahi > alo) { */
379 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
380 while ( (TrimAxes[current_axis]->InTolerance() == false )
381 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
384 x2=x1-d*d0*f1/(f3-f1);
385 TrimAxes[current_axis]->SetControl(x2);
386 TrimAxes[current_axis]->Run();
387 f2=TrimAxes[current_axis]->GetState();
389 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
390 << ", " << x2 << ", " << x3 << endl;
391 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
397 //cout << "Solution is between x1 and x2" << endl;
399 else if(f2*f3 <= 0.0) {
403 //cout << "Solution is between x2 and x3" << endl;
410 if(Nsub < max_sub_iterations) success=true;
415 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
417 produces an interval (xlo..xhi) on one side or the other of the current
418 control value in which a solution exists. This domain is, hopefully,
419 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
420 to find the solution. This is, hopefully, more efficient than having the
421 solver start from scratch every time. Maybe it isn't though...
422 This tries to take advantage of the idea that the changes from iteration to
423 iteration will be small after the first one or two top-level iterations.
425 assumes that changing the control will a produce significant change in the
426 accel i.e. checkLimits() has already been called.
428 if a solution is found above the current control, the function returns true
429 and xlo is set to the current control, xhi to the interval max it found, and
430 solutionDomain is set to 1.
431 if the solution lies below the current control, then the function returns
432 true and xlo is set to the interval min it found and xmax to the current
433 control. if no solution is found, then the function returns false.
436 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
437 no assumptions about the state of the sim after this function has run
440 bool FGTrim::findInterval(void) {
443 double current_control=TrimAxes[current_axis]->GetControl();
444 double current_accel=TrimAxes[current_axis]->GetState();;
445 double xmin=TrimAxes[current_axis]->GetControlMin();
446 double xmax=TrimAxes[current_axis]->GetControlMax();
447 double lastxlo,lastxhi,lastalo,lastahi;
449 step=0.025*fabs(xmax);
450 xlo=xhi=current_control;
451 alo=ahi=current_accel;
452 lastxlo=xlo;lastxhi=xhi;
453 lastalo=alo;lastahi=ahi;
459 if(xlo < xmin) xlo=xmin;
461 if(xhi > xmax) xhi=xmax;
462 TrimAxes[current_axis]->SetControl(xlo);
463 TrimAxes[current_axis]->Run();
464 alo=TrimAxes[current_axis]->GetState();
465 TrimAxes[current_axis]->SetControl(xhi);
466 TrimAxes[current_axis]->Run();
467 ahi=TrimAxes[current_axis]->GetState();
468 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
469 if(alo*ahi <=0) { //found interval with root
471 if(alo*current_accel <= 0) { //narrow interval down a bit
475 //xhi=current_control;
481 //xlo=current_control;
485 lastxlo=xlo;lastxhi=xhi;
486 lastalo=alo;lastahi=ahi;
487 if( !found && xlo==xmin && xhi==xmax ) continue;
489 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
490 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
491 } while(!found && (Nsub <= max_sub_iterations) );
495 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
496 //checks to see which side of the current control value the solution is on
497 //and sets solutionDomain accordingly:
498 // 1 if solution is between the current and max
499 // -1 if solution is between the min and current
500 // 0 if there is no solution
502 //if changing the control produces no significant change in the accel then
503 //solutionDomain is set to zero and the function returns false
504 //if a solution is found, then xlo and xhi are set so that they bracket
505 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
506 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
507 //xhi=xmax and ahi=accel(xmax)
508 //in all cases the sim is left such that the control=xmax and accel=ahi
510 bool FGTrim::checkLimits(void) {
512 double current_control=TrimAxes[current_axis]->GetControl();
513 double current_accel=TrimAxes[current_axis]->GetState();
514 xlo=TrimAxes[current_axis]->GetControlMin();
515 xhi=TrimAxes[current_axis]->GetControlMax();
517 TrimAxes[current_axis]->SetControl(xlo);
518 TrimAxes[current_axis]->Run();
519 alo=TrimAxes[current_axis]->GetState();
520 TrimAxes[current_axis]->SetControl(xhi);
521 TrimAxes[current_axis]->Run();
522 ahi=TrimAxes[current_axis]->GetState();
524 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
525 << alo << ", " << ahi << endl;
527 solutionExists=false;
528 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
529 if(alo*current_accel <= 0) {
534 } else if(current_accel*ahi < 0){
541 TrimAxes[current_axis]->SetControl(current_control);
542 TrimAxes[current_axis]->Run();
543 return solutionExists;
546 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
548 void FGTrim::setupPullup() {
550 g=fdmex->GetInertial()->gravity();
551 cgamma=cos(fgic->GetFlightPathAngleRadIC());
552 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
553 << fgic->GetVtrueFpsIC() << endl;
554 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
555 cout << targetNlf << ", " << q << endl;
556 fgic->SetQRadpsIC(q);
557 cout << "setPitchRateInPullup() complete" << endl;
561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
563 void FGTrim::setupTurn(void){
565 phi = fgic->GetPhiRadIC();
566 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
567 targetNlf = 1 / cos(phi);
568 g = fdmex->GetInertial()->gravity();
569 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
570 cout << targetNlf << ", " << psidot << endl;
575 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
577 void FGTrim::updateRates(void){
578 if( mode == tTurn ) {
579 double phi = fgic->GetPhiRadIC();
580 double g = fdmex->GetInertial()->gravity();
582 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
583 theta=fgic->GetThetaRadIC();
584 phi=fgic->GetPhiRadIC();
585 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
586 p=-psidot*sin(theta);
587 q=psidot*cos(theta)*sin(phi);
588 r=psidot*cos(theta)*cos(phi);
592 fgic->SetPRadpsIC(p);
593 fgic->SetQRadpsIC(q);
594 fgic->SetRRadpsIC(r);
595 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
597 g=fdmex->GetInertial()->gravity();
598 cgamma=cos(fgic->GetFlightPathAngleRadIC());
599 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
600 fgic->SetQRadpsIC(q);
604 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
606 void FGTrim::setDebug(void) {
607 if(debug_axis == tAll ||
608 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
617 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
619 void FGTrim::SetMode(TrimMode tt) {
625 cout << " Full Trim" << endl;
626 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
627 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
628 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
629 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
630 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
631 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
632 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
636 cout << " Longitudinal Trim" << endl;
637 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
638 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
639 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
643 cout << " Ground Trim" << endl;
644 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
645 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
646 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
649 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
650 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
651 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
652 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
653 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
654 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
655 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
659 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
660 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
661 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
663 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
669 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
670 sub_iterations=new double[TrimAxes.size()];
671 successful=new double[TrimAxes.size()];
672 solution=new bool[TrimAxes.size()];
675 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.