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