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"
57 #pragma warning (disable : 4786 4788)
60 static const char *IdSrc = "$Id$";
61 static const char *IdHdr = ID_TRIM;
63 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
65 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
69 max_sub_iterations=100;
71 A_Tolerance = Tolerance / 10;
84 cout << " Full Trim" << endl;
85 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
86 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
87 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
88 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
89 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
90 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
91 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
94 cout << " Longitudinal Trim" << endl;
95 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
96 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
97 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
100 cout << " Ground Trim" << endl;
101 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
102 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
103 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
106 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
107 sub_iterations=new double[TrimAxes.size()];
108 successful=new double[TrimAxes.size()];
109 solution=new bool[TrimAxes.size()];
112 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
115 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
117 FGTrim::~FGTrim(void) {
118 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
119 delete TrimAxes[current_axis];
121 delete[] sub_iterations;
124 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
127 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
129 void FGTrim::TrimStats() {
132 cout << endl << " Trim Statistics: " << endl;
133 cout << " Total Iterations: " << total_its << endl;
135 cout << " Sub-iterations:" << endl;
136 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
137 run_sum+=TrimAxes[current_axis]->GetRunCount();
138 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
139 TrimAxes[current_axis]->GetStateName().c_str(),
140 sub_iterations[current_axis],
141 sub_iterations[current_axis]/double(total_its),
142 successful[current_axis],
143 TrimAxes[current_axis]->GetAvgStability() );
146 cout << " Run Count: " << run_sum << endl;
150 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
152 void FGTrim::Report(void) {
153 cout << " Trim Results: " << endl;
154 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
155 TrimAxes[current_axis]->AxisReport();
159 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
161 void FGTrim::ClearStates(void) {
165 vector<FGTrimAxis*>::iterator iAxes;
166 iAxes = TrimAxes.begin();
167 while (iAxes != TrimAxes.end()) {
173 cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
176 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
178 bool FGTrim::AddState( State state, Control control ) {
183 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
184 while (iAxes != TrimAxes.end()) {
186 if( ta->GetStateType() == state )
191 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
192 delete[] sub_iterations;
195 sub_iterations=new double[TrimAxes.size()];
196 successful=new double[TrimAxes.size()];
197 solution=new bool[TrimAxes.size()];
202 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
204 bool FGTrim::RemoveState( State state ) {
209 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
210 while (iAxes != TrimAxes.end()) {
212 if( ta->GetStateType() == state ) {
214 TrimAxes.erase(iAxes);
221 delete[] sub_iterations;
224 sub_iterations=new double[TrimAxes.size()];
225 successful=new double[TrimAxes.size()];
226 solution=new bool[TrimAxes.size()];
231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
233 bool FGTrim::EditState( State state, Control new_control ){
238 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
239 while (iAxes != TrimAxes.end()) {
241 if( ta->GetStateType() == state ) {
242 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
244 TrimAxes.erase(iAxes+1);
254 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
256 bool FGTrim::DoTrim(void) {
261 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
262 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
265 fdmex->GetOutput()->Disable();
267 //clear the sub iterations counts & zero out the controls
268 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
269 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
270 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
271 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
273 TrimAxes[current_axis]->initTheta();
275 xlo=TrimAxes[current_axis]->GetControlMin();
276 xhi=TrimAxes[current_axis]->GetControlMax();
277 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
278 TrimAxes[current_axis]->Run();
279 //TrimAxes[current_axis]->AxisReport();
280 sub_iterations[current_axis]=0;
281 successful[current_axis]=0;
282 solution[current_axis]=false;
286 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
288 if(!solution[current_axis]) {
290 solution[current_axis]=true;
293 } else if(findInterval()) {
296 solution[current_axis]=false;
298 sub_iterations[current_axis]+=Nsub;
300 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
301 //these checks need to be done after all the axes have run
302 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
303 if(TrimAxes[current_axis]->InTolerance()) {
305 successful[current_axis]++;
310 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
311 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
312 //At this point we can check the input limits of the failed axis
313 //and declare the trim failed if there is no sign change. If there
314 //is, keep going until success or max iteration count
316 //Oh, well: two out of three ain't bad
317 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
318 //these checks need to be done after all the axes have run
319 if(!TrimAxes[current_axis]->InTolerance()) {
321 // special case this for now -- if other cases arise proper
322 // support can be added to FGTrimAxis
323 if( (gamma_fallback) &&
324 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
325 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
326 cout << " Can't trim udot with throttle, trying flight"
327 << " path angle. (" << N << ")" << endl;
328 if(TrimAxes[current_axis]->GetState() > 0)
329 TrimAxes[current_axis]->SetControlToMin();
331 TrimAxes[current_axis]->SetControlToMax();
332 TrimAxes[current_axis]->Run();
333 delete TrimAxes[current_axis];
334 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
337 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
338 << " doesn't appear to be trimmable" << endl;
340 trim_failed=true; //force the trim to fail
345 } //all-but-one check
347 if(N > max_iterations)
349 } while((axis_count < TrimAxes.size()) && (!trim_failed));
350 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
352 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->GetOutput()->Enable();
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 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.