36# pragma warning (disable : 4786)
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;
77 tolerance=DEFAULT_TOLERANCE;
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;
98 control_min=-30*degtorad;
99 control_max=30*degtorad;
100 control_convert=radtodeg;
105 if(control_max <= control_min) {
106 control_max=20*degtorad;
107 control_min=-5*degtorad;
109 control_value= (control_min+control_max)/2;
110 control_convert=radtodeg;
111 solver_eps=tolerance/100;
121 state_convert=radtodeg;
122 solver_eps=tolerance/100;
128 solver_eps=tolerance/100;
133 state_convert=radtodeg;
138 state_convert=radtodeg;
139 control_convert=radtodeg;
142 solver_eps=tolerance/100;
143 control_min=-80*degtorad;
144 control_max=80*degtorad;
145 control_convert=radtodeg;
150 state_convert=radtodeg;
167void FGTrimAxis::getState(
void) {
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;
185void FGTrimAxis::getControl(
void) {
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;
193 case tAileron: control_value=fdmex->
GetFCS() -> GetDaCmd();
break;
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;
206double FGTrimAxis::computeHmgt(
void) {
213 return (diff + 2*M_PI);
214 }
else if( diff > M_PI ) {
215 return (diff - 2*M_PI);
225void FGTrimAxis::setControl(
void) {
227 case tThrottle: setThrottlesPct();
break;
230 case tPitchTrim: fdmex->
GetFCS()->SetPitchTrimCmd(control_value);
break;
231 case tElevator: fdmex->
GetFCS()->SetDeCmd(control_value);
break;
233 case tAileron: fdmex->
GetFCS()->SetDaCmd(control_value);
break;
235 case tRudder: fdmex->
GetFCS()->SetDrCmd(control_value);
break;
238 case tPhi: fgic->
SetPhiRadIC(control_value);
break;
240 case tHeading: fgic->
SetPsiRadIC(control_value);
break;
248 double last_state_value;
256 last_state_value=state_value;
261 if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
266 its_to_stable_value=i;
267 total_stability_iterations+=its_to_stable_value;
273void FGTrimAxis::setThrottlesPct(
void) {
275 for(
unsigned i=0;i<fdmex->
GetPropulsion()->GetNumEngines();i++) {
281 fdmex->
GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
282 fdmex->
GetPropulsion()->in.ThrottlePos[i] = tMin +control_value*(tMax - tMin);
292void FGTrimAxis::AxisReport(
void) {
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();
303 if( fabs(GetState()+state_target) < fabs(GetTolerance()) )
304 cout <<
" Passed" << endl;
306 cout <<
" Failed" << endl;
308 cout.flags(originalFormat);
309 cout.precision(originalPrecision);
310 cout.width(originalWidth);
315double FGTrimAxis::GetAvgStability(
void ) {
316 if(total_iterations > 0) {
317 return double(total_stability_iterations)/double(total_iterations);
341void FGTrimAxis::Debug(
int from)
344 if (debug_lvl <= 0)
return;
345 if (debug_lvl & 1 ) {
350 if (debug_lvl & 2 ) {
351 if (from == 0) cout <<
"Instantiated: FGTrimAxis" << endl;
352 if (from == 1) cout <<
"Destroyed: FGTrimAxis" << endl;
354 if (debug_lvl & 4 ) {
356 if (debug_lvl & 8 ) {
358 if (debug_lvl & 16) {
360 if (debug_lvl & 64) {
Encapsulates the JSBSim simulation executive.
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(FGFDMExec *fdmex, FGInitialCondition *IC, State state, Control control)
Constructor for Trim Axis class.