From 26f4b20af260f038457820dcc1cd580ddda96a14 Mon Sep 17 00:00:00 2001
From: Brandon Wampler <brandon.wampler@gmail.com>
Date: Wed, 8 Dec 2010 16:21:04 -0500
Subject: [PATCH] added components to FGStateSpace.h
---
configure.in | 4 +-
src/math/FGStateSpace.h | 152 +++++++++++++++++++++++++++++++++++++++-
2 files changed, 152 insertions(+), 4 deletions(-)
diff --git a/configure.in b/configure.in
index 9237b3a..cc37339 100644
--- a/configure.in
+++ b/configure.in
@@ -1,10 +1,10 @@
dnl Process this file with autoconf to produce a configure script.
-AC_INIT(JSBSim.cpp, 1.0.rc3-advtrim-5, jon@jsbsim.org)
+AC_INIT(JSBSim.cpp, 1.0.rc3-advtrim-6, jon@jsbsim.org)
dnl set the $host variable based on local machine/os
AC_CANONICAL_TARGET
-AM_INIT_AUTOMAKE(jsbsim, 1.0.rc3-advtrim-5)
+AM_INIT_AUTOMAKE(jsbsim, 1.0.rc3-advtrim-6)
AC_CONFIG_MACRO_DIR([m4])
# Before making a release, the LT_VERSION string should be modified.
diff --git a/src/math/FGStateSpace.h b/src/math/FGStateSpace.h
index ce57c45..2a5308e 100644
--- a/src/math/FGStateSpace.h
+++ b/src/math/FGStateSpace.h
@@ -641,7 +641,7 @@ public:
}
double getDeriv() const
{
- return m_fdm->GetPropagate()->GetVel(2)/m_fdm->GetPropagate()->GetRadius();
+ return m_fdm->GetPropagate()->GetVel(2)/(cos(m_fdm->GetPropagate()->GetLatitude())*m_fdm->GetPropagate()->GetRadius());
}
};
@@ -659,9 +659,157 @@ public:
}
double getDeriv() const
{
- return m_fdm->GetPropagate()->GetVel(1)/m_fdm->GetPropagate()->GetRadius();
+ return m_fdm->GetPropagate()->GetVel(1)/(m_fdm->GetPropagate()->GetRadius());
}
};
+
+ class Pi : public Component
+ {
+ public:
+ Pi() : Component("P inertial","rad/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetPQRi(1);
+ }
+ void set(double val)
+ {
+ //Set PQR from PQRi
+ //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+ m_fdm->GetIC()->SetQRadpsIC(val + \
+ (m_fdm->GetPropagate()->GetPQR(1) - m_fdm->GetPropagate()->GetPQRi(1)));
+ }
+ double getDeriv() const
+ {
+ return m_fdm->GetPropagate()->GetPQRdot(1);
+ }
+ };
+
+ class Qi : public Component
+ {
+ public:
+ Qi() : Component("Q inertial","rad/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetPQRi(2);
+ }
+ void set(double val)
+ {
+ //Set PQR from PQRi
+ //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+ m_fdm->GetIC()->SetQRadpsIC(val + \
+ (m_fdm->GetPropagate()->GetPQR(2) - m_fdm->GetPropagate()->GetPQRi(2)));
+ }
+ double getDeriv() const
+ {
+ return m_fdm->GetPropagate()->GetPQRdot(2);
+ }
+ };
+
+ class Ri : public Component
+ {
+ public:
+ Ri() : Component("R inertial","rad/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetPQRi(3);
+ }
+ void set(double val)
+ {
+ //Set PQR from PQRi
+ //VState.vPQR = VState.vPQRi - Ti2b * vOmegaEarth;
+ m_fdm->GetIC()->SetQRadpsIC(val + \
+ (m_fdm->GetPropagate()->GetPQR(3) - m_fdm->GetPropagate()->GetPQRi(3)));
+ }
+ double getDeriv() const
+ {
+ return m_fdm->GetPropagate()->GetPQRdot(3);
+ }
+ };
+
+ class Vn : public Component
+ {
+ public:
+ Vn() : Component("Vel north","feet/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetVel(1);
+ }
+ void set(double val)
+ {
+ m_fdm->GetIC()->SetVNorthFpsIC(val);
+ }
+ double getDeriv() const
+ {
+ //get NED accel from body accel
+ return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetPropagate()->GetUVWdot())(1);
+ }
+ };
+
+ class Ve : public Component
+ {
+ public:
+ Ve() : Component("Vel east","feet/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetVel(2);
+ }
+ void set(double val)
+ {
+ m_fdm->GetIC()->SetVEastFpsIC(val);
+ }
+ double getDeriv() const
+ {
+ //get NED accel from body accel
+ return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetPropagate()->GetUVWdot())(2);
+ }
+ };
+
+ class Vd : public Component
+ {
+ public:
+ Vd() : Component("Vel down","feet/s") {};
+ double get() const
+ {
+ return m_fdm->GetPropagate()->GetVel(3);
+ }
+ void set(double val)
+ {
+ m_fdm->GetIC()->SetVDownFpsIC(val);
+ }
+ double getDeriv() const
+ {
+ //get NED accel from body accel
+ return (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetPropagate()->GetUVWdot())(3);
+ }
+ };
+
+ class COG : public Component
+ {
+ public:
+ COG() : Component("Course Over Ground","rad") {};
+ double get() const
+ {
+ //cog = atan2(Ve,Vn)
+ return atan2(m_fdm->GetPropagate()->GetVel(2),m_fdm->GetPropagate()->GetVel(1));
+ }
+ void set(double val)
+ {
+ //set Vn and Ve according to vGround and COG
+ m_fdm->GetIC()->SetVNorthFpsIC(m_fdm->GetAuxiliary()->GetVground()*cos(val));
+ m_fdm->GetIC()->SetVEastFpsIC(m_fdm->GetAuxiliary()->GetVground()*sin(val));
+ }
+ double getDeriv() const
+ {
+ double Vn = m_fdm->GetPropagate()->GetVel(1);
+ double Vndot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetPropagate()->GetUVWdot())(1);
+ double Ve = m_fdm->GetPropagate()->GetVel(2);
+ double Vedot = (m_fdm->GetPropagate()->GetTb2l()*m_fdm->GetPropagate()->GetUVWdot())(2);
+
+ //dCOG/dt = dCOG/dVe*dVe/dt + dCOG/dVn*dVn/dt
+ return Vn/(Vn*Vn+Ve*Ve)*Vedot - Ve/(Vn*Vn+Ve*Ve)*Vndot;
+ }
+ };
+
};
// stream output
--
GitLab