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.
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 !!!!!!!
42 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
44 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
48 #include <FGFDMExec.h>
49 #include <models/FGAtmosphere.h>
50 #include "FGInitialCondition.h"
52 #include <models/FGAircraft.h>
53 #include <models/FGMassBalance.h>
54 #include <models/FGGroundReactions.h>
55 #include <models/FGInertial.h>
56 #include <models/FGAerodynamics.h>
57 #include <math/FGColumnVector3.h>
60 #pragma warning (disable : 4786 4788)
65 static const char *IdSrc = "$Id$";
66 static const char *IdHdr = ID_TRIM;
68 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
70 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
74 max_sub_iterations=100;
76 A_Tolerance = Tolerance / 10;
90 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
93 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95 FGTrim::~FGTrim(void) {
96 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
97 delete TrimAxes[current_axis];
99 delete[] sub_iterations;
102 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
105 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 void FGTrim::TrimStats() {
110 cout << endl << " Trim Statistics: " << endl;
111 cout << " Total Iterations: " << total_its << endl;
113 cout << " Sub-iterations:" << endl;
114 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
115 run_sum+=TrimAxes[current_axis]->GetRunCount();
116 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
117 TrimAxes[current_axis]->GetStateName().c_str(),
118 sub_iterations[current_axis],
119 sub_iterations[current_axis]/double(total_its),
120 successful[current_axis],
121 TrimAxes[current_axis]->GetAvgStability() );
124 cout << " Run Count: " << run_sum << endl;
128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130 void FGTrim::Report(void) {
131 cout << " Trim Results: " << endl;
132 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
133 TrimAxes[current_axis]->AxisReport();
137 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
139 void FGTrim::ClearStates(void) {
143 vector<FGTrimAxis*>::iterator iAxes;
144 iAxes = TrimAxes.begin();
145 while (iAxes != TrimAxes.end()) {
151 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
154 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
156 bool FGTrim::AddState( State state, Control control ) {
161 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
162 while (iAxes != TrimAxes.end()) {
164 if( ta->GetStateType() == state )
169 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
170 delete[] sub_iterations;
173 sub_iterations=new double[TrimAxes.size()];
174 successful=new double[TrimAxes.size()];
175 solution=new bool[TrimAxes.size()];
180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
182 bool FGTrim::RemoveState( State state ) {
187 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
188 while (iAxes != TrimAxes.end()) {
190 if( ta->GetStateType() == state ) {
192 TrimAxes.erase(iAxes);
199 delete[] sub_iterations;
202 sub_iterations=new double[TrimAxes.size()];
203 successful=new double[TrimAxes.size()];
204 solution=new bool[TrimAxes.size()];
209 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
211 bool FGTrim::EditState( State state, Control new_control ){
216 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
217 while (iAxes != TrimAxes.end()) {
219 if( ta->GetStateType() == state ) {
220 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
222 TrimAxes.erase(iAxes+1);
232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
234 bool FGTrim::DoTrim(void) {
239 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
240 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
243 fdmex->DisableOutput();
245 fgic->SetPRadpsIC(0.0);
246 fgic->SetQRadpsIC(0.0);
247 fgic->SetRRadpsIC(0.0);
249 //clear the sub iterations counts & zero out the controls
250 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
251 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
252 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
253 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
254 if(mode == tGround) {
255 TrimAxes[current_axis]->initTheta();
258 xlo=TrimAxes[current_axis]->GetControlMin();
259 xhi=TrimAxes[current_axis]->GetControlMax();
260 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
261 TrimAxes[current_axis]->Run();
262 //TrimAxes[current_axis]->AxisReport();
263 sub_iterations[current_axis]=0;
264 successful[current_axis]=0;
265 solution[current_axis]=false;
269 if(mode == tPullup ) {
270 cout << "Setting pitch rate and nlf... " << endl;
272 cout << "pitch rate done ... " << endl;
273 TrimAxes[0]->SetStateTarget(targetNlf);
274 cout << "nlf done" << endl;
275 } else if (mode == tTurn) {
277 //TrimAxes[0]->SetStateTarget(targetNlf);
282 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
286 if(!solution[current_axis]) {
288 solution[current_axis]=true;
291 } else if(findInterval()) {
294 solution[current_axis]=false;
296 sub_iterations[current_axis]+=Nsub;
298 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
299 //these checks need to be done after all the axes have run
300 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
301 if(TrimAxes[current_axis]->InTolerance()) {
303 successful[current_axis]++;
308 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
309 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
310 //At this point we can check the input limits of the failed axis
311 //and declare the trim failed if there is no sign change. If there
312 //is, keep going until success or max iteration count
314 //Oh, well: two out of three ain't bad
315 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
316 //these checks need to be done after all the axes have run
317 if(!TrimAxes[current_axis]->InTolerance()) {
319 // special case this for now -- if other cases arise proper
320 // support can be added to FGTrimAxis
321 if( (gamma_fallback) &&
322 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
323 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
324 cout << " Can't trim udot with throttle, trying flight"
325 << " path angle. (" << N << ")" << endl;
326 if(TrimAxes[current_axis]->GetState() > 0)
327 TrimAxes[current_axis]->SetControlToMin();
329 TrimAxes[current_axis]->SetControlToMax();
330 TrimAxes[current_axis]->Run();
331 delete TrimAxes[current_axis];
332 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
335 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
336 << " doesn't appear to be trimmable" << endl;
338 trim_failed=true; //force the trim to fail
343 } //all-but-one check
345 if(N > max_iterations)
347 } while((axis_count < TrimAxes.size()) && (!trim_failed));
348 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
351 cout << endl << " Trim successful" << endl;
355 cout << endl << " Trim failed" << endl;
357 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
358 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
360 fdmex->EnableOutput();
364 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
366 bool FGTrim::solve(void) {
368 double x1,x2,x3,f1,f2,f3,d,d0;
369 const double relax =0.9;
370 double eps=TrimAxes[current_axis]->GetSolverEps();
376 if( solutionDomain != 0) {
377 /* if(ahi > alo) { */
386 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
387 while ( (TrimAxes[current_axis]->InTolerance() == false )
388 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
391 x2=x1-d*d0*f1/(f3-f1);
392 TrimAxes[current_axis]->SetControl(x2);
393 TrimAxes[current_axis]->Run();
394 f2=TrimAxes[current_axis]->GetState();
396 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
397 << ", " << x2 << ", " << x3 << endl;
398 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
404 //cout << "Solution is between x1 and x2" << endl;
406 else if(f2*f3 <= 0.0) {
410 //cout << "Solution is between x2 and x3" << endl;
417 if(Nsub < max_sub_iterations) success=true;
422 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
424 produces an interval (xlo..xhi) on one side or the other of the current
425 control value in which a solution exists. This domain is, hopefully,
426 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
427 to find the solution. This is, hopefully, more efficient than having the
428 solver start from scratch every time. Maybe it isn't though...
429 This tries to take advantage of the idea that the changes from iteration to
430 iteration will be small after the first one or two top-level iterations.
432 assumes that changing the control will a produce significant change in the
433 accel i.e. checkLimits() has already been called.
435 if a solution is found above the current control, the function returns true
436 and xlo is set to the current control, xhi to the interval max it found, and
437 solutionDomain is set to 1.
438 if the solution lies below the current control, then the function returns
439 true and xlo is set to the interval min it found and xmax to the current
440 control. if no solution is found, then the function returns false.
443 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
444 no assumptions about the state of the sim after this function has run
447 bool FGTrim::findInterval(void) {
450 double current_control=TrimAxes[current_axis]->GetControl();
451 double current_accel=TrimAxes[current_axis]->GetState();;
452 double xmin=TrimAxes[current_axis]->GetControlMin();
453 double xmax=TrimAxes[current_axis]->GetControlMax();
454 double lastxlo,lastxhi,lastalo,lastahi;
456 step=0.025*fabs(xmax);
457 xlo=xhi=current_control;
458 alo=ahi=current_accel;
459 lastxlo=xlo;lastxhi=xhi;
460 lastalo=alo;lastahi=ahi;
466 if(xlo < xmin) xlo=xmin;
468 if(xhi > xmax) xhi=xmax;
469 TrimAxes[current_axis]->SetControl(xlo);
470 TrimAxes[current_axis]->Run();
471 alo=TrimAxes[current_axis]->GetState();
472 TrimAxes[current_axis]->SetControl(xhi);
473 TrimAxes[current_axis]->Run();
474 ahi=TrimAxes[current_axis]->GetState();
475 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
476 if(alo*ahi <=0) { //found interval with root
478 if(alo*current_accel <= 0) { //narrow interval down a bit
482 //xhi=current_control;
488 //xlo=current_control;
492 lastxlo=xlo;lastxhi=xhi;
493 lastalo=alo;lastahi=ahi;
494 if( !found && xlo==xmin && xhi==xmax ) continue;
496 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
497 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
498 } while(!found && (Nsub <= max_sub_iterations) );
502 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
503 //checks to see which side of the current control value the solution is on
504 //and sets solutionDomain accordingly:
505 // 1 if solution is between the current and max
506 // -1 if solution is between the min and current
507 // 0 if there is no solution
509 //if changing the control produces no significant change in the accel then
510 //solutionDomain is set to zero and the function returns false
511 //if a solution is found, then xlo and xhi are set so that they bracket
512 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
513 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
514 //xhi=xmax and ahi=accel(xmax)
515 //in all cases the sim is left such that the control=xmax and accel=ahi
517 bool FGTrim::checkLimits(void) {
519 double current_control=TrimAxes[current_axis]->GetControl();
520 double current_accel=TrimAxes[current_axis]->GetState();
521 xlo=TrimAxes[current_axis]->GetControlMin();
522 xhi=TrimAxes[current_axis]->GetControlMax();
524 TrimAxes[current_axis]->SetControl(xlo);
525 TrimAxes[current_axis]->Run();
526 alo=TrimAxes[current_axis]->GetState();
527 TrimAxes[current_axis]->SetControl(xhi);
528 TrimAxes[current_axis]->Run();
529 ahi=TrimAxes[current_axis]->GetState();
531 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
532 << alo << ", " << ahi << endl;
534 solutionExists=false;
535 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
536 if(alo*current_accel <= 0) {
541 } else if(current_accel*ahi < 0){
548 TrimAxes[current_axis]->SetControl(current_control);
549 TrimAxes[current_axis]->Run();
550 return solutionExists;
553 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
555 void FGTrim::setupPullup() {
557 g=fdmex->GetInertial()->gravity();
558 cgamma=cos(fgic->GetFlightPathAngleRadIC());
559 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
560 << fgic->GetVtrueFpsIC() << endl;
561 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
562 cout << targetNlf << ", " << q << endl;
563 fgic->SetQRadpsIC(q);
564 cout << "setPitchRateInPullup() complete" << endl;
568 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
570 void FGTrim::setupTurn(void){
572 phi = fgic->GetPhiRadIC();
573 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
574 targetNlf = 1 / cos(phi);
575 g = fdmex->GetInertial()->gravity();
576 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
577 cout << targetNlf << ", " << psidot << endl;
582 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
584 void FGTrim::updateRates(void){
585 if( mode == tTurn ) {
586 double phi = fgic->GetPhiRadIC();
587 double g = fdmex->GetInertial()->gravity();
589 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
590 theta=fgic->GetThetaRadIC();
591 phi=fgic->GetPhiRadIC();
592 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
593 p=-psidot*sin(theta);
594 q=psidot*cos(theta)*sin(phi);
595 r=psidot*cos(theta)*cos(phi);
599 fgic->SetPRadpsIC(p);
600 fgic->SetQRadpsIC(q);
601 fgic->SetRRadpsIC(r);
602 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
604 g=fdmex->GetInertial()->gravity();
605 cgamma=cos(fgic->GetFlightPathAngleRadIC());
606 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
607 fgic->SetQRadpsIC(q);
611 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
613 void FGTrim::setDebug(void) {
614 if(debug_axis == tAll ||
615 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
624 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
626 void FGTrim::SetMode(TrimMode tt) {
632 cout << " Full Trim" << endl;
633 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
634 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
635 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
636 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
637 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
638 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
639 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
643 cout << " Longitudinal Trim" << endl;
644 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
645 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
646 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
650 cout << " Ground Trim" << endl;
651 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
652 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
653 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
656 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
659 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
660 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
661 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
665 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
666 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
667 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
668 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
669 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
670 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
676 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
677 sub_iterations=new double[TrimAxes.size()];
678 successful=new double[TrimAxes.size()];
679 solution=new bool[TrimAxes.size()];
682 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.