]> git.mxchange.org Git - flightgear.git/blob - src/FDM/JSBSim/FGTrim.cpp
FG_ to SG_ namespace changes.
[flightgear.git] / src / FDM / JSBSim / FGTrim.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2  
3  Header:       FGTrim.cpp
4  Author:       Tony Peden
5  Date started: 9/8/99
6  
7  --------- Copyright (C) 1999  Anthony K. Peden (apeden@earthlink.net) ---------
8  
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
12  version.
13  
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
17  details.
18  
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.
22  
23  Further information about the GNU General Public License can also be found on
24  the world wide web at http://www.gnu.org.
25  
26  
27  HISTORY
28 --------------------------------------------------------------------------------
29 9/8/99   TP   Created
30  
31  
32 FUNCTIONAL DESCRIPTION
33 --------------------------------------------------------------------------------
34  
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
38 scheme. */
39
40 //  !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
41
42
43 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
44 INCLUDES
45 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
46
47 #include <stdlib.h>
48
49 #include "FGFDMExec.h"
50 #include "FGAtmosphere.h"
51 #include "FGInitialCondition.h"
52 #include "FGTrim.h"
53 #include "FGAircraft.h"
54
55 static const char *IdSrc = "$Header$";
56 static const char *IdHdr = ID_TRIM;
57
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
59
60 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
61
62   N=Nsub=0;
63   max_iterations=60;
64   max_sub_iterations=100;
65   Tolerance=1E-3;
66   A_Tolerance = Tolerance / 10;
67   
68   Debug=0;
69   fdmex=FDMExec;
70   fgic=FGIC;
71   total_its=0;
72   trimudot=true;
73   gamma_fallback=true;
74   axis_count=0;
75   mode=tt;
76   xlo=xhi=alo=ahi;
77   switch(mode) {
78   case tFull:
79     cout << "  Full 6-DOF Trim" << endl;
80     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
81     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
82     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
83     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi,Tolerance));
84     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron,A_Tolerance));
85     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder,A_Tolerance));
86     break;
87   case tLongitudinal:
88     cout << "  Longitudinal Trim" << endl;
89     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha,Tolerance));
90     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle,Tolerance));
91     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim,A_Tolerance));
92     break;
93   case tGround:
94     cout << "  Ground Trim" << endl;
95     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL,Tolerance));
96     TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta,A_Tolerance));
97     //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi,A_Tolerance));
98     break;
99   }
100   //cout << "NumAxes: " << TrimAxes.size() << endl;
101   NumAxes=TrimAxes.size();
102   sub_iterations=new float[NumAxes];
103   successful=new float[NumAxes];
104   solution=new bool[NumAxes];
105   current_axis=0;
106 }
107
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
109
110 FGTrim::~FGTrim(void) {
111   for(current_axis=0; current_axis<NumAxes; current_axis++) {
112     delete TrimAxes[current_axis];
113   }
114   delete[] sub_iterations;
115   delete[] successful;
116   delete[] solution;
117 }
118
119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
120
121 void FGTrim::TrimStats() {
122   char out[80];
123   int run_sum=0;
124   cout << endl << "  Trim Statistics: " << endl;
125   cout << "    Total Iterations: " << total_its << endl;
126   if(total_its > 0) {
127     cout << "    Sub-iterations:" << endl;
128     for(current_axis=0; current_axis<NumAxes; current_axis++) {
129       run_sum+=TrimAxes[current_axis]->GetRunCount();
130       sprintf(out,"   %5s: %3.0f average: %5.2f  successful: %3.0f  stability: %5.2f\n",
131                   TrimAxes[current_axis]->GetAccelName().c_str(),
132                   sub_iterations[current_axis],
133                   sub_iterations[current_axis]/float(total_its),
134                   successful[current_axis],
135                   TrimAxes[current_axis]->GetAvgStability() );
136       cout << out;
137     }
138     cout << "    Run Count: " << run_sum << endl;
139   }
140 }
141
142 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
143
144 void FGTrim::Report(void) {
145   cout << "  Trim Results: " << endl;
146   for(current_axis=0; current_axis<NumAxes; current_axis++)
147     TrimAxes[current_axis]->AxisReport();
148
149 }
150
151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
152
153 void FGTrim::ReportState(void) {
154   char out[80], flap[10], gear[10];
155   
156   cout << endl << "  JSBSim State" << endl;
157   sprintf(out,"    Weight: %7.0f lbs.  CG: %5.1f, %5.1f, %5.1f inches\n",
158                    fdmex->GetAircraft()->GetWeight(),
159                    fdmex->GetAircraft()->GetXYZcg()(1),
160                    fdmex->GetAircraft()->GetXYZcg()(2),
161                    fdmex->GetAircraft()->GetXYZcg()(3) );
162   cout << out;             
163   if( fdmex->GetFCS()->GetDfPos() <= 0.01)
164     sprintf(flap,"Up");
165   else
166     sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
167   if(fdmex->GetAircraft()->GetGearUp() == true)
168     sprintf(gear,"Up");
169   else
170     sprintf(gear,"Down");
171   sprintf(out, "    Flaps: %3s  Gear: %4s\n",flap,gear);
172   cout << out;
173   sprintf(out, "    Speed: %4.0f KCAS  Mach: %5.2f\n",
174                     fdmex->GetAuxiliary()->GetVcalibratedKTS(),
175                     fdmex->GetState()->GetParameter(FG_MACH),
176                     fdmex->GetPosition()->Geth() );
177   cout << out;
178   sprintf(out, "    Altitude: %7.0f ft.  AGL Altitude: %7.0f ft.\n",
179                     fdmex->GetPosition()->Geth(),
180                     fdmex->GetPosition()->GetDistanceAGL() );
181   cout << out;
182   sprintf(out, "    Angle of Attack: %6.2f deg  Pitch Angle: %6.2f deg\n",
183                     fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
184                     fdmex->GetRotation()->Gettht()*RADTODEG );
185   cout << out;
186   sprintf(out, "    Flight Path Angle: %6.2f deg  Climb Rate: %5.0f ft/min\n",
187                     fdmex->GetPosition()->GetGamma()*RADTODEG,
188                     fdmex->GetPosition()->Gethdot()*60 );
189   cout << out;                  
190   sprintf(out, "    Normal Load Factor: %4.2f g's  Pitch Rate: %5.2f deg/s\n",
191                     fdmex->GetAircraft()->GetNlf(),
192                     fdmex->GetState()->GetParameter(SG_PITCHRATE)*RADTODEG );
193   cout << out;
194   sprintf(out, "    Heading: %3.0f deg true  Sideslip: %5.2f deg\n",
195                     fdmex->GetRotation()->Getpsi()*RADTODEG,
196                     fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );                  
197   cout << out;
198   sprintf(out, "    Bank Angle: %5.2f deg\n",
199                     fdmex->GetRotation()->Getphi()*RADTODEG );
200   cout << out;
201   sprintf(out, "    Elevator: %5.2f deg  Left Aileron: %5.2f deg  Rudder: %5.2f deg\n",
202                     fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
203                     fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
204                     fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
205   cout << out;                  
206   sprintf(out, "    Throttle: %5.2f%c\n",
207                     fdmex->GetFCS()->GetThrottlePos(0),'%' );
208   cout << out;                                  
209 }
210
211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
212
213 bool FGTrim::DoTrim(void) {
214   
215   trim_failed=false;
216
217
218   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
219     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
220   }
221
222   fdmex->GetOutput()->Disable();
223
224   //clear the sub iterations counts & zero out the controls
225   for(current_axis=0;current_axis<NumAxes;current_axis++) {
226     //cout << current_axis << "  " << TrimAxes[current_axis]->GetAccelName()
227     //<< "  " << TrimAxes[current_axis]->GetControlName()<< endl;
228     if(TrimAxes[current_axis]->GetAccelType() == tQdot) {
229       if(mode == tGround) 
230           TrimAxes[current_axis]->initTheta();
231     }  
232     xlo=TrimAxes[current_axis]->GetControlMin();
233     xhi=TrimAxes[current_axis]->GetControlMax();
234     TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
235     TrimAxes[current_axis]->Run();
236     //TrimAxes[current_axis]->AxisReport();
237     sub_iterations[current_axis]=0;
238     successful[current_axis]=0;
239     solution[current_axis]=false;
240   }
241   do {
242     axis_count=0;
243     for(current_axis=0;current_axis<NumAxes;current_axis++) {
244       Nsub=0;
245       if(!solution[current_axis]) {
246         if(checkLimits()) { 
247           solution[current_axis]=true;
248           solve();
249         }  
250       } else if(findInterval()) {
251         solve();
252       } else {
253         solution[current_axis]=false;
254       }  
255       sub_iterations[current_axis]+=Nsub;
256     } 
257     for(current_axis=0;current_axis<NumAxes;current_axis++) {
258       //these checks need to be done after all the axes have run
259       if(Debug > 0) TrimAxes[current_axis]->AxisReport();
260       if(TrimAxes[current_axis]->InTolerance()) {
261         axis_count++;
262         successful[current_axis]++;
263       } 
264     }
265     
266
267     if((axis_count == NumAxes-1) && (NumAxes > 1)) {
268       //cout << NumAxes-1 << " out of " << NumAxes << "!" << endl;
269       //At this point we can check the input limits of the failed axis
270       //and declare the trim failed if there is no sign change. If there
271       //is, keep going until success or max iteration count
272
273       //Oh, well: two out of three ain't bad
274       for(current_axis=0;current_axis<NumAxes;current_axis++) {
275         //these checks need to be done after all the axes have run
276         if(!TrimAxes[current_axis]->InTolerance()) {
277           if(!checkLimits()) {
278             // special case this for now -- if other cases arise proper
279             // support can be added to FGTrimAxis
280             if( (gamma_fallback) &&
281                 (TrimAxes[current_axis]->GetAccelType() == tUdot) &&
282                 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
283               cout << "  Can't trim udot with throttle, trying flight"
284               << " path angle. (" << N << ")" << endl;
285               if(TrimAxes[current_axis]->GetAccel() > 0)
286                 TrimAxes[current_axis]->SetControlToMin();
287               else
288                 TrimAxes[current_axis]->SetControlToMax();
289               TrimAxes[current_axis]->Run();
290               delete TrimAxes[current_axis];
291               TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
292                                                     tGamma,Tolerance);
293             } else {
294               cout << "  Sorry, " << TrimAxes[current_axis]->GetAccelName()
295               << " doesn't appear to be trimmable" << endl;
296               //total_its=k;
297               trim_failed=true; //force the trim to fail
298             } //gamma_fallback
299           }
300         } //solution check
301       } //for loop
302     } //all-but-one check
303     N++;
304     if(N > max_iterations)
305       trim_failed=true;
306   } while((axis_count < NumAxes) && (!trim_failed));
307   if((!trim_failed) && (axis_count >= NumAxes)) {
308     total_its=N;
309     cout << endl << "  Trim successful" << endl;
310   } else {
311     total_its=N;
312     cout << endl << "  Trim failed" << endl;
313   }
314   for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
315     fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
316   }
317   fdmex->GetOutput()->Enable();
318   return !trim_failed;
319 }
320
321 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
322
323 bool FGTrim::solve(void) {
324
325   float x1,x2,x3,f1,f2,f3,d,d0;
326   const float relax =0.9;
327   float eps=TrimAxes[current_axis]->GetSolverEps();
328
329   x1=x2=x3=0;
330   d=1;
331   bool success=false;
332   //initializations
333   if( solutionDomain != 0) {
334    /* if(ahi > alo) { */
335       x1=xlo;f1=alo;
336       x3=xhi;f3=ahi;
337    /* } else {
338       x1=xhi;f1=ahi;
339       x3=xlo;f3=alo;
340     }   */
341
342     d0=fabs(x3-x1);
343     //iterations
344     //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
345     while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps) 
346               && (Nsub < max_sub_iterations)) {
347       Nsub++;
348       d=(x3-x1)/d0;
349       x2=x1-d*d0*f1/(f3-f1);
350       TrimAxes[current_axis]->SetControl(x2);
351       TrimAxes[current_axis]->Run();
352       f2=TrimAxes[current_axis]->GetAccel();
353       if(Debug > 1) {
354         cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
355         << ", " << x2 << ", " << x3 << endl;
356         cout << "                             " << f1 << ", " << f2 << ", " << f3 << endl;
357       }
358       if(f1*f2 <= 0.0) {
359         x3=x2;
360         f3=f2;
361         f1=relax*f1;
362         //cout << "Solution is between x1 and x2" << endl;
363       }
364       else if(f2*f3 <= 0.0) {
365         x1=x2;
366         f1=f2;
367         f3=relax*f3;
368         //cout << "Solution is between x2 and x3" << endl;
369
370       }
371       //cout << i << endl;
372
373       
374     }//end while
375     if(Nsub < max_sub_iterations) success=true;
376   }  
377   return success;
378 }
379
380 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
381 /*
382  produces an interval (xlo..xhi) on one side or the other of the current 
383  control value in which a solution exists.  This domain is, hopefully, 
384  smaller than xmin..0 or 0..xmax and the solver will require fewer iterations 
385  to find the solution. This is, hopefully, more efficient than having the 
386  solver start from scratch every time. Maybe it isn't though...
387  This tries to take advantage of the idea that the changes from iteration to
388  iteration will be small after the first one or two top-level iterations.
389
390  assumes that changing the control will a produce significant change in the
391  accel i.e. checkLimits() has already been called.
392
393  if a solution is found above the current control, the function returns true 
394  and xlo is set to the current control, xhi to the interval max it found, and 
395  solutionDomain is set to 1.
396  if the solution lies below the current control, then the function returns 
397  true and xlo is set to the interval min it found and xmax to the current 
398  control. if no solution is found, then the function returns false.
399  
400  
401  in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
402  no assumptions about the state of the sim after this function has run 
403  can be made.
404 */
405 bool FGTrim::findInterval(void) {
406   bool found=false;
407   float step;
408   float current_control=TrimAxes[current_axis]->GetControl();
409   float current_accel=TrimAxes[current_axis]->GetAccel();;
410   float xmin=TrimAxes[current_axis]->GetControlMin();
411   float xmax=TrimAxes[current_axis]->GetControlMax();
412   float lastxlo,lastxhi,lastalo,lastahi;
413   
414   step=0.025*fabs(xmax);
415   xlo=xhi=current_control;
416   alo=ahi=current_accel;
417   lastxlo=xlo;lastxhi=xhi;
418   lastalo=alo;lastahi=ahi;
419   do {
420     
421     Nsub++;
422     step*=2;
423     xlo-=step;
424     if(xlo < xmin) xlo=xmin;
425     xhi+=step;
426     if(xhi > xmax) xhi=xmax;
427     TrimAxes[current_axis]->SetControl(xlo);
428     TrimAxes[current_axis]->Run();
429     alo=TrimAxes[current_axis]->GetAccel();
430     TrimAxes[current_axis]->SetControl(xhi);
431     TrimAxes[current_axis]->Run();
432     ahi=TrimAxes[current_axis]->GetAccel();
433     if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
434     if(alo*ahi <=0) {  //found interval with root
435       found=true;
436       if(alo*current_accel <= 0) { //narrow interval down a bit
437         solutionDomain=-1;
438         xhi=lastxlo;
439         ahi=lastalo;
440         //xhi=current_control;
441         //ahi=current_accel;
442       } else {
443         solutionDomain=1;
444         xlo=lastxhi;
445         alo=lastahi;
446         //xlo=current_control;
447         //alo=current_accel;
448       }     
449     }
450     lastxlo=xlo;lastxhi=xhi;
451     lastalo=alo;lastahi=ahi;
452     if( !found && xlo==xmin && xhi==xmax ) continue;
453     if(Debug > 1)
454       cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
455                            << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
456   } while(!found && (Nsub <= max_sub_iterations) );
457   return found;
458 }
459
460 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
461 //checks to see which side of the current control value the solution is on
462 //and sets solutionDomain accordingly:
463 //  1 if solution is between the current and max
464 // -1 if solution is between the min and current
465 //  0 if there is no solution
466 // 
467 //if changing the control produces no significant change in the accel then
468 //solutionDomain is set to zero and the function returns false
469 //if a solution is found, then xlo and xhi are set so that they bracket
470 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
471 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
472 //xhi=xmax and ahi=accel(xmax) 
473 //in all cases the sim is left such that the control=xmax and accel=ahi
474
475 bool FGTrim::checkLimits(void) {
476   bool solutionExists;
477   float current_control=TrimAxes[current_axis]->GetControl();
478   float current_accel=TrimAxes[current_axis]->GetAccel();
479   xlo=TrimAxes[current_axis]->GetControlMin();
480   xhi=TrimAxes[current_axis]->GetControlMax();
481
482   TrimAxes[current_axis]->SetControl(xlo);
483   TrimAxes[current_axis]->Run();
484   alo=TrimAxes[current_axis]->GetAccel();
485   TrimAxes[current_axis]->SetControl(xhi);
486   TrimAxes[current_axis]->Run();
487   ahi=TrimAxes[current_axis]->GetAccel();
488   if(Debug > 1)
489     cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
490                                               << alo << ", " << ahi << endl;
491   solutionDomain=0;
492   solutionExists=false;
493   if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
494     if(alo*current_accel <= 0) {
495       solutionExists=true;
496       solutionDomain=-1;
497       xhi=current_control;
498       ahi=current_accel;
499     } else if(current_accel*ahi < 0){
500       solutionExists=true;
501       solutionDomain=1;
502       xlo=current_control;
503       alo=current_accel;  
504     }
505   } 
506   TrimAxes[current_axis]->SetControl(current_control);
507   TrimAxes[current_axis]->Run();
508   return solutionExists;
509 }
510
511
512
513
514 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
515