41 # pragma warning (disable : 4786) 47 #include "FGFDMExec.h" 48 #include "models/FGAtmosphere.h" 49 #include "initialization/FGInitialCondition.h" 50 #include "FGTrimAnalysisControl.h" 51 #include "models/FGAircraft.h" 52 #include "models/FGPropulsion.h" 53 #include "models/FGAerodynamics.h" 57 IDENT(IdSrc,
"$Id: FGTrimAnalysisControl.cpp,v 1.7 2014/01/13 10:46:00 ehofman Exp $");
58 IDENT(IdHdr,ID_TRIMANALYSISCONTROL);
69 control_initial_value = 0.;
75 control_tolerance = DEFAULT_TOLERANCE;
82 control_initial_value = 0.5;
83 control_value=control_initial_value;
84 control_name =
"Throttle (cmd,norm)";
87 control_min=-30*degtorad;
88 control_max=30*degtorad;
89 control_step=1*degtorad;
90 control_convert=radtodeg;
95 if(control_max <= control_min) {
96 control_max=20*degtorad;
97 control_min=-5*degtorad;
99 control_step=1*degtorad;
100 control_initial_value = (control_min+control_max)/2;
101 control_value= control_initial_value;
102 control_convert=radtodeg;
105 control_name =
"Pitch Trim (cmd,norm)";
109 state_convert=radtodeg;
112 control_name =
"Elevator (cmd,norm)";
116 state_convert=radtodeg;
119 control_name =
"Roll Trim (cmd,norm)";
123 state_convert=radtodeg;
126 control_name =
"Ailerons (cmd,norm)";
130 state_convert=radtodeg;
133 control_name =
"Yaw Trim (cmd,norm)";
137 state_convert=radtodeg;
140 control_name =
"Rudder (cmd,norm)";
144 state_convert=radtodeg;
147 control_name =
"Altitude (ft)";
151 control_initial_value = fdmex->
GetPropagate()->GetDistanceAGL();
152 control_value = control_initial_value;
155 control_name =
"Phi (rad)";
158 control_step=1*degtorad;
159 state_convert=radtodeg;
160 control_convert=radtodeg;
163 control_name =
"Theta (rad)";
166 control_step=1*degtorad;
167 state_convert=radtodeg;
170 control_name =
"Heading (rad)";
173 control_step=1*degtorad;
174 state_convert=radtodeg;
177 control_name =
"Gamma (rad)";
178 control_min=-80*degtorad;
179 control_max=80*degtorad;
180 control_step=1*degtorad;
181 control_convert=radtodeg;
200 void FGTrimAnalysisControl::getState(
void) {
202 case taUdot: state_value=fdmex->
GetPropagate()->GetUVWdot(1)-state_target;
break;
203 case taVdot: state_value=fdmex->
GetPropagate()->GetUVWdot(2)-state_target;
break;
204 case taWdot: state_value=fdmex->
GetPropagate()->GetUVWdot(3)-state_target;
break;
205 case taPdot: state_value=fdmex->
GetPropagate()->GetPQRdot(1)-state_target;
break;
206 case taQdot: state_value=fdmex->
GetPropagate()->GetPQRdot(2)-state_target;
break;
207 case taRdot: state_value=fdmex->
GetPropagate()->GetPQRdot(3)-state_target;
break;
208 case taHmgt: state_value=computeHmgt()-state_target;
break;
209 case taNlf: state_value=fdmex->
GetAircraft()->GetNlf()-state_target;
break;
218 void FGTrimAnalysisControl::getControl(
void) {
221 case taBeta: control_value=fdmex->
GetAuxiliary()->Getbeta();
break;
222 case taAlpha: control_value=fdmex->
GetAuxiliary()->Getalpha();
break;
224 case taElevator: control_value=fdmex->
GetFCS()->
GetDeCmd();
break;
229 case taAltAGL: control_value=fdmex->
GetPropagate()->GetDistanceAGL();
break;
232 case taGamma: control_value=fdmex->
GetAuxiliary()->GetGamma();
break;
239 double FGTrimAnalysisControl::computeHmgt(
void) {
246 return (diff + 2*M_PI);
247 }
else if( diff > M_PI ) {
248 return (diff - 2*M_PI);
258 void FGTrimAnalysisControl::setControl(
void) {
260 case taThrottle: setThrottlesPct();
break;
271 case taPhi: fgic->
SetPhiRadIC(control_value);
break;
273 case taHeading: fgic->
SetPsiRadIC(control_value);
break;
295 i=0; ref=-1; center=-1;
296 while( (ref < 0) && (i < fdmex->GetGroundReactions()->GetNumGearUnits()) ) {
305 if((ref < 0) && (center >= 0)) {
308 cout <<
"SetThetaOnGround ref gear: " << ref << endl;
315 double hagl = -1*lx*sin(ff) +
320 cout <<
"SetThetaOnGround new alt: " << hagl << endl;
323 cout <<
"SetThetaOnGround new theta: " << ff << endl;
332 double zAft,zForward,zDiff,theta;
333 double xAft,xForward,xDiff;
345 for( i=0; i<N; i++ ) {
352 for( i=0; i<N; i++ ) {
362 xDiff = xForward - xAft;
365 zDiff = zForward - zAft;
368 while(!level && (i < 100)) {
369 theta+=radtodeg*atan(zDiff/xDiff);
377 zDiff = zForward - zAft;
381 if(fabs(zDiff ) < 0.1)
388 cout <<
" Used gear unit " << iAft <<
" as aft and " << iForward <<
" as forward" << endl;
390 control_min=(theta+5)*degtorad;
391 control_max=(theta-5)*degtorad;
406 while ( (ref < 0) && (i < fdmex->GetGroundReactions()->GetNumGearUnits()) ) {
418 double hagl = -1*lx*st +
437 void FGTrimAnalysisControl::setThrottlesPct(
void) {
475 void FGTrimAnalysisControl::Debug(
int from)
478 if (debug_lvl <= 0)
return;
479 if (debug_lvl & 1 ) {
484 if (debug_lvl & 2 ) {
485 if (from == 0) cout <<
"Instantiated: FGTrimAnalysisControl" << endl;
486 if (from == 1) cout <<
"Destroyed: FGTrimAnalysisControl" << endl;
488 if (debug_lvl & 4 ) {
490 if (debug_lvl & 8 ) {
492 if (debug_lvl & 16) {
494 if (debug_lvl & 64) {
496 cout << IdSrc << endl;
497 cout << IdHdr << endl;
void SetThetaDegIC(double theta)
Sets pitch angle initial condition in degrees.
void SetThetaRadIC(double theta)
Sets the initial pitch angle.
double GetThetaDegIC(void) const
Gets the initial pitch angle.
FGColumnVector3 GetBodyLocation(void) const
Gets the location of the gear in Body axes.
double GetCosEuler(int idx) const
Retrieves the cosine of a vehicle Euler angle component.
void Run(void)
This function iterates through a call to the FGFDMExec::RunIC() function until the desired trimming c...
double GetDeCmd(void) const
Gets the elevator command.
double GetAltitudeAGLFtIC(void) const
Gets the initial altitude above ground level.
void SuspendIntegration(void)
Suspends the simulation and sets the delta T to zero.
void SetPhiRadIC(double phi)
Sets the initial roll angle.
FGAuxiliary * GetAuxiliary(void)
Returns the FGAuxiliary pointer.
FGEngine * GetEngine(unsigned int index) const
Retrieves an engine object pointer from the list of engines.
double GetSinEuler(int idx) const
Retrieves the sine of a vehicle Euler angle component.
void SetThrottleCmd(int engine, double cmd)
Sets the throttle command for the specified engine.
void SetDaCmd(double cmd)
Sets the aileron command.
void SetPhiOnGround(double ff)
Set phi value on ground for trim.
void SetFlightPathAngleRadIC(double gamma)
Sets the initial flight path angle.
void SetPitchTrimCmd(double cmd)
Sets the pitch trim command.
void Initialize(FGInitialCondition *FGIC)
Initializes the simulation with initial conditions.
double GetPitchTrimCmd(void) const
Gets the pitch trim command.
double GetThrottleCmd(int engine) const
Gets the throttle command.
bool initTheta(void)
Calculate steady state thetas value on ground.
FGAircraft * GetAircraft(void)
Returns the FGAircraft pointer.
FGGroundReactions * GetGroundReactions(void)
Returns the FGGroundReactions pointer.
void SetThetaOnGround(double ff)
Set theta value on ground for trim.
~FGTrimAnalysisControl()
Destructor.
void SetDrCmd(double cmd)
Sets the rudder command.
const FGColumnVector3 & GetEuler(void) const
Retrieves the Euler angles that define the vehicle orientation.
void SetDeCmd(double cmd)
Sets the elevator command.
void SetAltitudeAGLFtIC(double agl)
Sets the initial Altitude above ground level.
void ResumeIntegration(void)
Resumes the simulation by resetting delta T to the correct value.
double GetDaCmd(void) const
Gets the aileron command.
Initializes the simulation run.
FGFCS * GetFCS(void)
Returns the FGFCS pointer.
void SetAlphaRadIC(double alpha)
Sets the initial angle of attack.
bool GetSteadyState(void)
Loops the engines until thrust output steady (used for trimming)
FGTrimAnalysisControl(FGFDMExec *fdmex, FGInitialCondition *IC, TaControl control)
Constructor for Trim Analysis Control class.
FGLGear * GetGearUnit(int gear) const
Gets a gear instance.
FGAerodynamics * GetAerodynamics(void)
Returns the FGAerodynamics pointer.
FGPropulsion * GetPropulsion(void)
Returns the FGPropulsion pointer.
bool Run(void)
This function executes each scheduled model in succession.
Encapsulates the JSBSim simulation executive.
void SetBetaRadIC(double beta)
Sets the initial sideslip angle.
double GetDrCmd(void) const
Gets the rudder command.
void SetPsiRadIC(double psi)
Sets the initial heading angle.
bool GetWOW(void) const
Gets the Weight On Wheels flag value.
FGPropagate * GetPropagate(void)
Returns the FGPropagate pointer.
unsigned int GetNumEngines(void) const
Retrieves the number of engines defined for the aircraft.