36 # pragma warning (disable : 4786)
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"
70 its_to_stable_value=0;
72 total_stability_iterations=0;
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;
97 control_min=-30*degtorad;
98 control_max=30*degtorad;
99 control_convert=radtodeg;
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;
108 control_value= (control_min+control_max)/2;
109 control_convert=radtodeg;
110 solver_eps=tolerance/100;
120 state_convert=radtodeg;
121 solver_eps=tolerance/100;
127 solver_eps=tolerance/100;
132 state_convert=radtodeg;
137 state_convert=radtodeg;
138 control_convert=radtodeg;
141 solver_eps=tolerance/100;
142 control_min=-80*degtorad;
143 control_max=80*degtorad;
144 control_convert=radtodeg;
149 state_convert=radtodeg;
159 FGTrimAxis::~FGTrimAxis(
void)
166 void FGTrimAxis::getState(
void) {
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;
184 void FGTrimAxis::getControl(
void) {
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;
192 case tAileron: control_value=fdmex->GetFCS() -> GetDaCmd();
break;
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;
205 double FGTrimAxis::computeHmgt(
void) {
208 diff = fdmex->GetPropagate()->GetEuler(ePsi) -
209 fdmex->GetAuxiliary()->GetGroundTrack();
212 return (diff + 2*M_PI);
213 }
else if( diff > M_PI ) {
214 return (diff - 2*M_PI);
224 void FGTrimAxis::setControl(
void) {
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;
232 case tAileron: fdmex->GetFCS()->SetDaCmd(control_value);
break;
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;
245 void FGTrimAxis::Run(
void) {
247 double last_state_value;
255 last_state_value=state_value;
256 fdmex->Initialize(fgic);
260 if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
265 its_to_stable_value=i;
266 total_stability_iterations+=its_to_stable_value;
272 void FGTrimAxis::setThrottlesPct(
void) {
274 for(
unsigned i=0;i<fdmex->GetPropulsion()->GetNumEngines();i++) {
275 tMin=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMin();
276 tMax=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMax();
280 fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
281 fdmex->GetPropulsion()->in.ThrottlePos[i] = tMin +control_value*(tMax - tMin);
283 fdmex->Initialize(fgic);
285 fdmex->GetPropulsion()->GetSteadyState();
291 void FGTrimAxis::AxisReport(
void) {
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();
302 if( fabs(GetState()+state_target) < fabs(GetTolerance()) )
303 cout <<
" Passed" << endl;
305 cout <<
" Failed" << endl;
307 cout.flags(originalFormat);
308 cout.precision(originalPrecision);
309 cout.width(originalWidth);
314 double FGTrimAxis::GetAvgStability(
void ) {
315 if(total_iterations > 0) {
316 return double(total_stability_iterations)/double(total_iterations);
340 void FGTrimAxis::Debug(
int from)
343 if (debug_lvl <= 0)
return;
344 if (debug_lvl & 1 ) {
349 if (debug_lvl & 2 ) {
350 if (from == 0) cout <<
"Instantiated: FGTrimAxis" << endl;
351 if (from == 1) cout <<
"Destroyed: FGTrimAxis" << endl;
353 if (debug_lvl & 4 ) {
355 if (debug_lvl & 8 ) {
357 if (debug_lvl & 16) {
359 if (debug_lvl & 64) {
Encapsulates the JSBSim simulation executive.
Initializes the simulation run.
double GetPhiRadIC(void) const
Gets the initial roll angle.
double GetAltitudeAGLFtIC(void) const
Gets the initial altitude above ground level.
double GetPsiRadIC(void) const
Gets the initial heading angle.
double GetThetaRadIC(void) const
Gets the initial pitch angle.