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"
55 static const char *IdSrc = "$Header$";
56 static const char *IdHdr = ID_TRIM;
58 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
60 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
64 max_sub_iterations=100;
66 A_Tolerance = Tolerance / 10;
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));
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));
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));
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];
108 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
110 FGTrim::~FGTrim(void) {
111 for(current_axis=0; current_axis<NumAxes; current_axis++) {
112 delete TrimAxes[current_axis];
114 delete[] sub_iterations;
119 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
121 void FGTrim::TrimStats() {
124 cout << endl << " Trim Statistics: " << endl;
125 cout << " Total Iterations: " << total_its << endl;
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() );
138 cout << " Run Count: " << run_sum << endl;
142 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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();
151 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153 void FGTrim::ReportState(void) {
154 char out[80], flap[10], gear[10];
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) );
163 if( fdmex->GetFCS()->GetDfPos() <= 0.01)
166 sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
167 if(fdmex->GetAircraft()->GetGearUp() == true)
170 sprintf(gear,"Down");
171 sprintf(out, " Flaps: %3s Gear: %4s\n",flap,gear);
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() );
178 sprintf(out, " Altitude: %7.0f ft. AGL Altitude: %7.0f ft.\n",
179 fdmex->GetPosition()->Geth(),
180 fdmex->GetPosition()->GetDistanceAGL() );
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 );
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 );
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(FG_PITCHRATE)*RADTODEG );
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 );
198 sprintf(out, " Bank Angle: %5.2f deg\n",
199 fdmex->GetRotation()->Getphi()*RADTODEG );
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 );
206 sprintf(out, " Throttle: %5.2f%c\n",
207 fdmex->GetFCS()->GetThrottlePos(0),'%' );
211 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
213 bool FGTrim::DoTrim(void) {
218 for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
219 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
222 fdmex->GetOutput()->Disable();
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) {
230 TrimAxes[current_axis]->initTheta();
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;
243 for(current_axis=0;current_axis<NumAxes;current_axis++) {
245 if(!solution[current_axis]) {
247 solution[current_axis]=true;
250 } else if(findInterval()) {
253 solution[current_axis]=false;
255 sub_iterations[current_axis]+=Nsub;
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()) {
262 successful[current_axis]++;
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
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()) {
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();
288 TrimAxes[current_axis]->SetControlToMax();
289 TrimAxes[current_axis]->Run();
290 delete TrimAxes[current_axis];
291 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
294 cout << " Sorry, " << TrimAxes[current_axis]->GetAccelName()
295 << " doesn't appear to be trimmable" << endl;
297 trim_failed=true; //force the trim to fail
302 } //all-but-one check
304 if(N > max_iterations)
306 } while((axis_count < NumAxes) && (!trim_failed));
307 if((!trim_failed) && (axis_count >= NumAxes)) {
309 cout << endl << " Trim successful" << endl;
312 cout << endl << " Trim failed" << endl;
314 for(int i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
315 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
317 fdmex->GetOutput()->Enable();
321 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
323 bool FGTrim::solve(void) {
325 float x1,x2,x3,f1,f2,f3,d,d0;
326 const float relax =0.9;
327 float eps=TrimAxes[current_axis]->GetSolverEps();
333 if( solutionDomain != 0) {
334 /* if(ahi > alo) { */
344 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
345 while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
346 && (Nsub < max_sub_iterations)) {
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();
354 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
355 << ", " << x2 << ", " << x3 << endl;
356 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
362 //cout << "Solution is between x1 and x2" << endl;
364 else if(f2*f3 <= 0.0) {
368 //cout << "Solution is between x2 and x3" << endl;
375 if(Nsub < max_sub_iterations) success=true;
380 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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.
390 assumes that changing the control will a produce significant change in the
391 accel i.e. checkLimits() has already been called.
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.
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
405 bool FGTrim::findInterval(void) {
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;
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;
424 if(xlo < xmin) xlo=xmin;
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
436 if(alo*current_accel <= 0) { //narrow interval down a bit
440 //xhi=current_control;
446 //xlo=current_control;
450 lastxlo=xlo;lastxhi=xhi;
451 lastalo=alo;lastahi=ahi;
452 if( !found && xlo==xmin && xhi==xmax ) continue;
454 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
455 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
456 } while(!found && (Nsub <= max_sub_iterations) );
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
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
475 bool FGTrim::checkLimits(void) {
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();
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();
489 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
490 << alo << ", " << ahi << endl;
492 solutionExists=false;
493 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
494 if(alo*current_accel <= 0) {
499 } else if(current_accel*ahi < 0){
506 TrimAxes[current_axis]->SetControl(current_control);
507 TrimAxes[current_axis]->Run();
508 return solutionExists;
514 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.