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"
56 #pragma warning (disable : 4786 4788)
59 static const char *IdSrc = "$Id$";
60 static const char *IdHdr = ID_TRIM;
62 extern short debug_lvl;
64 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
66 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
70 max_sub_iterations=100;
72 A_Tolerance = Tolerance / 10;
85 cout << " Full Trim" << endl;
86 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
87 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
88 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
89 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
90 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
91 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
92 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
95 cout << " Longitudinal Trim" << endl;
96 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
97 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
98 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
101 cout << " Ground Trim" << endl;
102 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
103 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
104 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
107 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
108 sub_iterations=new float[TrimAxes.size()];
109 successful=new float[TrimAxes.size()];
110 solution=new bool[TrimAxes.size()];
113 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
116 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
118 FGTrim::~FGTrim(void) {
119 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
120 delete TrimAxes[current_axis];
122 delete[] sub_iterations;
125 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
128 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
130 void FGTrim::TrimStats() {
133 cout << endl << " Trim Statistics: " << endl;
134 cout << " Total Iterations: " << total_its << endl;
136 cout << " Sub-iterations:" << endl;
137 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
138 run_sum+=TrimAxes[current_axis]->GetRunCount();
139 sprintf(out," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
140 TrimAxes[current_axis]->GetStateName().c_str(),
141 sub_iterations[current_axis],
142 sub_iterations[current_axis]/float(total_its),
143 successful[current_axis],
144 TrimAxes[current_axis]->GetAvgStability() );
147 cout << " Run Count: " << run_sum << endl;
151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153 void FGTrim::Report(void) {
154 cout << " Trim Results: " << endl;
155 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
156 TrimAxes[current_axis]->AxisReport();
160 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
162 void FGTrim::ReportState(void) {
163 char out[80], flap[10], gear[10];
165 cout << endl << " JSBSim State" << endl;
166 sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n",
167 fdmex->GetAircraft()->GetWeight(),
168 fdmex->GetAircraft()->GetXYZcg(1),
169 fdmex->GetAircraft()->GetXYZcg(2),
170 fdmex->GetAircraft()->GetXYZcg(3));
172 if( fdmex->GetFCS()->GetDfPos() <= 0.01)
175 sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
176 if(fdmex->GetAircraft()->GetGearUp() == true)
179 sprintf(gear,"Down");
180 sprintf(out, " Flaps: %3s Gear: %4s\n",flap,gear);
182 sprintf(out, " Speed: %4.0f KCAS Mach: %5.2f\n",
183 fdmex->GetAuxiliary()->GetVcalibratedKTS(),
184 fdmex->GetState()->GetParameter(FG_MACH),
185 fdmex->GetPosition()->Geth() );
187 sprintf(out, " Altitude: %7.0f ft. AGL Altitude: %7.0f ft.\n",
188 fdmex->GetPosition()->Geth(),
189 fdmex->GetPosition()->GetDistanceAGL() );
191 sprintf(out, " Angle of Attack: %6.2f deg Pitch Angle: %6.2f deg\n",
192 fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
193 fdmex->GetRotation()->Gettht()*RADTODEG );
195 sprintf(out, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n",
196 fdmex->GetPosition()->GetGamma()*RADTODEG,
197 fdmex->GetPosition()->Gethdot()*60 );
199 sprintf(out, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n",
200 fdmex->GetAircraft()->GetNlf(),
201 fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG );
203 sprintf(out, " Heading: %3.0f deg true Sideslip: %5.2f deg\n",
204 fdmex->GetRotation()->Getpsi()*RADTODEG,
205 fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );
207 sprintf(out, " Bank Angle: %5.2f deg\n",
208 fdmex->GetRotation()->Getphi()*RADTODEG );
210 sprintf(out, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n",
211 fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
212 fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
213 fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
215 sprintf(out, " Throttle: %5.2f%c\n",
216 fdmex->GetFCS()->GetThrottlePos(0),'%' );
219 sprintf(out, " Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n",
220 fdmex->GetAuxiliary()->GetHeadWind()*jsbFPSTOKTS,
221 fdmex->GetAuxiliary()->GetCrossWind()*jsbFPSTOKTS );
224 sprintf(out, " Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n",
225 fdmex->GetPosition()->GetVground()*jsbFPSTOKTS,
226 fdmex->GetPosition()->GetGroundTrack()*RADTODEG );
231 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
233 void FGTrim::ClearStates(void) {
237 vector<FGTrimAxis*>::iterator iAxes;
238 iAxes = TrimAxes.begin();
239 while (iAxes != TrimAxes.end()) {
245 cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
248 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
250 bool FGTrim::AddState( State state, Control control ) {
255 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
256 while (iAxes != TrimAxes.end()) {
258 if( ta->GetStateType() == state )
263 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
264 delete[] sub_iterations;
267 sub_iterations=new float[TrimAxes.size()];
268 successful=new float[TrimAxes.size()];
269 solution=new bool[TrimAxes.size()];
274 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
276 bool FGTrim::RemoveState( State state ) {
281 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
282 while (iAxes != TrimAxes.end()) {
284 if( ta->GetStateType() == state ) {
286 TrimAxes.erase(iAxes);
293 delete[] sub_iterations;
296 sub_iterations=new float[TrimAxes.size()];
297 successful=new float[TrimAxes.size()];
298 solution=new bool[TrimAxes.size()];
303 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
305 bool FGTrim::EditState( State state, Control new_control ){
310 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
311 while (iAxes != TrimAxes.end()) {
313 if( ta->GetStateType() == state ) {
314 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
316 TrimAxes.erase(iAxes+1);
322 cout << "Exit FGTrim::EditState(...)" << endl;
327 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
329 bool FGTrim::DoTrim(void) {
334 for(i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
335 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
338 fdmex->GetOutput()->Disable();
340 //clear the sub iterations counts & zero out the controls
341 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
342 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
343 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
344 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
346 TrimAxes[current_axis]->initTheta();
348 xlo=TrimAxes[current_axis]->GetControlMin();
349 xhi=TrimAxes[current_axis]->GetControlMax();
350 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
351 TrimAxes[current_axis]->Run();
352 //TrimAxes[current_axis]->AxisReport();
353 sub_iterations[current_axis]=0;
354 successful[current_axis]=0;
355 solution[current_axis]=false;
359 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
361 if(!solution[current_axis]) {
363 solution[current_axis]=true;
366 } else if(findInterval()) {
369 solution[current_axis]=false;
371 sub_iterations[current_axis]+=Nsub;
373 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
374 //these checks need to be done after all the axes have run
375 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
376 if(TrimAxes[current_axis]->InTolerance()) {
378 successful[current_axis]++;
383 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
384 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
385 //At this point we can check the input limits of the failed axis
386 //and declare the trim failed if there is no sign change. If there
387 //is, keep going until success or max iteration count
389 //Oh, well: two out of three ain't bad
390 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
391 //these checks need to be done after all the axes have run
392 if(!TrimAxes[current_axis]->InTolerance()) {
394 // special case this for now -- if other cases arise proper
395 // support can be added to FGTrimAxis
396 if( (gamma_fallback) &&
397 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
398 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
399 cout << " Can't trim udot with throttle, trying flight"
400 << " path angle. (" << N << ")" << endl;
401 if(TrimAxes[current_axis]->GetState() > 0)
402 TrimAxes[current_axis]->SetControlToMin();
404 TrimAxes[current_axis]->SetControlToMax();
405 TrimAxes[current_axis]->Run();
406 delete TrimAxes[current_axis];
407 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
410 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
411 << " doesn't appear to be trimmable" << endl;
413 trim_failed=true; //force the trim to fail
418 } //all-but-one check
420 if(N > max_iterations)
422 } while((axis_count < TrimAxes.size()) && (!trim_failed));
423 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
425 cout << endl << " Trim successful" << endl;
428 cout << endl << " Trim failed" << endl;
430 for(i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
431 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
433 fdmex->GetOutput()->Enable();
437 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
439 bool FGTrim::solve(void) {
441 float x1,x2,x3,f1,f2,f3,d,d0;
442 const float relax =0.9;
443 float eps=TrimAxes[current_axis]->GetSolverEps();
449 if( solutionDomain != 0) {
450 /* if(ahi > alo) { */
460 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
461 while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
462 && (Nsub < max_sub_iterations)) {
465 x2=x1-d*d0*f1/(f3-f1);
466 TrimAxes[current_axis]->SetControl(x2);
467 TrimAxes[current_axis]->Run();
468 f2=TrimAxes[current_axis]->GetState();
470 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
471 << ", " << x2 << ", " << x3 << endl;
472 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
478 //cout << "Solution is between x1 and x2" << endl;
480 else if(f2*f3 <= 0.0) {
484 //cout << "Solution is between x2 and x3" << endl;
491 if(Nsub < max_sub_iterations) success=true;
496 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
498 produces an interval (xlo..xhi) on one side or the other of the current
499 control value in which a solution exists. This domain is, hopefully,
500 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
501 to find the solution. This is, hopefully, more efficient than having the
502 solver start from scratch every time. Maybe it isn't though...
503 This tries to take advantage of the idea that the changes from iteration to
504 iteration will be small after the first one or two top-level iterations.
506 assumes that changing the control will a produce significant change in the
507 accel i.e. checkLimits() has already been called.
509 if a solution is found above the current control, the function returns true
510 and xlo is set to the current control, xhi to the interval max it found, and
511 solutionDomain is set to 1.
512 if the solution lies below the current control, then the function returns
513 true and xlo is set to the interval min it found and xmax to the current
514 control. if no solution is found, then the function returns false.
517 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
518 no assumptions about the state of the sim after this function has run
521 bool FGTrim::findInterval(void) {
524 float current_control=TrimAxes[current_axis]->GetControl();
525 float current_accel=TrimAxes[current_axis]->GetState();;
526 float xmin=TrimAxes[current_axis]->GetControlMin();
527 float xmax=TrimAxes[current_axis]->GetControlMax();
528 float lastxlo,lastxhi,lastalo,lastahi;
530 step=0.025*fabs(xmax);
531 xlo=xhi=current_control;
532 alo=ahi=current_accel;
533 lastxlo=xlo;lastxhi=xhi;
534 lastalo=alo;lastahi=ahi;
540 if(xlo < xmin) xlo=xmin;
542 if(xhi > xmax) xhi=xmax;
543 TrimAxes[current_axis]->SetControl(xlo);
544 TrimAxes[current_axis]->Run();
545 alo=TrimAxes[current_axis]->GetState();
546 TrimAxes[current_axis]->SetControl(xhi);
547 TrimAxes[current_axis]->Run();
548 ahi=TrimAxes[current_axis]->GetState();
549 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
550 if(alo*ahi <=0) { //found interval with root
552 if(alo*current_accel <= 0) { //narrow interval down a bit
556 //xhi=current_control;
562 //xlo=current_control;
566 lastxlo=xlo;lastxhi=xhi;
567 lastalo=alo;lastahi=ahi;
568 if( !found && xlo==xmin && xhi==xmax ) continue;
570 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
571 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
572 } while(!found && (Nsub <= max_sub_iterations) );
576 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
577 //checks to see which side of the current control value the solution is on
578 //and sets solutionDomain accordingly:
579 // 1 if solution is between the current and max
580 // -1 if solution is between the min and current
581 // 0 if there is no solution
583 //if changing the control produces no significant change in the accel then
584 //solutionDomain is set to zero and the function returns false
585 //if a solution is found, then xlo and xhi are set so that they bracket
586 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
587 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
588 //xhi=xmax and ahi=accel(xmax)
589 //in all cases the sim is left such that the control=xmax and accel=ahi
591 bool FGTrim::checkLimits(void) {
593 float current_control=TrimAxes[current_axis]->GetControl();
594 float current_accel=TrimAxes[current_axis]->GetState();
595 xlo=TrimAxes[current_axis]->GetControlMin();
596 xhi=TrimAxes[current_axis]->GetControlMax();
598 TrimAxes[current_axis]->SetControl(xlo);
599 TrimAxes[current_axis]->Run();
600 alo=TrimAxes[current_axis]->GetState();
601 TrimAxes[current_axis]->SetControl(xhi);
602 TrimAxes[current_axis]->Run();
603 ahi=TrimAxes[current_axis]->GetState();
605 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
606 << alo << ", " << ahi << endl;
608 solutionExists=false;
609 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
610 if(alo*current_accel <= 0) {
615 } else if(current_accel*ahi < 0){
622 TrimAxes[current_axis]->SetControl(current_control);
623 TrimAxes[current_axis]->Run();
624 return solutionExists;
627 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.