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)
64 static const char *IdSrc = "$Id$";
65 static const char *IdHdr = ID_TRIM;
67 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
69 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
73 max_sub_iterations=100;
75 A_Tolerance = Tolerance / 10;
89 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
92 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
94 FGTrim::~FGTrim(void) {
95 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
96 delete TrimAxes[current_axis];
98 delete[] sub_iterations;
101 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
104 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
106 void FGTrim::TrimStats() {
108 cout << endl << " Trim Statistics: " << endl;
109 cout << " Total Iterations: " << total_its << endl;
111 cout << " Sub-iterations:" << endl;
112 for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
113 run_sum += TrimAxes[current_axis]->GetRunCount();
114 cout << " " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
115 << ": " << setprecision(3) << sub_iterations[current_axis]
116 << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
117 << " successful: " << setprecision(3) << successful[current_axis]
118 << " stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
121 cout << " Run Count: " << run_sum << endl;
125 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
127 void FGTrim::Report(void) {
128 cout << " Trim Results: " << endl;
129 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
130 TrimAxes[current_axis]->AxisReport();
134 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
136 void FGTrim::ClearStates(void) {
140 vector<FGTrimAxis*>::iterator iAxes;
141 iAxes = TrimAxes.begin();
142 while (iAxes != TrimAxes.end()) {
148 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153 bool FGTrim::AddState( State state, Control control ) {
158 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
159 while (iAxes != TrimAxes.end()) {
161 if( ta->GetStateType() == state )
166 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
167 delete[] sub_iterations;
170 sub_iterations=new double[TrimAxes.size()];
171 successful=new double[TrimAxes.size()];
172 solution=new bool[TrimAxes.size()];
177 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
179 bool FGTrim::RemoveState( State state ) {
184 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
185 while (iAxes != TrimAxes.end()) {
187 if( ta->GetStateType() == state ) {
189 iAxes = TrimAxes.erase(iAxes);
196 delete[] sub_iterations;
199 sub_iterations=new double[TrimAxes.size()];
200 successful=new double[TrimAxes.size()];
201 solution=new bool[TrimAxes.size()];
206 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
208 bool FGTrim::EditState( State state, Control new_control ){
213 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
214 while (iAxes != TrimAxes.end()) {
216 if( ta->GetStateType() == state ) {
217 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
219 TrimAxes.erase(iAxes+1);
229 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
231 bool FGTrim::DoTrim(void) {
236 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
237 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
240 fdmex->DisableOutput();
242 fgic->SetPRadpsIC(0.0);
243 fgic->SetQRadpsIC(0.0);
244 fgic->SetRRadpsIC(0.0);
246 //clear the sub iterations counts & zero out the controls
247 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
248 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
249 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
250 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
251 if(mode == tGround) {
252 TrimAxes[current_axis]->initTheta();
255 xlo=TrimAxes[current_axis]->GetControlMin();
256 xhi=TrimAxes[current_axis]->GetControlMax();
257 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
258 TrimAxes[current_axis]->Run();
259 //TrimAxes[current_axis]->AxisReport();
260 sub_iterations[current_axis]=0;
261 successful[current_axis]=0;
262 solution[current_axis]=false;
266 if(mode == tPullup ) {
267 cout << "Setting pitch rate and nlf... " << endl;
269 cout << "pitch rate done ... " << endl;
270 TrimAxes[0]->SetStateTarget(targetNlf);
271 cout << "nlf done" << endl;
272 } else if (mode == tTurn) {
274 //TrimAxes[0]->SetStateTarget(targetNlf);
279 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
283 if(!solution[current_axis]) {
285 solution[current_axis]=true;
288 } else if(findInterval()) {
291 solution[current_axis]=false;
293 sub_iterations[current_axis]+=Nsub;
295 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
296 //these checks need to be done after all the axes have run
297 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
298 if(TrimAxes[current_axis]->InTolerance()) {
300 successful[current_axis]++;
305 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
306 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
307 //At this point we can check the input limits of the failed axis
308 //and declare the trim failed if there is no sign change. If there
309 //is, keep going until success or max iteration count
311 //Oh, well: two out of three ain't bad
312 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
313 //these checks need to be done after all the axes have run
314 if(!TrimAxes[current_axis]->InTolerance()) {
316 // special case this for now -- if other cases arise proper
317 // support can be added to FGTrimAxis
318 if( (gamma_fallback) &&
319 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
320 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
321 cout << " Can't trim udot with throttle, trying flight"
322 << " path angle. (" << N << ")" << endl;
323 if(TrimAxes[current_axis]->GetState() > 0)
324 TrimAxes[current_axis]->SetControlToMin();
326 TrimAxes[current_axis]->SetControlToMax();
327 TrimAxes[current_axis]->Run();
328 delete TrimAxes[current_axis];
329 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
332 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
333 << " doesn't appear to be trimmable" << endl;
335 trim_failed=true; //force the trim to fail
340 } //all-but-one check
342 if(N > max_iterations)
344 } while((axis_count < TrimAxes.size()) && (!trim_failed));
345 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
348 cout << endl << " Trim successful" << endl;
352 cout << endl << " Trim failed" << endl;
354 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
355 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
357 fdmex->EnableOutput();
361 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
363 bool FGTrim::solve(void) {
365 double x1,x2,x3,f1,f2,f3,d,d0;
366 const double relax =0.9;
367 double eps=TrimAxes[current_axis]->GetSolverEps();
373 if( solutionDomain != 0) {
374 /* if(ahi > alo) { */
383 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
384 while ( (TrimAxes[current_axis]->InTolerance() == false )
385 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
388 x2=x1-d*d0*f1/(f3-f1);
389 TrimAxes[current_axis]->SetControl(x2);
390 TrimAxes[current_axis]->Run();
391 f2=TrimAxes[current_axis]->GetState();
393 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
394 << ", " << x2 << ", " << x3 << endl;
395 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
401 //cout << "Solution is between x1 and x2" << endl;
403 else if(f2*f3 <= 0.0) {
407 //cout << "Solution is between x2 and x3" << endl;
414 if(Nsub < max_sub_iterations) success=true;
419 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
421 produces an interval (xlo..xhi) on one side or the other of the current
422 control value in which a solution exists. This domain is, hopefully,
423 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
424 to find the solution. This is, hopefully, more efficient than having the
425 solver start from scratch every time. Maybe it isn't though...
426 This tries to take advantage of the idea that the changes from iteration to
427 iteration will be small after the first one or two top-level iterations.
429 assumes that changing the control will a produce significant change in the
430 accel i.e. checkLimits() has already been called.
432 if a solution is found above the current control, the function returns true
433 and xlo is set to the current control, xhi to the interval max it found, and
434 solutionDomain is set to 1.
435 if the solution lies below the current control, then the function returns
436 true and xlo is set to the interval min it found and xmax to the current
437 control. if no solution is found, then the function returns false.
440 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
441 no assumptions about the state of the sim after this function has run
444 bool FGTrim::findInterval(void) {
447 double current_control=TrimAxes[current_axis]->GetControl();
448 double current_accel=TrimAxes[current_axis]->GetState();;
449 double xmin=TrimAxes[current_axis]->GetControlMin();
450 double xmax=TrimAxes[current_axis]->GetControlMax();
451 double lastxlo,lastxhi,lastalo,lastahi;
453 step=0.025*fabs(xmax);
454 xlo=xhi=current_control;
455 alo=ahi=current_accel;
456 lastxlo=xlo;lastxhi=xhi;
457 lastalo=alo;lastahi=ahi;
463 if(xlo < xmin) xlo=xmin;
465 if(xhi > xmax) xhi=xmax;
466 TrimAxes[current_axis]->SetControl(xlo);
467 TrimAxes[current_axis]->Run();
468 alo=TrimAxes[current_axis]->GetState();
469 TrimAxes[current_axis]->SetControl(xhi);
470 TrimAxes[current_axis]->Run();
471 ahi=TrimAxes[current_axis]->GetState();
472 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
473 if(alo*ahi <=0) { //found interval with root
475 if(alo*current_accel <= 0) { //narrow interval down a bit
479 //xhi=current_control;
485 //xlo=current_control;
489 lastxlo=xlo;lastxhi=xhi;
490 lastalo=alo;lastahi=ahi;
491 if( !found && xlo==xmin && xhi==xmax ) continue;
493 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
494 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
495 } while(!found && (Nsub <= max_sub_iterations) );
499 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
500 //checks to see which side of the current control value the solution is on
501 //and sets solutionDomain accordingly:
502 // 1 if solution is between the current and max
503 // -1 if solution is between the min and current
504 // 0 if there is no solution
506 //if changing the control produces no significant change in the accel then
507 //solutionDomain is set to zero and the function returns false
508 //if a solution is found, then xlo and xhi are set so that they bracket
509 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
510 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
511 //xhi=xmax and ahi=accel(xmax)
512 //in all cases the sim is left such that the control=xmax and accel=ahi
514 bool FGTrim::checkLimits(void) {
516 double current_control=TrimAxes[current_axis]->GetControl();
517 double current_accel=TrimAxes[current_axis]->GetState();
518 xlo=TrimAxes[current_axis]->GetControlMin();
519 xhi=TrimAxes[current_axis]->GetControlMax();
521 TrimAxes[current_axis]->SetControl(xlo);
522 TrimAxes[current_axis]->Run();
523 alo=TrimAxes[current_axis]->GetState();
524 TrimAxes[current_axis]->SetControl(xhi);
525 TrimAxes[current_axis]->Run();
526 ahi=TrimAxes[current_axis]->GetState();
528 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
529 << alo << ", " << ahi << endl;
531 solutionExists=false;
532 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
533 if(alo*current_accel <= 0) {
538 } else if(current_accel*ahi < 0){
545 TrimAxes[current_axis]->SetControl(current_control);
546 TrimAxes[current_axis]->Run();
547 return solutionExists;
550 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
552 void FGTrim::setupPullup() {
554 g=fdmex->GetInertial()->gravity();
555 cgamma=cos(fgic->GetFlightPathAngleRadIC());
556 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
557 << fgic->GetVtrueFpsIC() << endl;
558 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
559 cout << targetNlf << ", " << q << endl;
560 fgic->SetQRadpsIC(q);
561 cout << "setPitchRateInPullup() complete" << endl;
565 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
567 void FGTrim::setupTurn(void){
569 phi = fgic->GetPhiRadIC();
570 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
571 targetNlf = 1 / cos(phi);
572 g = fdmex->GetInertial()->gravity();
573 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
574 cout << targetNlf << ", " << psidot << endl;
579 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
581 void FGTrim::updateRates(void){
582 if( mode == tTurn ) {
583 double phi = fgic->GetPhiRadIC();
584 double g = fdmex->GetInertial()->gravity();
586 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
587 theta=fgic->GetThetaRadIC();
588 phi=fgic->GetPhiRadIC();
589 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
590 p=-psidot*sin(theta);
591 q=psidot*cos(theta)*sin(phi);
592 r=psidot*cos(theta)*cos(phi);
596 fgic->SetPRadpsIC(p);
597 fgic->SetQRadpsIC(q);
598 fgic->SetRRadpsIC(r);
599 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
601 g=fdmex->GetInertial()->gravity();
602 cgamma=cos(fgic->GetFlightPathAngleRadIC());
603 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
604 fgic->SetQRadpsIC(q);
608 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
610 void FGTrim::setDebug(void) {
611 if(debug_axis == tAll ||
612 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
621 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
623 void FGTrim::SetMode(TrimMode tt) {
629 cout << " Full Trim" << endl;
630 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
631 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
632 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
633 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
634 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
635 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
636 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
640 cout << " Longitudinal Trim" << endl;
641 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
642 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
643 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
647 cout << " Ground Trim" << endl;
648 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
649 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
650 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
653 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
654 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
655 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
656 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
659 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
663 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
664 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
665 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
666 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
667 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
673 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
674 sub_iterations=new double[TrimAxes.size()];
675 successful=new double[TrimAxes.size()];
676 solution=new bool[TrimAxes.size()];
679 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.