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 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
47 #include <models/FGAtmosphere.h>
48 #include "FGInitialCondition.h"
49 #include <models/FGAircraft.h>
50 #include <models/FGMassBalance.h>
51 #include <models/FGGroundReactions.h>
52 #include <models/FGInertial.h>
53 #include <models/FGAerodynamics.h>
54 #include <math/FGColumnVector3.h>
57 #pragma warning (disable : 4786 4788)
62 static const char *IdSrc = "$Id$";
63 static const char *IdHdr = ID_TRIM;
65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
67 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
71 max_sub_iterations=100;
73 A_Tolerance = Tolerance / 10;
87 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
90 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
92 FGTrim::~FGTrim(void) {
93 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
94 delete TrimAxes[current_axis];
96 delete[] sub_iterations;
99 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
102 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
104 void FGTrim::TrimStats() {
106 cout << endl << " Trim Statistics: " << endl;
107 cout << " Total Iterations: " << total_its << endl;
109 cout << " Sub-iterations:" << endl;
110 for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
111 run_sum += TrimAxes[current_axis]->GetRunCount();
112 cout << " " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
113 << ": " << setprecision(3) << sub_iterations[current_axis]
114 << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
115 << " successful: " << setprecision(3) << successful[current_axis]
116 << " stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
119 cout << " Run Count: " << run_sum << endl;
123 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
125 void FGTrim::Report(void) {
126 cout << " Trim Results: " << endl;
127 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
128 TrimAxes[current_axis]->AxisReport();
132 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
134 void FGTrim::ClearStates(void) {
138 vector<FGTrimAxis*>::iterator iAxes;
139 iAxes = TrimAxes.begin();
140 while (iAxes != TrimAxes.end()) {
146 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
149 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
151 bool FGTrim::AddState( State state, Control control ) {
156 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
157 while (iAxes != TrimAxes.end()) {
159 if( ta->GetStateType() == state )
164 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
165 delete[] sub_iterations;
168 sub_iterations=new double[TrimAxes.size()];
169 successful=new double[TrimAxes.size()];
170 solution=new bool[TrimAxes.size()];
175 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
177 bool FGTrim::RemoveState( State state ) {
182 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
183 while (iAxes != TrimAxes.end()) {
185 if( ta->GetStateType() == state ) {
187 iAxes = TrimAxes.erase(iAxes);
194 delete[] sub_iterations;
197 sub_iterations=new double[TrimAxes.size()];
198 successful=new double[TrimAxes.size()];
199 solution=new bool[TrimAxes.size()];
204 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
206 bool FGTrim::EditState( State state, Control new_control ){
211 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
212 while (iAxes != TrimAxes.end()) {
214 if( ta->GetStateType() == state ) {
215 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
217 TrimAxes.erase(iAxes+1);
227 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
229 bool FGTrim::DoTrim(void) {
234 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
235 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
238 fdmex->DisableOutput();
240 fgic->SetPRadpsIC(0.0);
241 fgic->SetQRadpsIC(0.0);
242 fgic->SetRRadpsIC(0.0);
244 //clear the sub iterations counts & zero out the controls
245 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
246 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
247 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
248 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
249 if(mode == tGround) {
250 TrimAxes[current_axis]->initTheta();
253 xlo=TrimAxes[current_axis]->GetControlMin();
254 xhi=TrimAxes[current_axis]->GetControlMax();
255 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
256 TrimAxes[current_axis]->Run();
257 //TrimAxes[current_axis]->AxisReport();
258 sub_iterations[current_axis]=0;
259 successful[current_axis]=0;
260 solution[current_axis]=false;
264 if(mode == tPullup ) {
265 cout << "Setting pitch rate and nlf... " << endl;
267 cout << "pitch rate done ... " << endl;
268 TrimAxes[0]->SetStateTarget(targetNlf);
269 cout << "nlf done" << endl;
270 } else if (mode == tTurn) {
272 //TrimAxes[0]->SetStateTarget(targetNlf);
277 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
281 if(!solution[current_axis]) {
283 solution[current_axis]=true;
286 } else if(findInterval()) {
289 solution[current_axis]=false;
291 sub_iterations[current_axis]+=Nsub;
293 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
294 //these checks need to be done after all the axes have run
295 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
296 if(TrimAxes[current_axis]->InTolerance()) {
298 successful[current_axis]++;
303 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
304 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
305 //At this point we can check the input limits of the failed axis
306 //and declare the trim failed if there is no sign change. If there
307 //is, keep going until success or max iteration count
309 //Oh, well: two out of three ain't bad
310 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
311 //these checks need to be done after all the axes have run
312 if(!TrimAxes[current_axis]->InTolerance()) {
314 // special case this for now -- if other cases arise proper
315 // support can be added to FGTrimAxis
316 if( (gamma_fallback) &&
317 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
318 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
319 cout << " Can't trim udot with throttle, trying flight"
320 << " path angle. (" << N << ")" << endl;
321 if(TrimAxes[current_axis]->GetState() > 0)
322 TrimAxes[current_axis]->SetControlToMin();
324 TrimAxes[current_axis]->SetControlToMax();
325 TrimAxes[current_axis]->Run();
326 delete TrimAxes[current_axis];
327 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
330 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
331 << " doesn't appear to be trimmable" << endl;
333 trim_failed=true; //force the trim to fail
338 } //all-but-one check
340 if(N > max_iterations)
342 } while((axis_count < TrimAxes.size()) && (!trim_failed));
343 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
346 cout << endl << " Trim successful" << endl;
350 cout << endl << " Trim failed" << endl;
352 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
353 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
355 fdmex->EnableOutput();
359 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
361 bool FGTrim::solve(void) {
363 double x1,x2,x3,f1,f2,f3,d,d0;
364 const double relax =0.9;
365 double eps=TrimAxes[current_axis]->GetSolverEps();
371 if( solutionDomain != 0) {
372 /* if(ahi > alo) { */
381 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
382 while ( (TrimAxes[current_axis]->InTolerance() == false )
383 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
386 x2=x1-d*d0*f1/(f3-f1);
387 TrimAxes[current_axis]->SetControl(x2);
388 TrimAxes[current_axis]->Run();
389 f2=TrimAxes[current_axis]->GetState();
391 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
392 << ", " << x2 << ", " << x3 << endl;
393 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
399 //cout << "Solution is between x1 and x2" << endl;
401 else if(f2*f3 <= 0.0) {
405 //cout << "Solution is between x2 and x3" << endl;
412 if(Nsub < max_sub_iterations) success=true;
417 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
419 produces an interval (xlo..xhi) on one side or the other of the current
420 control value in which a solution exists. This domain is, hopefully,
421 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
422 to find the solution. This is, hopefully, more efficient than having the
423 solver start from scratch every time. Maybe it isn't though...
424 This tries to take advantage of the idea that the changes from iteration to
425 iteration will be small after the first one or two top-level iterations.
427 assumes that changing the control will a produce significant change in the
428 accel i.e. checkLimits() has already been called.
430 if a solution is found above the current control, the function returns true
431 and xlo is set to the current control, xhi to the interval max it found, and
432 solutionDomain is set to 1.
433 if the solution lies below the current control, then the function returns
434 true and xlo is set to the interval min it found and xmax to the current
435 control. if no solution is found, then the function returns false.
438 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
439 no assumptions about the state of the sim after this function has run
442 bool FGTrim::findInterval(void) {
445 double current_control=TrimAxes[current_axis]->GetControl();
446 double current_accel=TrimAxes[current_axis]->GetState();;
447 double xmin=TrimAxes[current_axis]->GetControlMin();
448 double xmax=TrimAxes[current_axis]->GetControlMax();
449 double lastxlo,lastxhi,lastalo,lastahi;
451 step=0.025*fabs(xmax);
452 xlo=xhi=current_control;
453 alo=ahi=current_accel;
454 lastxlo=xlo;lastxhi=xhi;
455 lastalo=alo;lastahi=ahi;
461 if(xlo < xmin) xlo=xmin;
463 if(xhi > xmax) xhi=xmax;
464 TrimAxes[current_axis]->SetControl(xlo);
465 TrimAxes[current_axis]->Run();
466 alo=TrimAxes[current_axis]->GetState();
467 TrimAxes[current_axis]->SetControl(xhi);
468 TrimAxes[current_axis]->Run();
469 ahi=TrimAxes[current_axis]->GetState();
470 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
471 if(alo*ahi <=0) { //found interval with root
473 if(alo*current_accel <= 0) { //narrow interval down a bit
477 //xhi=current_control;
483 //xlo=current_control;
487 lastxlo=xlo;lastxhi=xhi;
488 lastalo=alo;lastahi=ahi;
489 if( !found && xlo==xmin && xhi==xmax ) continue;
491 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
492 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
493 } while(!found && (Nsub <= max_sub_iterations) );
497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
498 //checks to see which side of the current control value the solution is on
499 //and sets solutionDomain accordingly:
500 // 1 if solution is between the current and max
501 // -1 if solution is between the min and current
502 // 0 if there is no solution
504 //if changing the control produces no significant change in the accel then
505 //solutionDomain is set to zero and the function returns false
506 //if a solution is found, then xlo and xhi are set so that they bracket
507 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
508 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
509 //xhi=xmax and ahi=accel(xmax)
510 //in all cases the sim is left such that the control=xmax and accel=ahi
512 bool FGTrim::checkLimits(void) {
514 double current_control=TrimAxes[current_axis]->GetControl();
515 double current_accel=TrimAxes[current_axis]->GetState();
516 xlo=TrimAxes[current_axis]->GetControlMin();
517 xhi=TrimAxes[current_axis]->GetControlMax();
519 TrimAxes[current_axis]->SetControl(xlo);
520 TrimAxes[current_axis]->Run();
521 alo=TrimAxes[current_axis]->GetState();
522 TrimAxes[current_axis]->SetControl(xhi);
523 TrimAxes[current_axis]->Run();
524 ahi=TrimAxes[current_axis]->GetState();
526 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
527 << alo << ", " << ahi << endl;
529 solutionExists=false;
530 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
531 if(alo*current_accel <= 0) {
536 } else if(current_accel*ahi < 0){
543 TrimAxes[current_axis]->SetControl(current_control);
544 TrimAxes[current_axis]->Run();
545 return solutionExists;
548 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
550 void FGTrim::setupPullup() {
552 g=fdmex->GetInertial()->gravity();
553 cgamma=cos(fgic->GetFlightPathAngleRadIC());
554 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
555 << fgic->GetVtrueFpsIC() << endl;
556 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
557 cout << targetNlf << ", " << q << endl;
558 fgic->SetQRadpsIC(q);
559 cout << "setPitchRateInPullup() complete" << endl;
563 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
565 void FGTrim::setupTurn(void){
567 phi = fgic->GetPhiRadIC();
568 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
569 targetNlf = 1 / cos(phi);
570 g = fdmex->GetInertial()->gravity();
571 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
572 cout << targetNlf << ", " << psidot << endl;
577 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
579 void FGTrim::updateRates(void){
580 if( mode == tTurn ) {
581 double phi = fgic->GetPhiRadIC();
582 double g = fdmex->GetInertial()->gravity();
584 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
585 theta=fgic->GetThetaRadIC();
586 phi=fgic->GetPhiRadIC();
587 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
588 p=-psidot*sin(theta);
589 q=psidot*cos(theta)*sin(phi);
590 r=psidot*cos(theta)*cos(phi);
594 fgic->SetPRadpsIC(p);
595 fgic->SetQRadpsIC(q);
596 fgic->SetRRadpsIC(r);
597 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
599 g=fdmex->GetInertial()->gravity();
600 cgamma=cos(fgic->GetFlightPathAngleRadIC());
601 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
602 fgic->SetQRadpsIC(q);
606 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
608 void FGTrim::setDebug(void) {
609 if(debug_axis == tAll ||
610 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
619 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
621 void FGTrim::SetMode(TrimMode tt) {
627 cout << " Full Trim" << endl;
628 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
629 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
630 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
631 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
632 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
633 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
634 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
638 cout << " Longitudinal Trim" << endl;
639 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
640 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
641 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
645 cout << " Ground Trim" << endl;
646 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
647 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
648 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
651 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
652 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
653 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
654 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
655 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
656 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
660 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
661 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
663 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
664 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
665 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
671 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
672 sub_iterations=new double[TrimAxes.size()];
673 successful=new double[TrimAxes.size()];
674 solution=new bool[TrimAxes.size()];
677 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.