19 #ifndef JSBSim_FGStateSpace_H 20 #define JSBSim_FGStateSpace_H 22 #include "FGFDMExec.h" 23 #include "models/FGPropulsion.h" 24 #include "models/FGAccelerations.h" 25 #include "models/propulsion/FGEngine.h" 26 #include "models/propulsion/FGThruster.h" 27 #include "models/propulsion/FGTurbine.h" 28 #include "models/propulsion/FGTurboProp.h" 29 #include "models/FGAuxiliary.h" 30 #include "models/FGFCS.h" 48 std::string m_name, m_unit;
50 Component(
const std::string & name,
const std::string & unit) :
51 m_stateSpace(), m_fdm(), m_name(name), m_unit(unit) {};
53 virtual double get()
const = 0;
54 virtual void set(
double val) = 0;
55 virtual double getDeriv()
const 58 std::vector<double> x0 = m_stateSpace->x.get();
62 m_fdm->
Setdt(1./120.);
66 m_stateSpace->x.set(x0);
69 std::cout << std::scientific
74 <<
"\tdf/dt: " << (f1-f0)/m_fdm->
GetDeltaT()
75 << std::fixed << std::endl;
77 double deriv = (f1-f0)/m_fdm->
GetDeltaT();
85 m_stateSpace = stateSpace;
91 const std::string & getName()
const 95 const std::string & getUnit()
const 106 m_stateSpace(stateSpace), m_fdm(fdm), m_components() {}
109 m_stateSpace = componentVector.m_stateSpace;
110 m_fdm = componentVector.m_fdm;
111 m_components = componentVector.m_components;
115 m_stateSpace(componentVector.m_stateSpace),
116 m_fdm(componentVector.m_fdm),
117 m_components(componentVector.m_components)
122 comp->setStateSpace(m_stateSpace);
124 m_components.push_back(comp);
126 size_t getSize()
const 128 return m_components.size();
132 return m_components[i];
136 return m_components[i];
138 double get(
int i)
const 140 return m_components[i]->get();
142 void set(
int i,
double val)
144 m_components[i]->set(val);
149 return m_components[i]->get();
151 std::vector<double>
get()
const 153 std::vector<double> val;
154 for (
unsigned int i=0;i<getSize();i++) val.push_back(m_components[i]->get());
157 void get(
double * array)
const 159 for (
unsigned int i=0;i<getSize();i++) array[i] = m_components[i]->
get();
161 double getDeriv(
int i)
163 return m_components[i]->getDeriv();
165 std::vector<double> getDeriv()
const 167 std::vector<double> val;
168 for (
unsigned int i=0;i<getSize();i++) val.push_back(m_components[i]->getDeriv());
171 void getDeriv(
double * array)
const 173 for (
unsigned int i=0;i<getSize();i++) array[i] = m_components[i]->getDeriv();
175 void set(std::vector<double> vals)
177 for (
unsigned int i=0;i<getSize();i++) m_components[i]->
set(vals[i]);
180 void set(
double * array)
182 for (
unsigned int i=0;i<getSize();i++) m_components[i]->
set(array[i]);
185 std::string getName(
int i)
const 187 return m_components[i]->getName();
189 std::vector<std::string> getName()
const 191 std::vector<std::string> name;
192 for (
unsigned int i=0;i<getSize();i++) name.push_back(m_components[i]->getName());
195 std::string getUnit(
int i)
const 197 return m_components[i]->getUnit();
199 std::vector<std::string> getUnit()
const 201 std::vector<std::string> unit;
202 for (
unsigned int i=0;i<getSize();i++) unit.push_back(m_components[i]->getUnit());
206 m_components.clear();
211 std::vector<Component *> m_components;
220 void setFdm(
FGFDMExec * fdm) { m_fdm = fdm; }
225 for (
unsigned int i=0; i<m_fdm->GetPropulsion()->GetNumEngines(); i++) {
226 m_fdm->GetPropulsion()->GetEngine(i)->InitRunning();
230 double cost = stateSum();
231 for(
int i=0;i<1000;i++) {
232 m_fdm->GetPropulsion()->GetSteadyState();
233 m_fdm->SetTrimStatus(
true);
234 m_fdm->DisableOutput();
235 m_fdm->SuspendIntegration();
237 m_fdm->SetTrimStatus(
false);
238 m_fdm->EnableOutput();
239 m_fdm->ResumeIntegration();
241 double costNew = stateSum();
242 double dcost = fabs(costNew - cost);
243 if (dcost < std::numeric_limits<double>::epsilon()) {
244 if(m_fdm->GetDebugLevel() > 1) {
245 std::cout <<
"cost convergd, i: " << i << std::endl;
250 if(m_fdm->GetDebugLevel() > 1) {
251 std::cout <<
"cost failed to converge, dcost: " 253 << dcost << std::endl;
263 for (
unsigned int i=0;i<x.getSize();i++) sum += x.get(i);
277 void linearize(std::vector<double> x0, std::vector<double> u0, std::vector<double> y0,
278 std::vector< std::vector<double> > & A,
279 std::vector< std::vector<double> > & B,
280 std::vector< std::vector<double> > & C,
281 std::vector< std::vector<double> > & D);
287 void numericalJacobian(std::vector< std::vector<double> > & J,
ComponentVector & y,
289 const std::vector<double> & x0,
double h=1e-5,
bool computeYDerivative =
false);
310 double getDeriv()
const 395 double getDeriv()
const 414 double getDeriv()
const 432 double getDeriv()
const 450 double getDeriv()
const 470 double getDeriv()
const 488 double getDeriv()
const 506 double getDeriv()
const 524 double getDeriv()
const 542 double getDeriv()
const 750 double getDeriv()
const 768 double getDeriv()
const 789 double getDeriv()
const 810 double getDeriv()
const 831 double getDeriv()
const 849 double getDeriv()
const 868 double getDeriv()
const 887 double getDeriv()
const 909 double getDeriv()
const 917 return Vn/(Vn*Vn+Ve*Ve)*Vedot - Ve/(Vn*Vn+Ve*Ve)*Vndot;
926 std::ostream &operator<<(std::ostream &out,
const FGStateSpace &ss );
927 std::ostream &operator<<( std::ostream &out, const std::vector< std::vector<double> > &vec2d );
928 std::ostream &operator<<( std::ostream &out, const std::vector<double> &vec );
void SetDePos(int form, double pos)
Sets the elevator position.
FGAccelerations * GetAccelerations(void)
Returns the FGAccelerations pointer.
void SetThetaRadIC(double theta)
Sets the initial pitch angle.
void SetVEastFpsIC(double ve)
Sets the initial local axis east velocity.
FGInitialCondition * GetIC(void)
Returns a pointer to the FGInitialCondition object.
void SetVtrueFpsIC(double vt)
Sets the initial true airspeed.
void SetLatitudeRadIC(double lat)
Sets the initial latitude.
double GetDeCmd(void) const
Gets the elevator command.
void SetVNorthFpsIC(double vn)
Sets the initial local axis north velocity.
double GetBetaDegIC(void) const
Gets the initial sideslip angle.
double GetDePos(int form=ofRad) const
Gets the elevator position.
void SetDrPos(int form, double pos)
Sets the rudder position.
double GetDaLPos(int form=ofRad) const
Gets the left aileron position.
void SetPhiRadIC(double phi)
Sets the initial roll angle.
const FGColumnVector3 & GetPQRi(void) const
Retrieves the body angular rates vector, relative to the ECI (inertial) frame.
FGAuxiliary * GetAuxiliary(void)
Returns the FGAuxiliary pointer.
FGEngine * GetEngine(unsigned int index) const
Retrieves an engine object pointer from the list of engines.
void SetQRadpsIC(double Q)
Sets the initial body axis pitch rate.
double GetThrottlePos(int engine) const
Gets the throttle position.
void DisableOutput(void)
Disables data logging to all outputs.
double GetAltitudeASL(void) const
Returns the current altitude above sea level.
const FGColumnVector3 & GetUVWdot(void) const
Retrieves the body axis acceleration.
void SetThrottleCmd(int engine, double cmd)
Sets the throttle command for the specified engine.
void SetDaCmd(double cmd)
Sets the aileron command.
void EnableOutput(void)
Enables data logging to all outputs.
bool Run(bool Holding)
Runs the Flight Controls model; called by the Executive Can pass in a value indicating if the executi...
void SetDaRPos(int form, double pos)
Sets the right aileron position.
void SetFlightPathAngleRadIC(double gamma)
Sets the initial flight path angle.
const FGColumnVector3 & GetUVW(void) const
Retrieves the body frame vehicle velocity vector.
const FGColumnVector3 & GetPQR(void) const
Retrieves the body angular rates vector, relative to the ECEF frame.
void Initialize(FGInitialCondition *FGIC)
Initializes the simulation with initial conditions.
void SetDaLPos(int form, double pos)
Sets the left aileron position.
double Setsim_time(double cur_time)
Sets the current sim time.
void Setdt(double delta_t)
Sets the integration time step for the simulation executive.
double GetThrottleCmd(int engine) const
Gets the throttle command.
int GetDebugLevel(void) const
Retrieves the current debug level setting.
void SetRRadpsIC(double R)
Sets the initial body axis yaw rate.
void SetVgroundFpsIC(double vg)
Sets the initial ground speed.
double GetPsiRadIC(void) const
Gets the initial heading angle.
const FGMatrix33 & GetTb2l(void) const
Retrieves the body-to-local transformation matrix.
const FGColumnVector3 & GetVel(void) const
Retrieves the velocity vector.
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 SetVDownFpsIC(double vd)
Sets the initial local axis down velocity.
double GetDaCmd(void) const
Gets the aileron command.
FGFCS * GetFCS(void)
Returns the FGFCS pointer.
double GetSimTime(void) const
Returns the cumulative simulation time in seconds.
void SetAlphaRadIC(double alpha)
Sets the initial angle of attack.
double Gethdot(void) const
Returns the current altitude rate.
double GetAlphaRadIC(void) const
Gets the initial angle of attack.
void SetThrottlePos(int engine, double cmd)
Sets the actual throttle setting for the specified engine.
double GetDeltaT(void) const
Returns the simulation delta T.
const FGColumnVector3 & GetPQRdot(void) const
Retrieves the body axis angular acceleration vector.
FGPropulsion * GetPropulsion(void)
Returns the FGPropulsion pointer.
bool Run(void)
This function executes each scheduled model in succession.
void SetPRadpsIC(double P)
Sets the initial body axis roll rate.
void SetLongitudeRadIC(double lon)
Sets the initial longitude.
double GetThetaRadIC(void) const
Gets the initial pitch angle.
Encapsulates the JSBSim simulation executive.
void SetBetaRadIC(double beta)
Sets the initial sideslip angle.
double GetDrCmd(void) const
Gets the rudder command.
double GetVground(void) const
Gets the ground speed in feet per second.
void SetAltitudeASLFtIC(double altitudeASL)
Sets the altitude above sea level initial condition in feet.
void SetPsiRadIC(double psi)
Sets the initial heading angle.
double GetVt(void) const
Gets the magnitude of total vehicle velocity including wind effects in feet per second.
FGPropagate * GetPropagate(void)
Returns the FGPropagate pointer.
double GetDrPos(int form=ofRad) const
Gets the rudder position.
unsigned int GetNumEngines(void) const
Retrieves the number of engines defined for the aircraft.