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/FGAtmosphere.h>
47 #include "FGInitialCondition.h"
48 #include <models/FGAircraft.h>
49 #include <models/FGMassBalance.h>
50 #include <models/FGGroundReactions.h>
51 #include <models/FGInertial.h>
52 #include <models/FGAerodynamics.h>
53 #include <math/FGColumnVector3.h>
56 #pragma warning (disable : 4786 4788)
61 static const char *IdSrc = "$Id$";
62 static const char *IdHdr = ID_TRIM;
64 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
66 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
70 max_sub_iterations=100;
72 A_Tolerance = Tolerance / 10;
86 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
89 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
91 FGTrim::~FGTrim(void) {
92 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
93 delete TrimAxes[current_axis];
95 delete[] sub_iterations;
98 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
101 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
103 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 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
113 TrimAxes[current_axis]->GetStateName().c_str(),
114 sub_iterations[current_axis],
115 sub_iterations[current_axis]/double(total_its),
116 successful[current_axis],
117 TrimAxes[current_axis]->GetAvgStability() );
120 cout << " Run Count: " << run_sum << endl;
124 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
126 void FGTrim::Report(void) {
127 cout << " Trim Results: " << endl;
128 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
129 TrimAxes[current_axis]->AxisReport();
133 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
135 void FGTrim::ClearStates(void) {
139 vector<FGTrimAxis*>::iterator iAxes;
140 iAxes = TrimAxes.begin();
141 while (iAxes != TrimAxes.end()) {
147 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
150 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
152 bool FGTrim::AddState( State state, Control control ) {
157 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
158 while (iAxes != TrimAxes.end()) {
160 if( ta->GetStateType() == state )
165 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
166 delete[] sub_iterations;
169 sub_iterations=new double[TrimAxes.size()];
170 successful=new double[TrimAxes.size()];
171 solution=new bool[TrimAxes.size()];
176 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
178 bool FGTrim::RemoveState( State state ) {
183 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
184 while (iAxes != TrimAxes.end()) {
186 if( ta->GetStateType() == state ) {
188 TrimAxes.erase(iAxes);
195 delete[] sub_iterations;
198 sub_iterations=new double[TrimAxes.size()];
199 successful=new double[TrimAxes.size()];
200 solution=new bool[TrimAxes.size()];
205 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
207 bool FGTrim::EditState( State state, Control new_control ){
212 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
213 while (iAxes != TrimAxes.end()) {
215 if( ta->GetStateType() == state ) {
216 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
218 TrimAxes.erase(iAxes+1);
228 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
230 bool FGTrim::DoTrim(void) {
235 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
236 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
239 fdmex->DisableOutput();
241 fgic->SetPRadpsIC(0.0);
242 fgic->SetQRadpsIC(0.0);
243 fgic->SetRRadpsIC(0.0);
245 //clear the sub iterations counts & zero out the controls
246 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
247 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
248 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
249 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
250 if(mode == tGround) {
251 TrimAxes[current_axis]->initTheta();
254 xlo=TrimAxes[current_axis]->GetControlMin();
255 xhi=TrimAxes[current_axis]->GetControlMax();
256 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
257 TrimAxes[current_axis]->Run();
258 //TrimAxes[current_axis]->AxisReport();
259 sub_iterations[current_axis]=0;
260 successful[current_axis]=0;
261 solution[current_axis]=false;
265 if(mode == tPullup ) {
266 cout << "Setting pitch rate and nlf... " << endl;
268 cout << "pitch rate done ... " << endl;
269 TrimAxes[0]->SetStateTarget(targetNlf);
270 cout << "nlf done" << endl;
271 } else if (mode == tTurn) {
273 //TrimAxes[0]->SetStateTarget(targetNlf);
278 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
282 if(!solution[current_axis]) {
284 solution[current_axis]=true;
287 } else if(findInterval()) {
290 solution[current_axis]=false;
292 sub_iterations[current_axis]+=Nsub;
294 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
295 //these checks need to be done after all the axes have run
296 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
297 if(TrimAxes[current_axis]->InTolerance()) {
299 successful[current_axis]++;
304 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
305 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
306 //At this point we can check the input limits of the failed axis
307 //and declare the trim failed if there is no sign change. If there
308 //is, keep going until success or max iteration count
310 //Oh, well: two out of three ain't bad
311 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
312 //these checks need to be done after all the axes have run
313 if(!TrimAxes[current_axis]->InTolerance()) {
315 // special case this for now -- if other cases arise proper
316 // support can be added to FGTrimAxis
317 if( (gamma_fallback) &&
318 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
319 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
320 cout << " Can't trim udot with throttle, trying flight"
321 << " path angle. (" << N << ")" << endl;
322 if(TrimAxes[current_axis]->GetState() > 0)
323 TrimAxes[current_axis]->SetControlToMin();
325 TrimAxes[current_axis]->SetControlToMax();
326 TrimAxes[current_axis]->Run();
327 delete TrimAxes[current_axis];
328 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
331 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
332 << " doesn't appear to be trimmable" << endl;
334 trim_failed=true; //force the trim to fail
339 } //all-but-one check
341 if(N > max_iterations)
343 } while((axis_count < TrimAxes.size()) && (!trim_failed));
344 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
347 cout << endl << " Trim successful" << endl;
351 cout << endl << " Trim failed" << endl;
353 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
354 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
356 fdmex->EnableOutput();
360 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
362 bool FGTrim::solve(void) {
364 double x1,x2,x3,f1,f2,f3,d,d0;
365 const double relax =0.9;
366 double eps=TrimAxes[current_axis]->GetSolverEps();
372 if( solutionDomain != 0) {
373 /* if(ahi > alo) { */
382 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
383 while ( (TrimAxes[current_axis]->InTolerance() == false )
384 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
387 x2=x1-d*d0*f1/(f3-f1);
388 TrimAxes[current_axis]->SetControl(x2);
389 TrimAxes[current_axis]->Run();
390 f2=TrimAxes[current_axis]->GetState();
392 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
393 << ", " << x2 << ", " << x3 << endl;
394 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
400 //cout << "Solution is between x1 and x2" << endl;
402 else if(f2*f3 <= 0.0) {
406 //cout << "Solution is between x2 and x3" << endl;
413 if(Nsub < max_sub_iterations) success=true;
418 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
420 produces an interval (xlo..xhi) on one side or the other of the current
421 control value in which a solution exists. This domain is, hopefully,
422 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
423 to find the solution. This is, hopefully, more efficient than having the
424 solver start from scratch every time. Maybe it isn't though...
425 This tries to take advantage of the idea that the changes from iteration to
426 iteration will be small after the first one or two top-level iterations.
428 assumes that changing the control will a produce significant change in the
429 accel i.e. checkLimits() has already been called.
431 if a solution is found above the current control, the function returns true
432 and xlo is set to the current control, xhi to the interval max it found, and
433 solutionDomain is set to 1.
434 if the solution lies below the current control, then the function returns
435 true and xlo is set to the interval min it found and xmax to the current
436 control. if no solution is found, then the function returns false.
439 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
440 no assumptions about the state of the sim after this function has run
443 bool FGTrim::findInterval(void) {
446 double current_control=TrimAxes[current_axis]->GetControl();
447 double current_accel=TrimAxes[current_axis]->GetState();;
448 double xmin=TrimAxes[current_axis]->GetControlMin();
449 double xmax=TrimAxes[current_axis]->GetControlMax();
450 double lastxlo,lastxhi,lastalo,lastahi;
452 step=0.025*fabs(xmax);
453 xlo=xhi=current_control;
454 alo=ahi=current_accel;
455 lastxlo=xlo;lastxhi=xhi;
456 lastalo=alo;lastahi=ahi;
462 if(xlo < xmin) xlo=xmin;
464 if(xhi > xmax) xhi=xmax;
465 TrimAxes[current_axis]->SetControl(xlo);
466 TrimAxes[current_axis]->Run();
467 alo=TrimAxes[current_axis]->GetState();
468 TrimAxes[current_axis]->SetControl(xhi);
469 TrimAxes[current_axis]->Run();
470 ahi=TrimAxes[current_axis]->GetState();
471 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
472 if(alo*ahi <=0) { //found interval with root
474 if(alo*current_accel <= 0) { //narrow interval down a bit
478 //xhi=current_control;
484 //xlo=current_control;
488 lastxlo=xlo;lastxhi=xhi;
489 lastalo=alo;lastahi=ahi;
490 if( !found && xlo==xmin && xhi==xmax ) continue;
492 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
493 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
494 } while(!found && (Nsub <= max_sub_iterations) );
498 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
499 //checks to see which side of the current control value the solution is on
500 //and sets solutionDomain accordingly:
501 // 1 if solution is between the current and max
502 // -1 if solution is between the min and current
503 // 0 if there is no solution
505 //if changing the control produces no significant change in the accel then
506 //solutionDomain is set to zero and the function returns false
507 //if a solution is found, then xlo and xhi are set so that they bracket
508 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
509 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
510 //xhi=xmax and ahi=accel(xmax)
511 //in all cases the sim is left such that the control=xmax and accel=ahi
513 bool FGTrim::checkLimits(void) {
515 double current_control=TrimAxes[current_axis]->GetControl();
516 double current_accel=TrimAxes[current_axis]->GetState();
517 xlo=TrimAxes[current_axis]->GetControlMin();
518 xhi=TrimAxes[current_axis]->GetControlMax();
520 TrimAxes[current_axis]->SetControl(xlo);
521 TrimAxes[current_axis]->Run();
522 alo=TrimAxes[current_axis]->GetState();
523 TrimAxes[current_axis]->SetControl(xhi);
524 TrimAxes[current_axis]->Run();
525 ahi=TrimAxes[current_axis]->GetState();
527 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
528 << alo << ", " << ahi << endl;
530 solutionExists=false;
531 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
532 if(alo*current_accel <= 0) {
537 } else if(current_accel*ahi < 0){
544 TrimAxes[current_axis]->SetControl(current_control);
545 TrimAxes[current_axis]->Run();
546 return solutionExists;
549 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
551 void FGTrim::setupPullup() {
553 g=fdmex->GetInertial()->gravity();
554 cgamma=cos(fgic->GetFlightPathAngleRadIC());
555 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
556 << fgic->GetVtrueFpsIC() << endl;
557 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
558 cout << targetNlf << ", " << q << endl;
559 fgic->SetQRadpsIC(q);
560 cout << "setPitchRateInPullup() complete" << endl;
564 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
566 void FGTrim::setupTurn(void){
568 phi = fgic->GetPhiRadIC();
569 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
570 targetNlf = 1 / cos(phi);
571 g = fdmex->GetInertial()->gravity();
572 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
573 cout << targetNlf << ", " << psidot << endl;
578 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
580 void FGTrim::updateRates(void){
581 if( mode == tTurn ) {
582 double phi = fgic->GetPhiRadIC();
583 double g = fdmex->GetInertial()->gravity();
585 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
586 theta=fgic->GetThetaRadIC();
587 phi=fgic->GetPhiRadIC();
588 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
589 p=-psidot*sin(theta);
590 q=psidot*cos(theta)*sin(phi);
591 r=psidot*cos(theta)*cos(phi);
595 fgic->SetPRadpsIC(p);
596 fgic->SetQRadpsIC(q);
597 fgic->SetRRadpsIC(r);
598 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
600 g=fdmex->GetInertial()->gravity();
601 cgamma=cos(fgic->GetFlightPathAngleRadIC());
602 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
603 fgic->SetQRadpsIC(q);
607 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
609 void FGTrim::setDebug(void) {
610 if(debug_axis == tAll ||
611 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
620 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
622 void FGTrim::SetMode(TrimMode tt) {
628 cout << " Full Trim" << endl;
629 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
630 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
631 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
632 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
633 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
634 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
635 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
639 cout << " Longitudinal Trim" << endl;
640 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
641 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
642 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
646 cout << " Ground Trim" << endl;
647 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
648 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
649 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
652 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
653 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
654 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
655 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
656 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
661 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
663 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
664 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
665 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
666 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
672 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
673 sub_iterations=new double[TrimAxes.size()];
674 successful=new double[TrimAxes.size()];
675 solution=new bool[TrimAxes.size()];
678 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.