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 "FGAerodynamics.h"
57 #pragma warning (disable : 4786 4788)
60 static const char *IdSrc = "$Id$";
61 static const char *IdHdr = ID_TRIM;
63 extern short debug_lvl;
65 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
67 FGTrim::FGTrim(FGFDMExec *FDMExec,FGInitialCondition *FGIC, TrimMode tt ) {
71 max_sub_iterations=100;
73 A_Tolerance = Tolerance / 10;
86 cout << " Full Trim" << endl;
87 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
88 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
89 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
90 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tHmgt,tBeta ));
91 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tVdot,tPhi ));
92 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tAileron ));
93 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tRdot,tRudder ));
96 cout << " Longitudinal Trim" << endl;
97 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAlpha ));
98 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tUdot,tThrottle ));
99 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tPitchTrim ));
102 cout << " Ground Trim" << endl;
103 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tWdot,tAltAGL ));
104 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tQdot,tTheta ));
105 //TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,tPdot,tPhi ));
108 //cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
109 sub_iterations=new float[TrimAxes.size()];
110 successful=new float[TrimAxes.size()];
111 solution=new bool[TrimAxes.size()];
114 if (debug_lvl & 2) cout << "Instantiated: FGTrim" << endl;
117 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
119 FGTrim::~FGTrim(void) {
120 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
121 delete TrimAxes[current_axis];
123 delete[] sub_iterations;
126 if (debug_lvl & 2) cout << "Destroyed: FGTrim" << endl;
129 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
131 void FGTrim::TrimStats() {
134 cout << endl << " Trim Statistics: " << endl;
135 cout << " Total Iterations: " << total_its << endl;
137 cout << " Sub-iterations:" << endl;
138 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++) {
139 run_sum+=TrimAxes[current_axis]->GetRunCount();
140 sprintf(out," %5s: %3.0f average: %5.2f successful: %3.0f stability: %5.2f\n",
141 TrimAxes[current_axis]->GetStateName().c_str(),
142 sub_iterations[current_axis],
143 sub_iterations[current_axis]/float(total_its),
144 successful[current_axis],
145 TrimAxes[current_axis]->GetAvgStability() );
148 cout << " Run Count: " << run_sum << endl;
152 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
154 void FGTrim::Report(void) {
155 cout << " Trim Results: " << endl;
156 for(current_axis=0; current_axis<TrimAxes.size(); current_axis++)
157 TrimAxes[current_axis]->AxisReport();
161 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
163 void FGTrim::ReportState(void) {
164 char out[80], flap[10], gear[10];
166 cout << endl << " JSBSim State" << endl;
167 sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n",
168 fdmex->GetMassBalance()->GetWeight(),
169 fdmex->GetMassBalance()->GetXYZcg(1),
170 fdmex->GetMassBalance()->GetXYZcg(2),
171 fdmex->GetMassBalance()->GetXYZcg(3));
173 if( fdmex->GetFCS()->GetDfPos() <= 0.01)
176 sprintf(flap,"%2.0f",fdmex->GetFCS()->GetDfPos());
177 if(fdmex->GetAircraft()->GetGearUp() == true)
180 sprintf(gear,"Down");
181 sprintf(out, " Flaps: %3s Gear: %4s\n",flap,gear);
183 sprintf(out, " Speed: %4.0f KCAS Mach: %5.2f\n",
184 fdmex->GetAuxiliary()->GetVcalibratedKTS(),
185 fdmex->GetState()->GetParameter(FG_MACH),
186 fdmex->GetPosition()->Geth() );
188 sprintf(out, " Altitude: %7.0f ft. AGL Altitude: %7.0f ft.\n",
189 fdmex->GetPosition()->Geth(),
190 fdmex->GetPosition()->GetDistanceAGL() );
192 sprintf(out, " Angle of Attack: %6.2f deg Pitch Angle: %6.2f deg\n",
193 fdmex->GetState()->GetParameter(FG_ALPHA)*RADTODEG,
194 fdmex->GetRotation()->Gettht()*RADTODEG );
196 sprintf(out, " Flight Path Angle: %6.2f deg Climb Rate: %5.0f ft/min\n",
197 fdmex->GetPosition()->GetGamma()*RADTODEG,
198 fdmex->GetPosition()->Gethdot()*60 );
200 sprintf(out, " Normal Load Factor: %4.2f g's Pitch Rate: %5.2f deg/s\n",
201 fdmex->GetAerodynamics()->GetNlf(),
202 fdmex->GetState()->GetParameter(FG_PITCHRATE)*RADTODEG );
204 sprintf(out, " Heading: %3.0f deg true Sideslip: %5.2f deg\n",
205 fdmex->GetRotation()->Getpsi()*RADTODEG,
206 fdmex->GetState()->GetParameter(FG_BETA)*RADTODEG );
208 sprintf(out, " Bank Angle: %5.2f deg\n",
209 fdmex->GetRotation()->Getphi()*RADTODEG );
211 sprintf(out, " Elevator: %5.2f deg Left Aileron: %5.2f deg Rudder: %5.2f deg\n",
212 fdmex->GetState()->GetParameter(FG_ELEVATOR_POS)*RADTODEG,
213 fdmex->GetState()->GetParameter(FG_AILERON_POS)*RADTODEG,
214 fdmex->GetState()->GetParameter(FG_RUDDER_POS)*RADTODEG );
216 sprintf(out, " Throttle: %5.2f%c\n",
217 fdmex->GetFCS()->GetThrottlePos(0),'%' );
220 sprintf(out, " Wind Components: %5.2f kts head wind, %5.2f kts cross wind\n",
221 fdmex->GetAuxiliary()->GetHeadWind()*jsbFPSTOKTS,
222 fdmex->GetAuxiliary()->GetCrossWind()*jsbFPSTOKTS );
225 sprintf(out, " Ground Speed: %4.0f knots , Ground Track: %3.0f deg true\n",
226 fdmex->GetPosition()->GetVground()*jsbFPSTOKTS,
227 fdmex->GetPosition()->GetGroundTrack()*RADTODEG );
232 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
234 void FGTrim::ClearStates(void) {
238 vector<FGTrimAxis*>::iterator iAxes;
239 iAxes = TrimAxes.begin();
240 while (iAxes != TrimAxes.end()) {
246 cout << "TrimAxes.size(): " << TrimAxes.size() << endl;
249 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
251 bool FGTrim::AddState( State state, Control control ) {
256 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
257 while (iAxes != TrimAxes.end()) {
259 if( ta->GetStateType() == state )
264 TrimAxes.push_back(new FGTrimAxis(fdmex,fgic,state,control));
265 delete[] sub_iterations;
268 sub_iterations=new float[TrimAxes.size()];
269 successful=new float[TrimAxes.size()];
270 solution=new bool[TrimAxes.size()];
275 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
277 bool FGTrim::RemoveState( State state ) {
282 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
283 while (iAxes != TrimAxes.end()) {
285 if( ta->GetStateType() == state ) {
287 TrimAxes.erase(iAxes);
294 delete[] sub_iterations;
297 sub_iterations=new float[TrimAxes.size()];
298 successful=new float[TrimAxes.size()];
299 solution=new bool[TrimAxes.size()];
304 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
306 bool FGTrim::EditState( State state, Control new_control ){
311 vector <FGTrimAxis*>::iterator iAxes = TrimAxes.begin();
312 while (iAxes != TrimAxes.end()) {
314 if( ta->GetStateType() == state ) {
315 TrimAxes.insert(iAxes,1,new FGTrimAxis(fdmex,fgic,state,new_control));
317 TrimAxes.erase(iAxes+1);
323 cout << "Exit FGTrim::EditState(...)" << endl;
328 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
330 bool FGTrim::DoTrim(void) {
335 for(i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
336 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(false);
339 fdmex->GetOutput()->Disable();
341 //clear the sub iterations counts & zero out the controls
342 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
343 //cout << current_axis << " " << TrimAxes[current_axis]->GetStateName()
344 //<< " " << TrimAxes[current_axis]->GetControlName()<< endl;
345 if(TrimAxes[current_axis]->GetStateType() == tQdot) {
347 TrimAxes[current_axis]->initTheta();
349 xlo=TrimAxes[current_axis]->GetControlMin();
350 xhi=TrimAxes[current_axis]->GetControlMax();
351 TrimAxes[current_axis]->SetControl((xlo+xhi)/2);
352 TrimAxes[current_axis]->Run();
353 //TrimAxes[current_axis]->AxisReport();
354 sub_iterations[current_axis]=0;
355 successful[current_axis]=0;
356 solution[current_axis]=false;
360 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
362 if(!solution[current_axis]) {
364 solution[current_axis]=true;
367 } else if(findInterval()) {
370 solution[current_axis]=false;
372 sub_iterations[current_axis]+=Nsub;
374 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
375 //these checks need to be done after all the axes have run
376 if(Debug > 0) TrimAxes[current_axis]->AxisReport();
377 if(TrimAxes[current_axis]->InTolerance()) {
379 successful[current_axis]++;
384 if((axis_count == TrimAxes.size()-1) && (TrimAxes.size() > 1)) {
385 //cout << TrimAxes.size()-1 << " out of " << TrimAxes.size() << "!" << endl;
386 //At this point we can check the input limits of the failed axis
387 //and declare the trim failed if there is no sign change. If there
388 //is, keep going until success or max iteration count
390 //Oh, well: two out of three ain't bad
391 for(current_axis=0;current_axis<TrimAxes.size();current_axis++) {
392 //these checks need to be done after all the axes have run
393 if(!TrimAxes[current_axis]->InTolerance()) {
395 // special case this for now -- if other cases arise proper
396 // support can be added to FGTrimAxis
397 if( (gamma_fallback) &&
398 (TrimAxes[current_axis]->GetStateType() == tUdot) &&
399 (TrimAxes[current_axis]->GetControlType() == tThrottle)) {
400 cout << " Can't trim udot with throttle, trying flight"
401 << " path angle. (" << N << ")" << endl;
402 if(TrimAxes[current_axis]->GetState() > 0)
403 TrimAxes[current_axis]->SetControlToMin();
405 TrimAxes[current_axis]->SetControlToMax();
406 TrimAxes[current_axis]->Run();
407 delete TrimAxes[current_axis];
408 TrimAxes[current_axis]=new FGTrimAxis(fdmex,fgic,tUdot,
411 cout << " Sorry, " << TrimAxes[current_axis]->GetStateName()
412 << " doesn't appear to be trimmable" << endl;
414 trim_failed=true; //force the trim to fail
419 } //all-but-one check
421 if(N > max_iterations)
423 } while((axis_count < TrimAxes.size()) && (!trim_failed));
424 if((!trim_failed) && (axis_count >= TrimAxes.size())) {
426 cout << endl << " Trim successful" << endl;
429 cout << endl << " Trim failed" << endl;
431 for(i=0;i < fdmex->GetAircraft()->GetNumGearUnits();i++){
432 fdmex->GetAircraft()->GetGearUnit(i)->SetReport(true);
434 fdmex->GetOutput()->Enable();
438 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
440 bool FGTrim::solve(void) {
442 float x1,x2,x3,f1,f2,f3,d,d0;
443 const float relax =0.9;
444 float eps=TrimAxes[current_axis]->GetSolverEps();
450 if( solutionDomain != 0) {
451 /* if(ahi > alo) { */
461 //max_sub_iterations=TrimAxes[current_axis]->GetIterationLimit();
462 while (!TrimAxes[current_axis]->InTolerance() && (fabs(d) > eps)
463 && (Nsub < max_sub_iterations)) {
466 x2=x1-d*d0*f1/(f3-f1);
467 TrimAxes[current_axis]->SetControl(x2);
468 TrimAxes[current_axis]->Run();
469 f2=TrimAxes[current_axis]->GetState();
471 cout << "FGTrim::solve Nsub,x1,x2,x3: " << Nsub << ", " << x1
472 << ", " << x2 << ", " << x3 << endl;
473 cout << " " << f1 << ", " << f2 << ", " << f3 << endl;
479 //cout << "Solution is between x1 and x2" << endl;
481 else if(f2*f3 <= 0.0) {
485 //cout << "Solution is between x2 and x3" << endl;
492 if(Nsub < max_sub_iterations) success=true;
497 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
499 produces an interval (xlo..xhi) on one side or the other of the current
500 control value in which a solution exists. This domain is, hopefully,
501 smaller than xmin..0 or 0..xmax and the solver will require fewer iterations
502 to find the solution. This is, hopefully, more efficient than having the
503 solver start from scratch every time. Maybe it isn't though...
504 This tries to take advantage of the idea that the changes from iteration to
505 iteration will be small after the first one or two top-level iterations.
507 assumes that changing the control will a produce significant change in the
508 accel i.e. checkLimits() has already been called.
510 if a solution is found above the current control, the function returns true
511 and xlo is set to the current control, xhi to the interval max it found, and
512 solutionDomain is set to 1.
513 if the solution lies below the current control, then the function returns
514 true and xlo is set to the interval min it found and xmax to the current
515 control. if no solution is found, then the function returns false.
518 in all cases, alo=accel(xlo) and ahi=accel(xhi) after the function exits.
519 no assumptions about the state of the sim after this function has run
522 bool FGTrim::findInterval(void) {
525 float current_control=TrimAxes[current_axis]->GetControl();
526 float current_accel=TrimAxes[current_axis]->GetState();;
527 float xmin=TrimAxes[current_axis]->GetControlMin();
528 float xmax=TrimAxes[current_axis]->GetControlMax();
529 float lastxlo,lastxhi,lastalo,lastahi;
531 step=0.025*fabs(xmax);
532 xlo=xhi=current_control;
533 alo=ahi=current_accel;
534 lastxlo=xlo;lastxhi=xhi;
535 lastalo=alo;lastahi=ahi;
541 if(xlo < xmin) xlo=xmin;
543 if(xhi > xmax) xhi=xmax;
544 TrimAxes[current_axis]->SetControl(xlo);
545 TrimAxes[current_axis]->Run();
546 alo=TrimAxes[current_axis]->GetState();
547 TrimAxes[current_axis]->SetControl(xhi);
548 TrimAxes[current_axis]->Run();
549 ahi=TrimAxes[current_axis]->GetState();
550 if(fabs(ahi-alo) <= TrimAxes[current_axis]->GetTolerance()) continue;
551 if(alo*ahi <=0) { //found interval with root
553 if(alo*current_accel <= 0) { //narrow interval down a bit
557 //xhi=current_control;
563 //xlo=current_control;
567 lastxlo=xlo;lastxhi=xhi;
568 lastalo=alo;lastahi=ahi;
569 if( !found && xlo==xmin && xhi==xmax ) continue;
571 cout << "FGTrim::findInterval: Nsub=" << Nsub << " Lo= " << xlo
572 << " Hi= " << xhi << " alo*ahi: " << alo*ahi << endl;
573 } while(!found && (Nsub <= max_sub_iterations) );
577 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
578 //checks to see which side of the current control value the solution is on
579 //and sets solutionDomain accordingly:
580 // 1 if solution is between the current and max
581 // -1 if solution is between the min and current
582 // 0 if there is no solution
584 //if changing the control produces no significant change in the accel then
585 //solutionDomain is set to zero and the function returns false
586 //if a solution is found, then xlo and xhi are set so that they bracket
587 //the solution, alo is set to accel(xlo), and ahi is set to accel(xhi)
588 //if there is no change or no solution then xlo=xmin, alo=accel(xmin) and
589 //xhi=xmax and ahi=accel(xmax)
590 //in all cases the sim is left such that the control=xmax and accel=ahi
592 bool FGTrim::checkLimits(void) {
594 float current_control=TrimAxes[current_axis]->GetControl();
595 float current_accel=TrimAxes[current_axis]->GetState();
596 xlo=TrimAxes[current_axis]->GetControlMin();
597 xhi=TrimAxes[current_axis]->GetControlMax();
599 TrimAxes[current_axis]->SetControl(xlo);
600 TrimAxes[current_axis]->Run();
601 alo=TrimAxes[current_axis]->GetState();
602 TrimAxes[current_axis]->SetControl(xhi);
603 TrimAxes[current_axis]->Run();
604 ahi=TrimAxes[current_axis]->GetState();
606 cout << "checkLimits() xlo,xhi,alo,ahi: " << xlo << ", " << xhi << ", "
607 << alo << ", " << ahi << endl;
609 solutionExists=false;
610 if(fabs(ahi-alo) > TrimAxes[current_axis]->GetTolerance()) {
611 if(alo*current_accel <= 0) {
616 } else if(current_accel*ahi < 0){
623 TrimAxes[current_axis]->SetControl(current_control);
624 TrimAxes[current_axis]->Run();
625 return solutionExists;
628 //YOU WERE WARNED, BUT YOU DID IT ANYWAY.