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