Branch data Line data Source code
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 Lesser 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 Lesser General Public License for more
17 : : details.
18 : :
19 : : You should have received a copy of the GNU Lesser 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 Lesser General Public License can also be found on
24 : : the world wide web at http://www.gnu.org.
25 : :
26 : : HISTORY
27 : : --------------------------------------------------------------------------------
28 : : 9/8/99 TP Created
29 : :
30 : : FUNCTIONAL DESCRIPTION
31 : : --------------------------------------------------------------------------------
32 : :
33 : : This class takes the given set of IC's and finds the angle of attack, elevator,
34 : : and throttle setting required to fly steady level. This is currently for in-air
35 : : conditions only. It is implemented using an iterative, one-axis-at-a-time
36 : : scheme. */
37 : :
38 : : // !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
39 : :
40 : : /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
41 : : INCLUDES
42 : : %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
43 : :
44 : : #include <cstdlib>
45 : : #include <iomanip>
46 : : #include "FGTrim.h"
47 : : #include "models/FGAtmosphere.h"
48 : : #include "FGInitialCondition.h"
49 : : #include "models/FGAircraft.h"
50 : : #include "models/FGMassBalance.h"
51 : : #include "models/FGGroundReactions.h"
52 : : #include "models/FGInertial.h"
53 : : #include "models/FGAerodynamics.h"
54 : : #include "models/FGPropulsion.h"
55 : : #include "models/propulsion/FGEngine.h"
56 : : #include "math/FGColumnVector3.h"
57 : :
58 : : #if _MSC_VER
59 : : #pragma warning (disable : 4786 4788)
60 : : #endif
61 : :
62 : : using namespace std;
63 : :
64 : : namespace JSBSim {
65 : :
66 : : static const char *IdSrc = "$Id: FGTrim.cpp,v 1.13 2010/04/23 17:23:40 dpculp Exp $";
67 : : static const char *IdHdr = ID_TRIM;
68 : :
69 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
70 : :
71 : 0 : FGTrim::FGTrim(FGFDMExec *FDMExec,TrimMode tt) {
72 : :
73 : 0 : N=Nsub=0;
74 : 0 : max_iterations=60;
75 : 0 : max_sub_iterations=100;
76 : 0 : Tolerance=1E-3;
77 : 0 : A_Tolerance = Tolerance / 10;
78 : :
79 : 0 : Debug=0;DebugLevel=0;
80 : 0 : fdmex=FDMExec;
81 : 0 : fgic=fdmex->GetIC();
82 : 0 : total_its=0;
83 : 0 : trimudot=true;
84 : 0 : gamma_fallback=false;
85 : 0 : axis_count=0;
86 : 0 : mode=tt;
87 : 0 : xlo=xhi=alo=ahi=0.0;
88 : 0 : targetNlf=1.0;
89 : 0 : debug_axis=tAll;
90 : 0 : SetMode(tt);
91 [ # # ][ # # ]: 0 : if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
92 : 0 : }
93 : :
94 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
95 : :
96 : 0 : FGTrim::~FGTrim(void) {
97 [ # # ][ # # ]: 0 : for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
98 [ # # ][ # # ]: 0 : delete TrimAxes[current_axis];
99 : : }
100 [ # # ][ # # ]: 0 : delete[] sub_iterations;
101 [ # # ][ # # ]: 0 : delete[] successful;
102 [ # # ][ # # ]: 0 : delete[] solution;
103 [ # # ][ # # ]: 0 : if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
104 : 0 : }
105 : :
106 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
107 : :
108 : 0 : void FGTrim::TrimStats() {
109 : 0 : int run_sum=0;
110 : 0 : cout << endl << " Trim Statistics: " << endl;
111 : 0 : cout << " Total Iterations: " << total_its << endl;
112 [ # # ]: 0 : if( total_its > 0) {
113 : 0 : cout << " Sub-iterations:" << endl;
114 [ # # ]: 0 : for (current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
115 : 0 : run_sum += TrimAxes[current_axis]->GetRunCount();
116 : : cout << " " << setw(5) << TrimAxes[current_axis]->GetStateName().c_str()
117 : : << ": " << setprecision(3) << sub_iterations[current_axis]
118 : : << " average: " << setprecision(5) << sub_iterations[current_axis]/double(total_its)
119 : : << " successful: " << setprecision(3) << successful[current_axis]
120 : : << " stability: " << setprecision(5) << TrimAxes[current_axis]->GetAvgStability()
121 : 0 : << endl;
122 : : }
123 : 0 : cout << " Run Count: " << run_sum << endl;
124 : : }
125 : 0 : }
126 : :
127 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
128 : :
129 : 0 : void FGTrim::Report(void) {
130 : 0 : cout << " Trim Results: " << endl;
131 [ # # ]: 0 : for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
132 : 0 : TrimAxes[current_axis]->AxisReport();
133 : :
134 : 0 : }
135 : :
136 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
137 : :
138 : 0 : void FGTrim::ClearStates(void) {
139 : : FGTrimAxis* ta;
140 : :
141 : 0 : mode=tCustom;
142 : : vector<FGTrimAxis*>::iterator iAxes;
143 : 0 : iAxes = TrimAxes.begin();
144 [ # # ]: 0 : while (iAxes != TrimAxes.end()) {
145 : 0 : ta=*iAxes;
146 [ # # ]: 0 : delete ta;
147 : 0 : iAxes++;
148 : : }
149 : 0 : TrimAxes.clear();
150 : : //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
151 : 0 : }
152 : :
153 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
154 : :
155 : 0 : bool FGTrim::AddState( State state, Control control ) {
156 : : FGTrimAxis* ta;
157 : 0 : bool result=true;
158 : :
159 : 0 : mode = tCustom;
160 : 0 : vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
161 [ # # ]: 0 : while (iAxes != TrimAxes.end()) {
162 : 0 : ta=*iAxes;
163 [ # # ]: 0 : if( ta->GetStateType() == state )
164 : 0 : result=false;
165 : 0 : iAxes++;
166 : : }
167 [ # # ]: 0 : if(result) {
168 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
169 [ # # ]: 0 : delete[] sub_iterations;
170 [ # # ]: 0 : delete[] successful;
171 [ # # ]: 0 : delete[] solution;
172 : 0 : sub_iterations=new double[TrimAxes.size()];
173 : 0 : successful=new double[TrimAxes.size()];
174 : 0 : solution=new bool[TrimAxes.size()];
175 : : }
176 : 0 : return result;
177 : : }
178 : :
179 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
180 : :
181 : 0 : bool FGTrim::RemoveState( State state ) {
182 : : FGTrimAxis* ta;
183 : 0 : bool result=false;
184 : :
185 : 0 : mode = tCustom;
186 : 0 : vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
187 [ # # ]: 0 : while (iAxes != TrimAxes.end()) {
188 : 0 : ta=*iAxes;
189 [ # # ]: 0 : if( ta->GetStateType() == state ) {
190 [ # # ]: 0 : delete ta;
191 : 0 : iAxes = TrimAxes.erase(iAxes);
192 : 0 : result=true;
193 : 0 : continue;
194 : : }
195 : 0 : iAxes++;
196 : : }
197 [ # # ]: 0 : if(result) {
198 [ # # ]: 0 : delete[] sub_iterations;
199 [ # # ]: 0 : delete[] successful;
200 [ # # ]: 0 : delete[] solution;
201 : 0 : sub_iterations=new double[TrimAxes.size()];
202 : 0 : successful=new double[TrimAxes.size()];
203 : 0 : solution=new bool[TrimAxes.size()];
204 : : }
205 : 0 : return result;
206 : : }
207 : :
208 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
209 : :
210 : 0 : bool FGTrim::EditState( State state, Control new_control ){
211 : : FGTrimAxis* ta;
212 : 0 : bool result=false;
213 : :
214 : 0 : mode = tCustom;
215 : 0 : vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
216 [ # # ]: 0 : while (iAxes != TrimAxes.end()) {
217 : 0 : ta=*iAxes;
218 [ # # ]: 0 : if( ta->GetStateType() == state ) {
219 : 0 : TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
220 [ # # ]: 0 : delete ta;
221 : 0 : TrimAxes.erase(iAxes+1);
222 : 0 : result=true;
223 : 0 : break;
224 : : }
225 : 0 : iAxes++;
226 : : }
227 : 0 : return result;
228 : : }
229 : :
230 : :
231 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
232 : :
233 : 0 : bool FGTrim::DoTrim(void) {
234 : :
235 : 0 : trim_failed=false;
236 : : int i;
237 : :
238 [ # # ]: 0 : for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
239 : 0 : fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(false);
240 : : }
241 : :
242 : 0 : fdmex->DisableOutput();
243 : :
244 : 0 : setEngineTrimMode(true);
245 : :
246 : 0 : fgic->SetPRadpsIC(0.0);
247 : 0 : fgic->SetQRadpsIC(0.0);
248 : 0 : fgic->SetRRadpsIC(0.0);
249 : :
250 : : //clear the sub iterations counts & zero out the controls
251 [ # # ]: 0 : 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 [ # # ]: 0 : if(TrimAxes[current_axis]->GetStateType() == tQdot) {
255 [ # # ]: 0 : if(mode == tGround) {
256 : 0 : TrimAxes[current_axis]->initTheta();
257 : : }
258 : : }
259 : 0 : xlo=TrimAxes[current_axis]->GetControlMin();
260 : 0 : xhi=TrimAxes[current_axis]->GetControlMax();
261 : 0 : TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
262 : 0 : TrimAxes[current_axis]->Run();
263 : : //TrimAxes[current_axis]->AxisReport();
264 : 0 : sub_iterations[current_axis]=0;
265 : 0 : successful[current_axis]=0;
266 : 0 : solution[current_axis]=false;
267 : : }
268 : :
269 : :
270 [ # # ]: 0 : if(mode == tPullup ) {
271 : 0 : cout << "Setting pitch rate and nlf... " << endl;
272 : 0 : setupPullup();
273 : 0 : cout << "pitch rate done ... " << endl;
274 : 0 : TrimAxes[0]->SetStateTarget(targetNlf);
275 : 0 : cout << "nlf done" << endl;
276 [ # # ]: 0 : } else if (mode == tTurn) {
277 : 0 : setupTurn();
278 : : //TrimAxes[0]->SetStateTarget(targetNlf);
279 : : }
280 : :
281 [ # # ][ # # ]: 0 : do {
[ # # ]
282 : 0 : axis_count=0;
283 [ # # ]: 0 : for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
284 : : setDebug();
285 : 0 : updateRates();
286 : 0 : Nsub=0;
287 [ # # ]: 0 : if(!solution[current_axis]) {
288 [ # # ]: 0 : if(checkLimits()) {
289 : 0 : solution[current_axis]=true;
290 : 0 : solve();
291 : : }
292 [ # # ]: 0 : } else if(findInterval()) {
293 : 0 : solve();
294 : : } else {
295 : 0 : solution[current_axis]=false;
296 : : }
297 : 0 : sub_iterations[current_axis]+=Nsub;
298 : : }
299 [ # # ]: 0 : for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
300 : : //these checks need to be done after all the axes have run
301 [ # # ]: 0 : if(Debug > 0) TrimAxes[current_axis]->AxisReport();
302 [ # # ]: 0 : if(TrimAxes[current_axis]->InTolerance()) {
303 : 0 : axis_count++;
304 : 0 : successful[current_axis]++;
305 : : }
306 : : }
307 : :
308 : :
309 [ # # ][ # # ]: 0 : 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
314 : :
315 : : //Oh, well: two out of three ain't bad
316 [ # # ]: 0 : for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
317 : : //these checks need to be done after all the axes have run
318 [ # # ]: 0 : if(!TrimAxes[current_axis]->InTolerance()) {
319 [ # # ]: 0 : if(!checkLimits()) {
320 : : // special case this for now -- if other cases arise proper
321 : : // support can be added to FGTrimAxis
322 [ # # ][ # # ]: 0 : 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 : 0 : << " path angle. (" << N << ")" << endl;
327 [ # # ]: 0 : if(TrimAxes[current_axis]->GetState() > 0)
328 : 0 : TrimAxes[current_axis]->SetControlToMin();
329 : : else
330 : 0 : TrimAxes[current_axis]->SetControlToMax();
331 : 0 : TrimAxes[current_axis]->Run();
332 [ # # ]: 0 : delete TrimAxes[current_axis];
333 : : TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
334 : 0 : tGamma );
335 : : } else {
336 : : cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
337 : 0 : << " doesn't appear to be trimmable" << endl;
338 : : //total_its=k;
339 : 0 : trim_failed=true; //force the trim to fail
340 : : } //gamma_fallback
341 : : }
342 : : } //solution check
343 : : } //for loop
344 : : } //all-but-one check
345 : 0 : N++;
346 [ # # ]: 0 : if(N > max_iterations)
347 : 0 : trim_failed=true;
348 : : } while((axis_count < TrimAxes.size()) && (!trim_failed));
349 [ # # ][ # # ]: 0 : if((!trim_failed) && (axis_count >= TrimAxes.size())) {
[ # # ]
350 : 0 : total_its=N;
351 [ # # ]: 0 : if (debug_lvl > 0)
352 : 0 : cout << endl << " Trim successful" << endl;
353 : : } else {
354 : 0 : total_its=N;
355 [ # # ]: 0 : if (debug_lvl > 0)
356 : 0 : cout << endl << " Trim failed" << endl;
357 : : }
358 [ # # ]: 0 : for(i=0;i < fdmex->GetGroundReactions()->GetNumGearUnits();i++){
359 : 0 : fdmex->GetGroundReactions()->GetGearUnit(i)->SetReport(true);
360 : : }
361 : 0 : setEngineTrimMode(false);
362 : 0 : fdmex->EnableOutput();
363 : 0 : return !trim_failed;
364 : : }
365 : :
366 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
367 : :
368 : 0 : bool FGTrim::solve(void) {
369 : :
370 : : double x1,x2,x3,f1,f2,f3,d,d0;
371 : 0 : const double relax =0.9;
372 : 0 : double eps=TrimAxes[current_axis]->GetSolverEps();
373 : :
374 : 0 : x1=x2=x3=0;
375 : 0 : d=1;
376 : 0 : bool success=false;
377 : : //initializations
378 [ # # ]: 0 : if( solutionDomain != 0) {
379 : : /* if(ahi > alo) { */
380 : 0 : x1=xlo;f1=alo;
381 : 0 : x3=xhi;f3=ahi;
382 : : /* } else {
383 : : x1=xhi;f1=ahi;
384 : : x3=xlo;f3=alo;
385 : : } */
386 : 0 : d0=fabs(x3-x1);
387 : : //iterations
388 : : //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
389 [ # # ][ # # ]: 0 : while ( (TrimAxes[current_axis]->InTolerance() == false )
[ # # ][ # # ]
390 : : && (fabs(d) > eps) && (Nsub < max_sub_iterations)) {
391 : 0 : Nsub++;
392 : 0 : d=(x3-x1)/d0;
393 : 0 : x2=x1-d*d0*f1/(f3-f1);
394 : 0 : TrimAxes[current_axis]->SetControl(x2);
395 : 0 : TrimAxes[current_axis]->Run();
396 : 0 : f2=TrimAxes[current_axis]->GetState();
397 [ # # ]: 0 : if(Debug > 1) {
398 : : cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
399 : 0 : << ", " << x2 << ", " << x3 << endl;
400 : 0 : cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
401 : : }
402 [ # # ]: 0 : if(f1*f2 <= 0.0) {
403 : 0 : x3=x2;
404 : 0 : f3=f2;
405 : 0 : f1=relax*f1;
406 : : //cout << "Solution is between x1 and x2" << endl;
407 : : }
408 [ # # ]: 0 : else if(f2*f3 <= 0.0) {
409 : 0 : x1=x2;
410 : 0 : f1=f2;
411 : 0 : f3=relax*f3;
412 : : //cout << "Solution is between x2 and x3" << endl;
413 : :
414 : : }
415 : : //cout << i << endl;
416 : :
417 : :
418 : : }//end while
419 [ # # ]: 0 : if(Nsub < max_sub_iterations) success=true;
420 : : }
421 : 0 : return success;
422 : : }
423 : :
424 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
425 : : /*
426 : : produces an interval (xlo..xhi) on one side or the other of the current
427 : : control value in which a solution exists. This domain is, hopefully,
428 : : smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
429 : : to find the solution. This is, hopefully, more efficient than having the
430 : : solver start from scratch every time. Maybe it isn't though...
431 : : This tries to take advantage of the idea that the changes from iteration to
432 : : iteration will be small after the first one or two top-level iterations.
433 : :
434 : : assumes that changing the control will a produce significant change in the
435 : : accel i.e. checkLimits() has already been called.
436 : :
437 : : if a solution is found above the current control, the function returns true
438 : : and xlo is set to the current control, xhi to the interval max it found, and
439 : : solutionDomain is set to 1.
440 : : if the solution lies below the current control, then the function returns
441 : : true and xlo is set to the interval min it found and xmax to the current
442 : : control. if no solution is found, then the function returns false.
443 : :
444 : :
445 : : in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
446 : : no assumptions about the state of the sim after this function has run
447 : : can be made.
448 : : */
449 : 0 : bool FGTrim::findInterval(void) {
450 : 0 : bool found=false;
451 : : double step;
452 : 0 : double current_control=TrimAxes[current_axis]->GetControl();
453 : 0 : double current_accel=TrimAxes[current_axis]->GetState();;
454 : 0 : double xmin=TrimAxes[current_axis]->GetControlMin();
455 : 0 : double xmax=TrimAxes[current_axis]->GetControlMax();
456 : : double lastxlo,lastxhi,lastalo,lastahi;
457 : :
458 : 0 : step=0.025*fabs(xmax);
459 : 0 : xlo=xhi=current_control;
460 : 0 : alo=ahi=current_accel;
461 : 0 : lastxlo=xlo;lastxhi=xhi;
462 : 0 : lastalo=alo;lastahi=ahi;
463 [ # # ][ # # ]: 0 : do {
464 : :
465 : 0 : Nsub++;
466 : 0 : step*=2;
467 : 0 : xlo-=step;
468 [ # # ]: 0 : if(xlo < xmin) xlo=xmin;
469 : 0 : xhi+=step;
470 [ # # ]: 0 : if(xhi > xmax) xhi=xmax;
471 : 0 : TrimAxes[current_axis]->SetControl(xlo);
472 : 0 : TrimAxes[current_axis]->Run();
473 : 0 : alo=TrimAxes[current_axis]->GetState();
474 : 0 : TrimAxes[current_axis]->SetControl(xhi);
475 : 0 : TrimAxes[current_axis]->Run();
476 : 0 : ahi=TrimAxes[current_axis]->GetState();
477 [ # # ]: 0 : if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
478 [ # # ]: 0 : if(alo*ahi <=0) { //found interval with root
479 : 0 : found=true;
480 [ # # ]: 0 : if(alo*current_accel <= 0) { //narrow interval down a bit
481 : 0 : solutionDomain=-1;
482 : 0 : xhi=lastxlo;
483 : 0 : ahi=lastalo;
484 : : //xhi=current_control;
485 : : //ahi=current_accel;
486 : : } else {
487 : 0 : solutionDomain=1;
488 : 0 : xlo=lastxhi;
489 : 0 : alo=lastahi;
490 : : //xlo=current_control;
491 : : //alo=current_accel;
492 : : }
493 : : }
494 : 0 : lastxlo=xlo;lastxhi=xhi;
495 : 0 : lastalo=alo;lastahi=ahi;
496 [ # # ][ # # ]: 0 : if( !found && xlo==xmin && xhi==xmax ) continue;
[ # # ]
497 [ # # ]: 0 : if(Debug > 1)
498 : : cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
499 : 0 : << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
500 : : } while(!found && (Nsub <= max_sub_iterations) );
501 : 0 : return found;
502 : : }
503 : :
504 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
505 : : //checks to see which side of the current control value the solution is on
506 : : //and sets solutionDomain accordingly:
507 : : // 1 if solution is between the current and max
508 : : // -1 if solution is between the min and current
509 : : // 0 if there is no solution
510 : : //
511 : : //if changing the control produces no significant change in the accel then
512 : : //solutionDomain is set to zero and the function returns false
513 : : //if a solution is found, then xlo and xhi are set so that they bracket
514 : : //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
515 : : //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
516 : : //xhi=xmax and ahi=accel(xmax)
517 : : //in all cases the sim is left such that the control=xmax and accel=ahi
518 : :
519 : 0 : bool FGTrim::checkLimits(void) {
520 : : bool solutionExists;
521 : 0 : double current_control=TrimAxes[current_axis]->GetControl();
522 : 0 : double current_accel=TrimAxes[current_axis]->GetState();
523 : 0 : xlo=TrimAxes[current_axis]->GetControlMin();
524 : 0 : xhi=TrimAxes[current_axis]->GetControlMax();
525 : :
526 : 0 : TrimAxes[current_axis]->SetControl(xlo);
527 : 0 : TrimAxes[current_axis]->Run();
528 : 0 : alo=TrimAxes[current_axis]->GetState();
529 : 0 : TrimAxes[current_axis]->SetControl(xhi);
530 : 0 : TrimAxes[current_axis]->Run();
531 : 0 : ahi=TrimAxes[current_axis]->GetState();
532 [ # # ]: 0 : if(Debug > 1)
533 : : cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
534 : 0 : << alo << ", " << ahi << endl;
535 : 0 : solutionDomain=0;
536 : 0 : solutionExists=false;
537 [ # # ]: 0 : if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
538 [ # # ]: 0 : if(alo*current_accel <= 0) {
539 : 0 : solutionExists=true;
540 : 0 : solutionDomain=-1;
541 : 0 : xhi=current_control;
542 : 0 : ahi=current_accel;
543 [ # # ]: 0 : } else if(current_accel*ahi < 0){
544 : 0 : solutionExists=true;
545 : 0 : solutionDomain=1;
546 : 0 : xlo=current_control;
547 : 0 : alo=current_accel;
548 : : }
549 : : }
550 : 0 : TrimAxes[current_axis]->SetControl(current_control);
551 : 0 : TrimAxes[current_axis]->Run();
552 : 0 : return solutionExists;
553 : : }
554 : :
555 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
556 : :
557 : 0 : void FGTrim::setupPullup() {
558 : : double g,q,cgamma;
559 : 0 : g=fdmex->GetInertial()->gravity();
560 : 0 : cgamma=cos(fgic->GetFlightPathAngleRadIC());
561 : : cout << "setPitchRateInPullup(): " << g << ", " << cgamma << ", "
562 : 0 : << fgic->GetVtrueFpsIC() << endl;
563 : 0 : q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
564 : 0 : cout << targetNlf << ", " << q << endl;
565 : 0 : fgic->SetQRadpsIC(q);
566 : 0 : cout << "setPitchRateInPullup() complete" << endl;
567 : :
568 : 0 : }
569 : :
570 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
571 : :
572 : 0 : void FGTrim::setupTurn(void){
573 : : double g,phi;
574 : 0 : phi = fgic->GetPhiRadIC();
575 [ # # ][ # # ]: 0 : if( fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
576 : 0 : targetNlf = 1 / cos(phi);
577 : 0 : g = fdmex->GetInertial()->gravity();
578 : 0 : psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
579 : 0 : cout << targetNlf << ", " << psidot << endl;
580 : : }
581 : :
582 : 0 : }
583 : :
584 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
585 : :
586 : 0 : void FGTrim::updateRates(void){
587 [ # # ]: 0 : if( mode == tTurn ) {
588 : 0 : double phi = fgic->GetPhiRadIC();
589 : 0 : double g = fdmex->GetInertial()->gravity();
590 : : double p,q,r,theta;
591 [ # # ][ # # ]: 0 : if(fabs(phi) > 0.001 && fabs(phi) < 1.56 ) {
592 : 0 : theta=fgic->GetThetaRadIC();
593 : 0 : phi=fgic->GetPhiRadIC();
594 : 0 : psidot = g*tan(phi) / fgic->GetUBodyFpsIC();
595 : 0 : p=-psidot*sin(theta);
596 : 0 : q=psidot*cos(theta)*sin(phi);
597 : 0 : r=psidot*cos(theta)*cos(phi);
598 : : } else {
599 : 0 : p=q=r=0;
600 : : }
601 : 0 : fgic->SetPRadpsIC(p);
602 : 0 : fgic->SetQRadpsIC(q);
603 : 0 : fgic->SetRRadpsIC(r);
604 [ # # ][ # # ]: 0 : } else if( mode == tPullup && fabs(targetNlf-1) > 0.01) {
605 : : double g,q,cgamma;
606 : 0 : g=fdmex->GetInertial()->gravity();
607 : 0 : cgamma=cos(fgic->GetFlightPathAngleRadIC());
608 : 0 : q=g*(targetNlf-cgamma)/fgic->GetVtrueFpsIC();
609 : 0 : fgic->SetQRadpsIC(q);
610 : : }
611 : 0 : }
612 : :
613 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
614 : :
615 : 0 : void FGTrim::setDebug(void) {
616 [ # # ][ # # ]: 0 : if(debug_axis == tAll ||
[ # # ][ # # ]
[ # # ][ # # ]
617 : : TrimAxes[current_axis]->GetStateType() == debug_axis ) {
618 : 0 : Debug=DebugLevel;
619 : 0 : return;
620 : : } else {
621 : 0 : Debug=0;
622 : 0 : return;
623 : : }
624 : : }
625 : :
626 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
627 : :
628 : 0 : void FGTrim::setEngineTrimMode(bool mode) {
629 : 0 : FGPropulsion* prop = fdmex->GetPropulsion();
630 [ # # ]: 0 : for (unsigned int i=0; i<prop->GetNumEngines(); i++) {
631 : 0 : prop->GetEngine(i)->SetTrimMode(mode);
632 : : }
633 : 0 : }
634 : :
635 : : //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
636 : :
637 : 0 : void FGTrim::SetMode(TrimMode tt) {
638 : 0 : ClearStates();
639 : 0 : mode=tt;
640 [ # # # # : 0 : switch(tt) {
# # ]
641 : : case tFull:
642 [ # # ]: 0 : if (debug_lvl > 0)
643 : 0 : cout << " Full Trim" << endl;
644 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
645 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
646 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
647 : : //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
648 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
649 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
650 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
651 : 0 : break;
652 : : case tLongitudinal:
653 [ # # ]: 0 : if (debug_lvl > 0)
654 : 0 : cout << " Longitudinal Trim" << endl;
655 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
656 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
657 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
658 : 0 : break;
659 : : case tGround:
660 [ # # ]: 0 : if (debug_lvl > 0)
661 : 0 : cout << " Ground Trim" << endl;
662 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
663 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
664 : : //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
665 : 0 : break;
666 : : case tPullup:
667 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tNlf,tAlpha ));
668 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
669 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
670 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
671 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
672 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
673 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
674 : 0 : break;
675 : : case tTurn:
676 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
677 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
678 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
679 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tBeta ));
680 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
681 : 0 : TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
682 : : break;
683 : : case tCustom:
684 : : case tNone:
685 : : break;
686 : : }
687 : : //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
688 : 0 : sub_iterations=new double[TrimAxes.size()];
689 : 0 : successful=new double[TrimAxes.size()];
690 : 0 : solution=new bool[TrimAxes.size()];
691 : 0 : current_axis=0;
692 : 0 : }
693 : : //YOU WERE WARNED, BUT YOU DID IT ANYWAY.
694 [ + + ][ + - ]: 12 : }
|
