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 "FGGroundReactions.h"
56 #include "FGInertial.h"
57 #include "FGAerodynamics.h"
58 #include "FGColumnVector3.h"
61 #pragma warning (disable : 4786 4788)
66 static const char *IdSrc = "$Id$";
67 static const char *IdHdr = ID_TRIM;
69 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
71 FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
75 max_sub_iterations=100;
77 A_Tolerance = Tolerance / 10;
91 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
94 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
96 FGTrim::~FGTrim(void) {
97 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
98 delete TrimAxes[current_axis];
100 delete[] sub_iterations;
103 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
106 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
108 void FGTrim::TrimStats() {
111 cout << endl << " Trim Statistics: " << endl;
112 cout << " Total Iterations: " << total_its << endl;
114 cout << " Sub-iterations:" << endl;
115 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
116 run_sum+=TrimAxes[current_axis]->GetRunCount();
117 snprintf(out,80," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
118 TrimAxes[current_axis]->GetStateName().c_str(),
119 sub_iterations[current_axis],
120 sub_iterations[current_axis]/double(total_its),
121 successful[current_axis],
122 TrimAxes[current_axis]->GetAvgStability() );
125 cout << " Run Count: " << run_sum << endl;
129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131 void FGTrim::Report(void) {
132 cout << " Trim Results: " << endl;
133 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
134 TrimAxes[current_axis]->AxisReport();
138 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
140 void FGTrim::ClearStates(void) {
144 vector<FGTrimAxis*>::iterator iAxes;
145 iAxes = TrimAxes.begin();
146 while (iAxes != TrimAxes.end()) {
152 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
155 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
157 bool FGTrim::AddState( State state, Control control ) {
162 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
163 while (iAxes != TrimAxes.end()) {
165 if( ta->GetStateType() == state )
170 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
171 delete[] sub_iterations;
174 sub_iterations=new double[TrimAxes.size()];
175 successful=new double[TrimAxes.size()];
176 solution=new bool[TrimAxes.size()];
181 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
183 bool FGTrim::RemoveState( State state ) {
188 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
189 while (iAxes != TrimAxes.end()) {
191 if( ta->GetStateType() == state ) {
193 TrimAxes.erase(iAxes);
200 delete[] sub_iterations;
203 sub_iterations=new double[TrimAxes.size()];
204 successful=new double[TrimAxes.size()];
205 solution=new bool[TrimAxes.size()];
210 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
212 bool FGTrim::EditState( State state, Control new_control ){
217 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
218 while (iAxes != TrimAxes.end()) {
220 if( ta->GetStateType() == state ) {
221 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
223 TrimAxes.erase(iAxes+1);
233 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
235 bool FGTrim::DoTrim(void) {
240 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
241 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
244 fdmex->GetOutput()->Disable();
246 fgic->SetPRadpsIC(0.0);
247 fgic->SetQRadpsIC(0.0);
248 fgic->SetRRadpsIC(0.0);
250 //clear the sub iterations counts & zero out the controls
251 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
252 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
253 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
254 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
255 if(mode == tGround) {
256 TrimAxes[current_axis]->initTheta();
259 xlo=TrimAxes[current_axis]->GetControlMin();
260 xhi=TrimAxes[current_axis]->GetControlMax();
261 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
262 TrimAxes[current_axis]->Run();
263 //TrimAxes[current_axis]->AxisReport();
264 sub_iterations[current_axis]=0;
265 successful[current_axis]=0;
266 solution[current_axis]=false;
270 if(mode == tPullup ) {
271 cout << "Setting pitch rate and nlf... " << endl;
273 cout << "pitch rate done ... " << endl;
274 TrimAxes[0]->SetStateTarget(targetNlf);
275 cout << "nlf done" << endl;
276 } else if (mode == tTurn) {
278 //TrimAxes[0]->SetStateTarget(targetNlf);
283 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
287 if(!solution[current_axis]) {
289 solution[current_axis]=true;
292 } else if(findInterval()) {
295 solution[current_axis]=false;
297 sub_iterations[current_axis]+=Nsub;
299 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
300 //these checks need to be done after all the axes have run
301 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
302 if(TrimAxes[current_axis]->InTolerance()) {
304 successful[current_axis]++;
309 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
310 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
311 //At this point we can check the input limits of the failed axis
312 //and declare the trim failed if there is no sign change. If there
313 //is, keep going until success or max iteration count
315 //Oh, well: two out of three ain't bad
316 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
317 //these checks need to be done after all the axes have run
318 if(!TrimAxes[current_axis]->InTolerance()) {
320 // special case this for now -- if other cases arise proper
321 // support can be added to FGTrimAxis
322 if( (gamma_fallback) &&
323 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
324 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
325 cout << " Can't trim udot with throttle, trying flight"
326 << " path angle. (" << N << ")" << endl;
327 if(TrimAxes[current_axis]->GetState() > 0)
328 TrimAxes[current_axis]->SetControlToMin();
330 TrimAxes[current_axis]->SetControlToMax();
331 TrimAxes[current_axis]->Run();
332 delete TrimAxes[current_axis];
333 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
336 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
337 << " doesn't appear to be trimmable" << endl;
339 trim_failed=true; //force the trim to fail
344 } //all-but-one check
346 if(N > max_iterations)
348 } while((axis_count < TrimAxes.size()) && (!trim_failed));
349 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
352 cout << endl << " Trim successful" << endl;
356 cout << endl << " Trim failed" << endl;
358 for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
359 fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
361 fdmex->GetOutput()->Enable();
365 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 bool FGTrim::solve(void) {
369 double x1,x2,x3,f1,f2,f3,d,d0;
370 const double relax =0.9;
371 double eps=TrimAxes[current_axis]->GetSolverEps();
377 if( solutionDomain != 0) {
378 /* if(ahi > alo) { */
387 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
388 while ( (TrimAxes[current_axis]->InTolerance() == false )
389 && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
392 x2=x1-d*d0*f1/(f3-f1);
393 TrimAxes[current_axis]->SetControl(x2);
394 TrimAxes[current_axis]->Run();
395 f2=TrimAxes[current_axis]->GetState();
397 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
398 << ", " << x2 << ", " << x3 << endl;
399 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
405 //cout << "Solution is between x1 and x2" << endl;
407 else if(f2*f3 <= 0.0) {
411 //cout << "Solution is between x2 and x3" << endl;
418 if(Nsub < max_sub_iterations) success=true;
423 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
425 produces an interval (xlo..xhi) on one side or the other of the current
426 control value in which a solution exists. This domain is, hopefully,
427 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
428 to find the solution. This is, hopefully, more efficient than having the
429 solver start from scratch every time. Maybe it isn't though...
430 This tries to take advantage of the idea that the changes from iteration to
431 iteration will be small after the first one or two top-level iterations.
433 assumes that changing the control will a produce significant change in the
434 accel i.e. checkLimits() has already been called.
436 if a solution is found above the current control, the function returns true
437 and xlo is set to the current control, xhi to the interval max it found, and
438 solutionDomain is set to 1.
439 if the solution lies below the current control, then the function returns
440 true and xlo is set to the interval min it found and xmax to the current
441 control. if no solution is found, then the function returns false.
444 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
445 no assumptions about the state of the sim after this function has run
448 bool FGTrim::findInterval(void) {
451 double current_control=TrimAxes[current_axis]->GetControl();
452 double current_accel=TrimAxes[current_axis]->GetState();;
453 double xmin=TrimAxes[current_axis]->GetControlMin();
454 double xmax=TrimAxes[current_axis]->GetControlMax();
455 double lastxlo,lastxhi,lastalo,lastahi;
457 step=0.025*fabs(xmax);
458 xlo=xhi=current_control;
459 alo=ahi=current_accel;
460 lastxlo=xlo;lastxhi=xhi;
461 lastalo=alo;lastahi=ahi;
467 if(xlo < xmin) xlo=xmin;
469 if(xhi > xmax) xhi=xmax;
470 TrimAxes[current_axis]->SetControl(xlo);
471 TrimAxes[current_axis]->Run();
472 alo=TrimAxes[current_axis]->GetState();
473 TrimAxes[current_axis]->SetControl(xhi);
474 TrimAxes[current_axis]->Run();
475 ahi=TrimAxes[current_axis]->GetState();
476 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
477 if(alo*ahi <=0) { //found interval with root
479 if(alo*current_accel <= 0) { //narrow interval down a bit
483 //xhi=current_control;
489 //xlo=current_control;
493 lastxlo=xlo;lastxhi=xhi;
494 lastalo=alo;lastahi=ahi;
495 if( !found && xlo==xmin && xhi==xmax ) continue;
497 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
498 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
499 } while(!found && (Nsub <= max_sub_iterations) );
503 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
504 //checks to see which side of the current control value the solution is on
505 //and sets solutionDomain accordingly:
506 // 1 if solution is between the current and max
507 // -1 if solution is between the min and current
508 // 0 if there is no solution
510 //if changing the control produces no significant change in the accel then
511 //solutionDomain is set to zero and the function returns false
512 //if a solution is found, then xlo and xhi are set so that they bracket
513 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
514 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
515 //xhi=xmax and ahi=accel(xmax)
516 //in all cases the sim is left such that the control=xmax and accel=ahi
518 bool FGTrim::checkLimits(void) {
520 double current_control=TrimAxes[current_axis]->GetControl();
521 double current_accel=TrimAxes[current_axis]->GetState();
522 xlo=TrimAxes[current_axis]->GetControlMin();
523 xhi=TrimAxes[current_axis]->GetControlMax();
525 TrimAxes[current_axis]->SetControl(xlo);
526 TrimAxes[current_axis]->Run();
527 alo=TrimAxes[current_axis]->GetState();
528 TrimAxes[current_axis]->SetControl(xhi);
529 TrimAxes[current_axis]->Run();
530 ahi=TrimAxes[current_axis]->GetState();
532 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
533 << alo << ", " << ahi << endl;
535 solutionExists=false;
536 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
537 if(alo*current_accel <= 0) {
542 } else if(current_accel*ahi < 0){
549 TrimAxes[current_axis]->SetControl(current_control);
550 TrimAxes[current_axis]->Run();
551 return solutionExists;
554 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
556 void FGTrim::setupPullup() {
558 g=fdmex->GetInertial()->gravity();
559 cgamma=cos(fgic->GetFlightPathAngleRadIC());
560 cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
561 << fgic->GetVtrueFpsIC() << endl;
562 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
563 cout << targetNlf << ", " << q << endl;
564 fgic->SetQRadpsIC(q);
565 cout << "setPitchRateInPullup() complete" << endl;
569 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
571 void FGTrim::setupTurn(void){
573 phi = fgic->GetRollAngleRadIC();
574 if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
575 targetNlf = 1 / cos(phi);
576 g = fdmex->GetInertial()->gravity();
577 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
578 cout << targetNlf << ", " << psidot << endl;
583 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
585 void FGTrim::updateRates(void){
586 if( mode == tTurn ) {
587 double phi = fgic->GetRollAngleRadIC();
588 double g = fdmex->GetInertial()->gravity();
590 if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
591 theta=fgic->GetPitchAngleRadIC();
592 phi=fgic->GetRollAngleRadIC();
593 psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
594 p=-psidot*sin(theta);
595 q=psidot*cos(theta)*sin(phi);
596 r=psidot*cos(theta)*cos(phi);
600 fgic->SetPRadpsIC(p);
601 fgic->SetQRadpsIC(q);
602 fgic->SetRRadpsIC(r);
603 } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
605 g=fdmex->GetInertial()->gravity();
606 cgamma=cos(fgic->GetFlightPathAngleRadIC());
607 q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
608 fgic->SetQRadpsIC(q);
612 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
614 void FGTrim::setDebug(void) {
615 if(debug_axis == tAll ||
616 TrimAxes[current_axis]->GetStateType() == debug_axis ) {
625 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
627 void FGTrim::SetMode(TrimMode tt) {
633 cout << " Full 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 ));
637 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
638 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
639 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
640 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
644 cout << " Longitudinal Trim" << endl;
645 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
646 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
647 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
651 cout << " Ground Trim" << endl;
652 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
653 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
654 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
657 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
658 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
659 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
660 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
661 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
662 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
663 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
666 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
667 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
668 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
669 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
670 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
671 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
677 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
678 sub_iterations=new double[TrimAxes.size()];
679 successful=new double[TrimAxes.size()];
680 solution=new bool[TrimAxes.size()];
683 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.