JSBSim Flight Dynamics Model  1.0 (02 March 2017)
An Open Source Flight Dynamics and Control Software Library in C++
FGTrimAxis.cpp
1 /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 
3  Header: FGTrimAxis.cpp
4  Author: Tony Peden
5  Date started: 7/3/00
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 
27  HISTORY
28 --------------------------------------------------------------------------------
29 7/3/00 TP Created
30 
31 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
32 INCLUDES
33 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
34 
35 #ifdef _MSC_VER
36 # pragma warning (disable : 4786)
37 #endif
38 
39 #include <string>
40 #include <cstdlib>
41 #include <iomanip>
42 #include "FGFDMExec.h"
43 #include "models/FGAtmosphere.h"
44 #include "FGInitialCondition.h"
45 #include "FGTrimAxis.h"
46 #include "models/FGPropulsion.h"
47 #include "models/FGAerodynamics.h"
48 #include "models/FGFCS.h"
49 #include "models/propulsion/FGEngine.h"
50 #include "models/FGAuxiliary.h"
51 #include "models/FGGroundReactions.h"
52 #include "models/FGPropagate.h"
53 #include "models/FGAccelerations.h"
54 
55 using namespace std;
56 
57 namespace JSBSim {
58 
59 IDENT(IdSrc,"$Id: FGTrimAxis.cpp,v 1.20 2016/01/17 15:54:04 bcoconni Exp $");
60 IDENT(IdHdr,ID_TRIMAXIS);
61 
62 /*****************************************************************************/
63 
64 FGTrimAxis::FGTrimAxis(FGFDMExec* fdex, FGInitialCondition* ic, State st,
65  Control ctrl) {
66 
67  fdmex=fdex;
68  fgic=ic;
69  state=st;
70  control=ctrl;
71  max_iterations=10;
72  control_value=0;
73  its_to_stable_value=0;
74  total_iterations=0;
75  total_stability_iterations=0;
76  state_convert=1.0;
77  control_convert=1.0;
78  state_value=0;
79  state_target=0;
80  switch(state) {
81  case tUdot: tolerance = DEFAULT_TOLERANCE; break;
82  case tVdot: tolerance = DEFAULT_TOLERANCE; break;
83  case tWdot: tolerance = DEFAULT_TOLERANCE; break;
84  case tQdot: tolerance = DEFAULT_TOLERANCE / 10; break;
85  case tPdot: tolerance = DEFAULT_TOLERANCE / 10; break;
86  case tRdot: tolerance = DEFAULT_TOLERANCE / 10; break;
87  case tHmgt: tolerance = 0.01; break;
88  case tNlf: state_target=1.0; tolerance = 1E-5; break;
89  case tAll: break;
90  }
91 
92  solver_eps=tolerance;
93  switch(control) {
94  case tThrottle:
95  control_min=0;
96  control_max=1;
97  control_value=0.5;
98  break;
99  case tBeta:
100  control_min=-30*degtorad;
101  control_max=30*degtorad;
102  control_convert=radtodeg;
103  break;
104  case tAlpha:
105  control_min=fdmex->GetAerodynamics()->GetAlphaCLMin();
106  control_max=fdmex->GetAerodynamics()->GetAlphaCLMax();
107  if(control_max <= control_min) {
108  control_max=20*degtorad;
109  control_min=-5*degtorad;
110  }
111  control_value= (control_min+control_max)/2;
112  control_convert=radtodeg;
113  solver_eps=tolerance/100;
114  break;
115  case tPitchTrim:
116  case tElevator:
117  case tRollTrim:
118  case tAileron:
119  case tYawTrim:
120  case tRudder:
121  control_min=-1;
122  control_max=1;
123  state_convert=radtodeg;
124  solver_eps=tolerance/100;
125  break;
126  case tAltAGL:
127  control_min=0;
128  control_max=30;
129  control_value=ic->GetAltitudeAGLFtIC();
130  solver_eps=tolerance/100;
131  break;
132  case tTheta:
133  control_min=ic->GetThetaRadIC() - 5*degtorad;
134  control_max=ic->GetThetaRadIC() + 5*degtorad;
135  state_convert=radtodeg;
136  break;
137  case tPhi:
138  control_min=ic->GetPhiRadIC() - 30*degtorad;
139  control_max=ic->GetPhiRadIC() + 30*degtorad;
140  state_convert=radtodeg;
141  control_convert=radtodeg;
142  break;
143  case tGamma:
144  solver_eps=tolerance/100;
145  control_min=-80*degtorad;
146  control_max=80*degtorad;
147  control_convert=radtodeg;
148  break;
149  case tHeading:
150  control_min=ic->GetPsiRadIC() - 30*degtorad;
151  control_max=ic->GetPsiRadIC() + 30*degtorad;
152  state_convert=radtodeg;
153  break;
154  }
155 
156 
157  Debug(0);
158 }
159 
160 /*****************************************************************************/
161 
162 FGTrimAxis::~FGTrimAxis(void)
163 {
164  Debug(1);
165 }
166 
167 /*****************************************************************************/
168 
169 void FGTrimAxis::getState(void) {
170  switch(state) {
171  case tUdot: state_value=fdmex->GetAccelerations()->GetUVWdot(1)-state_target; break;
172  case tVdot: state_value=fdmex->GetAccelerations()->GetUVWdot(2)-state_target; break;
173  case tWdot: state_value=fdmex->GetAccelerations()->GetUVWdot(3)-state_target; break;
174  case tQdot: state_value=fdmex->GetAccelerations()->GetPQRdot(2)-state_target;break;
175  case tPdot: state_value=fdmex->GetAccelerations()->GetPQRdot(1)-state_target; break;
176  case tRdot: state_value=fdmex->GetAccelerations()->GetPQRdot(3)-state_target; break;
177  case tHmgt: state_value=computeHmgt()-state_target; break;
178  case tNlf: state_value=fdmex->GetAuxiliary()->GetNlf()-state_target; break;
179  case tAll: break;
180  }
181 }
182 
183 /*****************************************************************************/
184 
185 //States are not settable
186 
187 void FGTrimAxis::getControl(void) {
188  switch(control) {
189  case tThrottle: control_value=fdmex->GetFCS()->GetThrottleCmd(0); break;
190  case tBeta: control_value=fdmex->GetAuxiliary()->Getbeta(); break;
191  case tAlpha: control_value=fdmex->GetAuxiliary()->Getalpha(); break;
192  case tPitchTrim: control_value=fdmex->GetFCS() -> GetPitchTrimCmd(); break;
193  case tElevator: control_value=fdmex->GetFCS() -> GetDeCmd(); break;
194  case tRollTrim:
195  case tAileron: control_value=fdmex->GetFCS() -> GetDaCmd(); break;
196  case tYawTrim:
197  case tRudder: control_value=fdmex->GetFCS() -> GetDrCmd(); break;
198  case tAltAGL: control_value=fdmex->GetPropagate()->GetDistanceAGL();break;
199  case tTheta: control_value=fdmex->GetPropagate()->GetEuler(eTht); break;
200  case tPhi: control_value=fdmex->GetPropagate()->GetEuler(ePhi); break;
201  case tGamma: control_value=fdmex->GetAuxiliary()->GetGamma();break;
202  case tHeading: control_value=fdmex->GetPropagate()->GetEuler(ePsi); break;
203  }
204 }
205 
206 /*****************************************************************************/
207 
208 double FGTrimAxis::computeHmgt(void) {
209  double diff;
210 
211  diff = fdmex->GetPropagate()->GetEuler(ePsi) -
212  fdmex->GetAuxiliary()->GetGroundTrack();
213 
214  if( diff < -M_PI ) {
215  return (diff + 2*M_PI);
216  } else if( diff > M_PI ) {
217  return (diff - 2*M_PI);
218  } else {
219  return diff;
220  }
221 
222 }
223 
224 /*****************************************************************************/
225 
226 
227 void FGTrimAxis::setControl(void) {
228  switch(control) {
229  case tThrottle: setThrottlesPct(); break;
230  case tBeta: fgic->SetBetaRadIC(control_value); break;
231  case tAlpha: fgic->SetAlphaRadIC(control_value); break;
232  case tPitchTrim: fdmex->GetFCS()->SetPitchTrimCmd(control_value); break;
233  case tElevator: fdmex->GetFCS()->SetDeCmd(control_value); break;
234  case tRollTrim:
235  case tAileron: fdmex->GetFCS()->SetDaCmd(control_value); break;
236  case tYawTrim:
237  case tRudder: fdmex->GetFCS()->SetDrCmd(control_value); break;
238  case tAltAGL: fgic->SetAltitudeAGLFtIC(control_value); break;
239  case tTheta: fgic->SetThetaRadIC(control_value); break;
240  case tPhi: fgic->SetPhiRadIC(control_value); break;
241  case tGamma: fgic->SetFlightPathAngleRadIC(control_value); break;
242  case tHeading: fgic->SetPsiRadIC(control_value); break;
243  }
244 }
245 
246 /*****************************************************************************/
247 
248 void FGTrimAxis::Run(void) {
249 
250  double last_state_value;
251  int i;
252  setControl();
253  //cout << "FGTrimAxis::Run: " << control_value << endl;
254  i=0;
255  bool stable=false;
256  while(!stable) {
257  i++;
258  last_state_value=state_value;
259  fdmex->Initialize(fgic);
260  fdmex->Run();
261  getState();
262  if(i > 1) {
263  if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
264  stable=true;
265  }
266  }
267 
268  its_to_stable_value=i;
269  total_stability_iterations+=its_to_stable_value;
270  total_iterations++;
271 }
272 
273 /*****************************************************************************/
274 
275 void FGTrimAxis::setThrottlesPct(void) {
276  double tMin,tMax;
277  for(unsigned i=0;i<fdmex->GetPropulsion()->GetNumEngines();i++) {
278  tMin=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMin();
279  tMax=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMax();
280 
281  // Both the main throttle setting in FGFCS and the copy of the position
282  // in the Propulsion::Inputs structure need to be set at this time.
283  fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
284  fdmex->GetPropulsion()->in.ThrottlePos[i] = tMin +control_value*(tMax - tMin);
285 
286  fdmex->Initialize(fgic);
287  fdmex->Run(); //apply throttle change
288  fdmex->GetPropulsion()->GetSteadyState();
289  }
290 }
291 
292 /*****************************************************************************/
293 
294 void FGTrimAxis::AxisReport(void) {
295  // Save original cout format characteristics
296  std::ios_base::fmtflags originalFormat = cout.flags();
297  std::streamsize originalPrecision = cout.precision();
298  std::streamsize originalWidth = cout.width();
299  cout << " " << setw(20) << GetControlName() << ": ";
300  cout << setw(6) << setprecision(2) << GetControl()*control_convert << ' ';
301  cout << setw(5) << GetStateName() << ": ";
302  cout << setw(9) << setprecision(2) << scientific << GetState()+state_target;
303  cout << " Tolerance: " << setw(3) << setprecision(0) << scientific << GetTolerance();
304 
305  if( fabs(GetState()+state_target) < fabs(GetTolerance()) )
306  cout << " Passed" << endl;
307  else
308  cout << " Failed" << endl;
309  // Restore original cout format characteristics
310  cout.flags(originalFormat);
311  cout.precision(originalPrecision);
312  cout.width(originalWidth);
313 }
314 
315 /*****************************************************************************/
316 
317 double FGTrimAxis::GetAvgStability( void ) {
318  if(total_iterations > 0) {
319  return double(total_stability_iterations)/double(total_iterations);
320  }
321  return 0;
322 }
323 
324 /*****************************************************************************/
325 // The bitmasked value choices are as follows:
326 // unset: In this case (the default) JSBSim would only print
327 // out the normally expected messages, essentially echoing
328 // the config files as they are read. If the environment
329 // variable is not set, debug_lvl is set to 1 internally
330 // 0: This requests JSBSim not to output any messages
331 // whatsoever.
332 // 1: This value explicity requests the normal JSBSim
333 // startup messages
334 // 2: This value asks for a message to be printed out when
335 // a class is instantiated
336 // 4: When this value is set, a message is displayed when a
337 // FGModel object executes its Run() method
338 // 8: When this value is set, various runtime state variables
339 // are printed out periodically
340 // 16: When set various parameters are sanity checked and
341 // a message is printed out when they go out of bounds
342 
343 void FGTrimAxis::Debug(int from)
344 {
345 
346  if (debug_lvl <= 0) return;
347  if (debug_lvl & 1 ) { // Standard console startup message output
348  if (from == 0) { // Constructor
349 
350  }
351  }
352  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
353  if (from == 0) cout << "Instantiated: FGTrimAxis" << endl;
354  if (from == 1) cout << "Destroyed: FGTrimAxis" << endl;
355  }
356  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
357  }
358  if (debug_lvl & 8 ) { // Runtime state variables
359  }
360  if (debug_lvl & 16) { // Sanity checking
361  }
362  if (debug_lvl & 64) {
363  if (from == 0) { // Constructor
364  cout << IdSrc << endl;
365  cout << IdHdr << endl;
366  }
367  }
368 }
369 }
double GetAltitudeAGLFtIC(void) const
Gets the initial altitude above ground level.
double GetPhiRadIC(void) const
Gets the initial roll angle.
STL namespace.
double GetPsiRadIC(void) const
Gets the initial heading angle.
Initializes the simulation run.
double GetThetaRadIC(void) const
Gets the initial pitch angle.
Encapsulates the JSBSim simulation executive.
Definition: FGFDMExec.h:189