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 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 General Public License for more
19 You should have received a copy of the GNU 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 General Public License can also be found on
24 the world wide web at http://www.gnu.org.
28 --------------------------------------------------------------------------------
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
35 This class takes the given set of IC's and finds the angle of attack, elevator,
36 and throttle setting required to fly steady level. This is currently for in-air
37 conditions only. It is implemented using an iterative, one-axis-at-a-time
40 // !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
43 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
45 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
49 #include "FGFDMExec.h"
50 #include "FGAtmosphere.h"
51 #include "FGInitialCondition.h"
53 #include "FGAircraft.h"
54 #include "FGMassBalance.h"
55 #include "FGAerodynamics.h"
56 #include "FGColumnVector3.h"
58 #pragma warning (disable : 4786 4788)
63 static const char *IdSrc = "$Id$";
64 static const char *IdHdr = ID_TRIM;
66 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
68 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
72 max_sub_iterations=100;
74 A_Tolerance = Tolerance / 10;
88 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
91 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
93 FGTrim::~FGTrim(void) {
94 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
95 delete TrimAxes[current_axis];
97 delete[] sub_iterations;
100 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
103 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
105 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 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
115 TrimAxes[current_axis]->GetStateName().c_str(),
116 sub_iterations[current_axis],
117 sub_iterations[current_axis]/double(total_its),
118 successful[current_axis],
119 TrimAxes[current_axis]->GetAvgStability() );
122 cout << " Run Count: " << run_sum << endl;
126 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 void FGTrim::Report(void) {
129 cout << " Trim Results: " << endl;
130 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
131 TrimAxes[current_axis]->AxisReport();
135 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
137 void FGTrim::ClearStates(void) {
141 vector<FGTrimAxis*>::iterator iAxes;
142 iAxes = TrimAxes.begin();
143 while (iAxes != TrimAxes.end()) {
149 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
154 bool FGTrim::AddState( State state, Control control ) {
159 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
160 while (iAxes != TrimAxes.end()) {
162 if( ta->GetStateType() == state )
167 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
168 delete[] sub_iterations;
171 sub_iterations=new double[TrimAxes.size()];
172 successful=new double[TrimAxes.size()];
173 solution=new bool[TrimAxes.size()];
178 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
180 bool FGTrim::RemoveState( State state ) {
185 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
186 while (iAxes != TrimAxes.end()) {
188 if( ta->GetStateType() == state ) {
190 TrimAxes.erase(iAxes);
197 delete[] sub_iterations;
200 sub_iterations=new double[TrimAxes.size()];
201 successful=new double[TrimAxes.size()];
202 solution=new bool[TrimAxes.size()];
207 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 bool FGTrim::EditState( State state, Control new_control ){
214 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
215 while (iAxes != TrimAxes.end()) {
217 if( ta->GetStateType() == state ) {
218 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
220 TrimAxes.erase(iAxes+1);
230 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
232 bool FGTrim::DoTrim(void) {
237 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
238 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
241 fdmex->GetOutput()->Disable();
243 //clear the sub iterations counts & zero out the controls
244 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
245 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
246 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
247 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
248 if(mode == tGround) {
249 TrimAxes[current_axis]->initTheta();
252 xlo=TrimAxes[current_axis]->GetControlMin();
253 xhi=TrimAxes[current_axis]->GetControlMax();
254 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
255 TrimAxes[current_axis]->Run();
256 //TrimAxes[current_axis]->AxisReport();
257 sub_iterations[current_axis]=0;
258 successful[current_axis]=0;
259 solution[current_axis]=false;
263 if(mode == tPullup ) {
264 cout << "Setting pitch rate and nlf... " << endl;
266 cout << "pitch rate done ... " << endl;
267 TrimAxes[0]->SetStateTarget(targetNlf);
268 cout << "nlf done" << endl;
269 } else if (mode == tTurn) {
271 //TrimAxes[0]->SetStateTarget(targetNlf);
276 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
280 if(!solution[current_axis]) {
282 solution[current_axis]=true;
285 } else if(findInterval()) {
288 solution[current_axis]=false;
290 sub_iterations[current_axis]+=Nsub;
292 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
293 //these checks need to be done after all the axes have run
294 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
295 if(TrimAxes[current_axis]->InTolerance()) {
297 successful[current_axis]++;
302 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
303 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
304 //At this point we can check the input limits of the failed axis
305 //and declare the trim failed if there is no sign change. If there
306 //is, keep going until success or max iteration count
308 //Oh, well: two out of three ain't bad
309 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
310 //these checks need to be done after all the axes have run
311 if(!TrimAxes[current_axis]->InTolerance()) {
313 // special case this for now -- if other cases arise proper
314 // support can be added to FGTrimAxis
315 if( (gamma_fallback) &&
316 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
317 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
318 cout << " Can't trim udot with throttle, trying flight"
319 << " path angle. (" << N << ")" << endl;
320 if(TrimAxes[current_axis]->GetState() > 0)
321 TrimAxes[current_axis]->SetControlToMin();
323 TrimAxes[current_axis]->SetControlToMax();
324 TrimAxes[current_axis]->Run();
325 delete TrimAxes[current_axis];
326 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
329 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
330 << " doesn't appear to be trimmable" << endl;
332 trim_failed=true; //force the trim to fail
337 } //all-but-one check
339 if(N > max_iterations)
341 } while((axis_count < TrimAxes.size()) && (!trim_failed));
342 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
344 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->GetOutput()->Enable();
356 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
358 bool FGTrim::solve(void) {
360 double x1,x2,x3,f1,f2,f3,d,d0;
361 const double relax =0.9;
362 double eps=TrimAxes[current_axis]->GetSolverEps();
368 if( solutionDomain != 0) {
369 /* if(ahi > alo) { */
378 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
379 while ( (TrimAxes[current_axis]->InTolerance() == false )
380 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
383 x2=x1-d*d0*f1/(f3-f1);
384 TrimAxes[current_axis]->SetControl(x2);
385 TrimAxes[current_axis]->Run();
386 f2=TrimAxes[current_axis]->GetState();
388 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
389 << ", " << x2 << ", " << x3 << endl;
390 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
396 //cout << "Solution is between x1 and x2" << endl;
398 else if(f2*f3 <= 0.0) {
402 //cout << "Solution is between x2 and x3" << endl;
409 if(Nsub < max_sub_iterations) success=true;
414 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
416 produces an interval (xlo..xhi) on one side or the other of the current
417 control value in which a solution exists. This domain is, hopefully,
418 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
419 to find the solution. This is, hopefully, more efficient than having the
420 solver start from scratch every time. Maybe it isn't though...
421 This tries to take advantage of the idea that the changes from iteration to
422 iteration will be small after the first one or two top-level iterations.
424 assumes that changing the control will a produce significant change in the
425 accel i.e. checkLimits() has already been called.
427 if a solution is found above the current control, the function returns true
428 and xlo is set to the current control, xhi to the interval max it found, and
429 solutionDomain is set to 1.
430 if the solution lies below the current control, then the function returns
431 true and xlo is set to the interval min it found and xmax to the current
432 control. if no solution is found, then the function returns false.
435 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
436 no assumptions about the state of the sim after this function has run
439 bool FGTrim::findInterval(void) {
442 double current_control=TrimAxes[current_axis]->GetControl();
443 double current_accel=TrimAxes[current_axis]->GetState();;
444 double xmin=TrimAxes[current_axis]->GetControlMin();
445 double xmax=TrimAxes[current_axis]->GetControlMax();
446 double lastxlo,lastxhi,lastalo,lastahi;
448 step=0.025*fabs(xmax);
449 xlo=xhi=current_control;
450 alo=ahi=current_accel;
451 lastxlo=xlo;lastxhi=xhi;
452 lastalo=alo;lastahi=ahi;
458 if(xlo < xmin) xlo=xmin;
460 if(xhi > xmax) xhi=xmax;
461 TrimAxes[current_axis]->SetControl(xlo);
462 TrimAxes[current_axis]->Run();
463 alo=TrimAxes[current_axis]->GetState();
464 TrimAxes[current_axis]->SetControl(xhi);
465 TrimAxes[current_axis]->Run();
466 ahi=TrimAxes[current_axis]->GetState();
467 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
468 if(alo*ahi <=0) { //found interval with root
470 if(alo*current_accel <= 0) { //narrow interval down a bit
474 //xhi=current_control;
480 //xlo=current_control;
484 lastxlo=xlo;lastxhi=xhi;
485 lastalo=alo;lastahi=ahi;
486 if( !found && xlo==xmin && xhi==xmax ) continue;
488 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
489 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
490 } while(!found && (Nsub <= max_sub_iterations) );
494 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
495 //checks to see which side of the current control value the solution is on
496 //and sets solutionDomain accordingly:
497 // 1 if solution is between the current and max
498 // -1 if solution is between the min and current
499 // 0 if there is no solution
501 //if changing the control produces no significant change in the accel then
502 //solutionDomain is set to zero and the function returns false
503 //if a solution is found, then xlo and xhi are set so that they bracket
504 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
505 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
506 //xhi=xmax and ahi=accel(xmax)
507 //in all cases the sim is left such that the control=xmax and accel=ahi
509 bool FGTrim::checkLimits(void) {
511 double current_control=TrimAxes[current_axis]->GetControl();
512 double current_accel=TrimAxes[current_axis]->GetState();
513 xlo=TrimAxes[current_axis]->GetControlMin();
514 xhi=TrimAxes[current_axis]->GetControlMax();
516 TrimAxes[current_axis]->SetControl(xlo);
517 TrimAxes[current_axis]->Run();
518 alo=TrimAxes[current_axis]->GetState();
519 TrimAxes[current_axis]->SetControl(xhi);
520 TrimAxes[current_axis]->Run();
521 ahi=TrimAxes[current_axis]->GetState();
523 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
524 << alo << ", " << ahi << endl;
526 solutionExists=false;
527 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
528 if(alo*current_accel <= 0) {
533 } else if(current_accel*ahi < 0){
540 TrimAxes[current_axis]->SetControl(current_control);
541 TrimAxes[current_axis]->Run();
542 return solutionExists;
545 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
547 void FGTrim::setupPullup() {
549 FGColumnVector3 vPQR;
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 fdmex->GetRotation()->SetPQR(0,q,0);
557 cout << "setPitchRateInPullup() complete" << endl;
561 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
563 void FGTrim::setupTurn(void){
565 phi = fgic->GetRollAngleRadIC();
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->GetRollAngleRadIC();
580 double g = fdmex->GetInertial()->gravity();
582 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
583 theta=fgic->GetPitchAngleRadIC();
584 phi=fgic->GetRollAngleRadIC();
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 fdmex->GetRotation()->SetPQR(p,q,r);
593 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
595 FGColumnVector3 vPQR;
596 g=fdmex->GetInertial()->gravity();
597 cgamma=cos(fgic->GetFlightPathAngleRadIC());
598 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
599 fdmex->GetRotation()->SetPQR(0,q,0);
603 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
605 void FGTrim::setDebug(void) {
606 if(debug_axis == tAll ||
607 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
616 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
618 void FGTrim::SetMode(TrimMode tt) {
623 cout << " Full Trim" << endl;
624 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
625 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
626 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
627 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
628 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
629 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
630 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
633 cout << " Longitudinal Trim" << endl;
634 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
635 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
636 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
639 cout << " Ground Trim" << endl;
640 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
641 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
642 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
645 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
646 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
647 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
648 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
649 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
650 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
651 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
654 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
655 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
656 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
659 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
665 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
666 sub_iterations=new double[TrimAxes.size()];
667 successful=new double[TrimAxes.size()];
668 solution=new bool[TrimAxes.size()];
671 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.