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)
61 static const char *IdSrc = "$Id$";
62 static const char *IdHdr = ID_TRIM;
64 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
66 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
70 max_sub_iterations=100;
72 A_Tolerance = Tolerance / 10;
87 cout << " Full Trim" << endl;
88 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
89 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
90 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
91 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
92 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
93 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
94 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
97 cout << " Longitudinal Trim" << endl;
98 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
99 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
100 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
103 cout << " Ground Trim" << endl;
104 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
105 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
106 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
109 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
110 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
111 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
112 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
113 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
114 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
115 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
118 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
119 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
120 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
121 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
122 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
123 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
127 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
128 sub_iterations=new double[TrimAxes.size()];
129 successful=new double[TrimAxes.size()];
130 solution=new bool[TrimAxes.size()];
133 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
136 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
138 FGTrim::~FGTrim(void) {
139 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
140 delete TrimAxes[current_axis];
142 delete[] sub_iterations;
145 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
148 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
150 void FGTrim::TrimStats() {
153 cout << endl << " Trim Statistics: " << endl;
154 cout << " Total Iterations: " << total_its << endl;
156 cout << " Sub-iterations:" << endl;
157 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
158 run_sum+=TrimAxes[current_axis]->GetRunCount();
159 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
160 TrimAxes[current_axis]->GetStateName().c_str(),
161 sub_iterations[current_axis],
162 sub_iterations[current_axis]/double(total_its),
163 successful[current_axis],
164 TrimAxes[current_axis]->GetAvgStability() );
167 cout << " Run Count: " << run_sum << endl;
171 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
173 void FGTrim::Report(void) {
174 cout << " Trim Results: " << endl;
175 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
176 TrimAxes[current_axis]->AxisReport();
180 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
182 void FGTrim::ClearStates(void) {
186 vector<FGTrimAxis*>::iterator iAxes;
187 iAxes = TrimAxes.begin();
188 while (iAxes != TrimAxes.end()) {
194 cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
197 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
199 bool FGTrim::AddState( State state, Control control ) {
204 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
205 while (iAxes != TrimAxes.end()) {
207 if( ta->GetStateType() == state )
212 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
213 delete[] sub_iterations;
216 sub_iterations=new double[TrimAxes.size()];
217 successful=new double[TrimAxes.size()];
218 solution=new bool[TrimAxes.size()];
223 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
225 bool FGTrim::RemoveState( State state ) {
230 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
231 while (iAxes != TrimAxes.end()) {
233 if( ta->GetStateType() == state ) {
235 TrimAxes.erase(iAxes);
242 delete[] sub_iterations;
245 sub_iterations=new double[TrimAxes.size()];
246 successful=new double[TrimAxes.size()];
247 solution=new bool[TrimAxes.size()];
252 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
254 bool FGTrim::EditState( State state, Control new_control ){
259 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
260 while (iAxes != TrimAxes.end()) {
262 if( ta->GetStateType() == state ) {
263 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
265 TrimAxes.erase(iAxes+1);
275 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
277 bool FGTrim::DoTrim(void) {
282 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
283 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
286 fdmex->GetOutput()->Disable();
288 //clear the sub iterations counts & zero out the controls
289 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
290 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
291 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
292 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
294 TrimAxes[current_axis]->initTheta();
296 xlo=TrimAxes[current_axis]->GetControlMin();
297 xhi=TrimAxes[current_axis]->GetControlMax();
298 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
299 TrimAxes[current_axis]->Run();
300 //TrimAxes[current_axis]->AxisReport();
301 sub_iterations[current_axis]=0;
302 successful[current_axis]=0;
303 solution[current_axis]=false;
307 if(mode == tPullup ) {
308 cout << "Setting pitch rate and nlf... " << endl;
310 cout << "pitch rate done ... " << endl;
311 TrimAxes[0]->SetStateTarget(targetNlf);
312 cout << "nlf done" << endl;
313 } else if (mode == tTurn) {
315 TrimAxes[0]->SetStateTarget(targetNlf);
320 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
324 if(!solution[current_axis]) {
326 solution[current_axis]=true;
329 } else if(findInterval()) {
332 solution[current_axis]=false;
334 sub_iterations[current_axis]+=Nsub;
336 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
337 //these checks need to be done after all the axes have run
338 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
339 if(TrimAxes[current_axis]->InTolerance()) {
341 successful[current_axis]++;
346 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
347 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
348 //At this point we can check the input limits of the failed axis
349 //and declare the trim failed if there is no sign change. If there
350 //is, keep going until success or max iteration count
352 //Oh, well: two out of three ain't bad
353 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
354 //these checks need to be done after all the axes have run
355 if(!TrimAxes[current_axis]->InTolerance()) {
357 // special case this for now -- if other cases arise proper
358 // support can be added to FGTrimAxis
359 if( (gamma_fallback) &&
360 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
361 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
362 cout << " Can't trim udot with throttle, trying flight"
363 << " path angle. (" << N << ")" << endl;
364 if(TrimAxes[current_axis]->GetState() > 0)
365 TrimAxes[current_axis]->SetControlToMin();
367 TrimAxes[current_axis]->SetControlToMax();
368 TrimAxes[current_axis]->Run();
369 delete TrimAxes[current_axis];
370 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
373 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
374 << " doesn't appear to be trimmable" << endl;
376 trim_failed=true; //force the trim to fail
381 } //all-but-one check
383 if(N > max_iterations)
385 } while((axis_count < TrimAxes.size()) && (!trim_failed));
386 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
388 cout << endl << " Trim successful" << endl;
391 cout << endl << " Trim failed" << endl;
393 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
394 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
396 fdmex->GetOutput()->Enable();
400 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
402 bool FGTrim::solve(void) {
404 double x1,x2,x3,f1,f2,f3,d,d0;
405 const double relax =0.9;
406 double eps=TrimAxes[current_axis]->GetSolverEps();
412 if( solutionDomain != 0) {
413 /* if(ahi > alo) { */
422 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
423 while ( (TrimAxes[current_axis]->InTolerance() == false )
424 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
427 x2=x1-d*d0*f1/(f3-f1);
428 TrimAxes[current_axis]->SetControl(x2);
429 TrimAxes[current_axis]->Run();
430 f2=TrimAxes[current_axis]->GetState();
432 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
433 << ", " << x2 << ", " << x3 << endl;
434 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
440 //cout << "Solution is between x1 and x2" << endl;
442 else if(f2*f3 <= 0.0) {
446 //cout << "Solution is between x2 and x3" << endl;
453 if(Nsub < max_sub_iterations) success=true;
458 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
460 produces an interval (xlo..xhi) on one side or the other of the current
461 control value in which a solution exists. This domain is, hopefully,
462 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
463 to find the solution. This is, hopefully, more efficient than having the
464 solver start from scratch every time. Maybe it isn't though...
465 This tries to take advantage of the idea that the changes from iteration to
466 iteration will be small after the first one or two top-level iterations.
468 assumes that changing the control will a produce significant change in the
469 accel i.e. checkLimits() has already been called.
471 if a solution is found above the current control, the function returns true
472 and xlo is set to the current control, xhi to the interval max it found, and
473 solutionDomain is set to 1.
474 if the solution lies below the current control, then the function returns
475 true and xlo is set to the interval min it found and xmax to the current
476 control. if no solution is found, then the function returns false.
479 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
480 no assumptions about the state of the sim after this function has run
483 bool FGTrim::findInterval(void) {
486 double current_control=TrimAxes[current_axis]->GetControl();
487 double current_accel=TrimAxes[current_axis]->GetState();;
488 double xmin=TrimAxes[current_axis]->GetControlMin();
489 double xmax=TrimAxes[current_axis]->GetControlMax();
490 double lastxlo,lastxhi,lastalo,lastahi;
492 step=0.025*fabs(xmax);
493 xlo=xhi=current_control;
494 alo=ahi=current_accel;
495 lastxlo=xlo;lastxhi=xhi;
496 lastalo=alo;lastahi=ahi;
502 if(xlo < xmin) xlo=xmin;
504 if(xhi > xmax) xhi=xmax;
505 TrimAxes[current_axis]->SetControl(xlo);
506 TrimAxes[current_axis]->Run();
507 alo=TrimAxes[current_axis]->GetState();
508 TrimAxes[current_axis]->SetControl(xhi);
509 TrimAxes[current_axis]->Run();
510 ahi=TrimAxes[current_axis]->GetState();
511 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
512 if(alo*ahi <=0) { //found interval with root
514 if(alo*current_accel <= 0) { //narrow interval down a bit
518 //xhi=current_control;
524 //xlo=current_control;
528 lastxlo=xlo;lastxhi=xhi;
529 lastalo=alo;lastahi=ahi;
530 if( !found && xlo==xmin && xhi==xmax ) continue;
532 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
533 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
534 } while(!found && (Nsub <= max_sub_iterations) );
538 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
539 //checks to see which side of the current control value the solution is on
540 //and sets solutionDomain accordingly:
541 // 1 if solution is between the current and max
542 // -1 if solution is between the min and current
543 // 0 if there is no solution
545 //if changing the control produces no significant change in the accel then
546 //solutionDomain is set to zero and the function returns false
547 //if a solution is found, then xlo and xhi are set so that they bracket
548 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
549 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
550 //xhi=xmax and ahi=accel(xmax)
551 //in all cases the sim is left such that the control=xmax and accel=ahi
553 bool FGTrim::checkLimits(void) {
555 double current_control=TrimAxes[current_axis]->GetControl();
556 double current_accel=TrimAxes[current_axis]->GetState();
557 xlo=TrimAxes[current_axis]->GetControlMin();
558 xhi=TrimAxes[current_axis]->GetControlMax();
560 TrimAxes[current_axis]->SetControl(xlo);
561 TrimAxes[current_axis]->Run();
562 alo=TrimAxes[current_axis]->GetState();
563 TrimAxes[current_axis]->SetControl(xhi);
564 TrimAxes[current_axis]->Run();
565 ahi=TrimAxes[current_axis]->GetState();
567 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
568 << alo << ", " << ahi << endl;
570 solutionExists=false;
571 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
572 if(alo*current_accel <= 0) {
577 } else if(current_accel*ahi < 0){
584 TrimAxes[current_axis]->SetControl(current_control);
585 TrimAxes[current_axis]->Run();
586 return solutionExists;
589 void FGTrim::setupPullup() {
591 FGColumnVector3 vPQR;
592 g=fdmex->GetInertial()->gravity();
593 cgamma=cos(fgic->GetFlightPathAngleRadIC());
594 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
595 << fgic->GetVtrueFpsIC() << endl;
596 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
597 cout << targetNlf << ", " << q << endl;
600 cout << vPQR << endl;
601 fdmex->GetRotation()->SetPQR(vPQR);
602 cout << "setPitchRateInPullup() complete" << endl;
606 void FGTrim::setupTurn(void){
607 FGColumnVector3 vPQR;
608 float g,q,r,phi, psidot;
609 phi = fgic->GetRollAngleRadIC();
610 if( fabs(phi) > 0.01 && fabs(phi) < 1.56 ) {
611 targetNlf = 1 / cos(phi);
612 g = fdmex->GetInertial()->gravity();
613 psidot = g*tan(phi) / fgic->GetVtrueFpsIC();
616 vPQR(1)=0;vPQR(2)=q;vPQR(3)=r;
617 fdmex->GetRotation()->SetPQR(vPQR);
618 cout << targetNlf << ", " << vPQR*57.29577 << endl;
623 void FGTrim::setDebug(void) {
624 if(debug_axis == tAll ||
625 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
634 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.