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" 59 IDENT(IdSrc,
"$Id: FGTrimAxis.cpp,v 1.20 2016/01/17 15:54:04 bcoconni Exp $");
60 IDENT(IdHdr,ID_TRIMAXIS);
73 its_to_stable_value=0;
75 total_stability_iterations=0;
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;
100 control_min=-30*degtorad;
101 control_max=30*degtorad;
102 control_convert=radtodeg;
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;
111 control_value= (control_min+control_max)/2;
112 control_convert=radtodeg;
113 solver_eps=tolerance/100;
123 state_convert=radtodeg;
124 solver_eps=tolerance/100;
130 solver_eps=tolerance/100;
135 state_convert=radtodeg;
140 state_convert=radtodeg;
141 control_convert=radtodeg;
144 solver_eps=tolerance/100;
145 control_min=-80*degtorad;
146 control_max=80*degtorad;
147 control_convert=radtodeg;
152 state_convert=radtodeg;
162 FGTrimAxis::~FGTrimAxis(
void)
169 void FGTrimAxis::getState(
void) {
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;
187 void FGTrimAxis::getControl(
void) {
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;
195 case tAileron: control_value=fdmex->GetFCS() -> GetDaCmd();
break;
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;
208 double FGTrimAxis::computeHmgt(
void) {
211 diff = fdmex->GetPropagate()->GetEuler(ePsi) -
212 fdmex->GetAuxiliary()->GetGroundTrack();
215 return (diff + 2*M_PI);
216 }
else if( diff > M_PI ) {
217 return (diff - 2*M_PI);
227 void FGTrimAxis::setControl(
void) {
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;
235 case tAileron: fdmex->GetFCS()->SetDaCmd(control_value);
break;
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;
248 void FGTrimAxis::Run(
void) {
250 double last_state_value;
258 last_state_value=state_value;
259 fdmex->Initialize(fgic);
263 if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
268 its_to_stable_value=i;
269 total_stability_iterations+=its_to_stable_value;
275 void FGTrimAxis::setThrottlesPct(
void) {
277 for(
unsigned i=0;i<fdmex->GetPropulsion()->GetNumEngines();i++) {
278 tMin=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMin();
279 tMax=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMax();
283 fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
284 fdmex->GetPropulsion()->in.ThrottlePos[i] = tMin +control_value*(tMax - tMin);
286 fdmex->Initialize(fgic);
288 fdmex->GetPropulsion()->GetSteadyState();
294 void FGTrimAxis::AxisReport(
void) {
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();
305 if( fabs(GetState()+state_target) < fabs(GetTolerance()) )
306 cout <<
" Passed" << endl;
308 cout <<
" Failed" << endl;
310 cout.flags(originalFormat);
311 cout.precision(originalPrecision);
312 cout.width(originalWidth);
317 double FGTrimAxis::GetAvgStability(
void ) {
318 if(total_iterations > 0) {
319 return double(total_stability_iterations)/double(total_iterations);
343 void FGTrimAxis::Debug(
int from)
346 if (debug_lvl <= 0)
return;
347 if (debug_lvl & 1 ) {
352 if (debug_lvl & 2 ) {
353 if (from == 0) cout <<
"Instantiated: FGTrimAxis" << endl;
354 if (from == 1) cout <<
"Destroyed: FGTrimAxis" << endl;
356 if (debug_lvl & 4 ) {
358 if (debug_lvl & 8 ) {
360 if (debug_lvl & 16) {
362 if (debug_lvl & 64) {
364 cout << IdSrc << endl;
365 cout << IdHdr << endl;
double GetAltitudeAGLFtIC(void) const
Gets the initial altitude above ground level.
double GetPhiRadIC(void) const
Gets the initial roll angle.
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.