fly/flightgear
1
0
Fork 0
Code Activity

Latest JSBSim updates.

Browse source
This commit is contained in:
david 2002-08-05 20:13:34 +00:00
parent f71e09be69
commit 844a55c3d1
20 changed files with 1151 additions and 979 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/FDM/JSBSim/FGAtmosphere.cpp
View file

@ -152,10 +152,10 @@ bool FGAtmosphere::Run(void)
Debug(2);
return false;
} else { // skip Run() execution this time
return true;
}
return false;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

7
src/FDM/JSBSim/FGFCS.cpp
View file

@ -120,10 +120,11 @@ bool FGFCS::Run(void)
for (i=0; i<PropAdvance.size(); i++) PropAdvance[i] = PropAdvanceCmd[i];
for (i=0; i<Components.size(); i++) Components[i]->Run();
if (DoNormalize) Normalize();
} else {
}
return false;
return false;
} else {
return true;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

10
src/FDM/JSBSim/FGFDMExec.cpp
View file

@ -115,8 +115,6 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root)
modelLoaded = false;
IsSlave = false;
cout << "FGFDMExec::FGFDMExec, FDMctr: " << FDMctr << endl;
IdFDM = FDMctr;
FDMctr++;
@ -132,6 +130,7 @@ FGFDMExec::FGFDMExec(FGPropertyManager* root)
else master = root;
instance = master->GetNode("/fdm/jsbsim",IdFDM,true);
instance->SetDouble("zero",0);
Debug(0);
@ -157,8 +156,7 @@ FGFDMExec::~FGFDMExec()
for (unsigned int i=1; i<SlaveFDMList.size(); i++) delete SlaveFDMList[i]->exec;
SlaveFDMList.clear();
cout << "FGFDMExec::~FGFDMExec, FDMctr: " << FDMctr << endl;
FDMctr--;
Debug(1);
}
@ -339,9 +337,9 @@ bool FGFDMExec::Run(void)
// Run(i)
}
while (!model_iterator->Run()) {
while (model_iterator != 0L) {
model_iterator->Run();
model_iterator = model_iterator->NextModel;
if (model_iterator == 0L) break;
}
frame = Frame++;

18
src/FDM/JSBSim/FGGroundReactions.cpp
View file

@ -134,6 +134,8 @@ string FGGroundReactions::GetGroundReactionStrings(void)
GroundReactionStrings += (lGear[i].GetName() + "_strokeVel, ");
GroundReactionStrings += (lGear[i].GetName() + "_CompressForce, ");
GroundReactionStrings += (lGear[i].GetName() + "_WhlSideForce, ");
GroundReactionStrings += (lGear[i].GetName() + "_WhlVelVecX, ");
GroundReactionStrings += (lGear[i].GetName() + "_WhlVelVecY, ");
GroundReactionStrings += (lGear[i].GetName() + "_WhlRollForce, ");
GroundReactionStrings += (lGear[i].GetName() + "_BodyXForce, ");
GroundReactionStrings += (lGear[i].GetName() + "_BodyYForce, ");
@ -142,6 +144,13 @@ string FGGroundReactions::GetGroundReactionStrings(void)
firstime = false;
}
GroundReactionStrings += ", TotalGearForce_X, ";
GroundReactionStrings += "TotalGearForce_Y, ";
GroundReactionStrings += "TotalGearForce_Z, ";
GroundReactionStrings += "TotalGearMoment_L, ";
GroundReactionStrings += "TotalGearMoment_M, ";
GroundReactionStrings += "TotalGearMoment_N";
return GroundReactionStrings;
}
@ -160,6 +169,8 @@ string FGGroundReactions::GetGroundReactionValues(void)
GroundReactionValues += (string(gcvt(lGear[i].GetCompLen(), 5, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetCompVel(), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetCompForce(), 10, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetWheelVel(eX), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetWheelVel(eY), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetWheelSideForce(), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetWheelRollForce(), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i].GetBodyXForce(), 6, buff)) + ", ");
@ -169,6 +180,13 @@ string FGGroundReactions::GetGroundReactionValues(void)
firstime = false;
}
GroundReactionValues += (", " + string(gcvt(vForces(eX), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(vForces(eY), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(vForces(eZ), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(vMoments(eX), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(vMoments(eY), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(vMoments(eZ), 6, buff)));
return GroundReactionValues;
}

27
src/FDM/JSBSim/FGInertial.cpp
View file

@ -59,6 +59,9 @@ FGInertial::FGInertial(FGFDMExec* fgex) : FGModel(fgex)
RadiusReference = 20925650.00;
gAccelReference = GM/(RadiusReference*RadiusReference);
gAccel = GM/(RadiusReference*RadiusReference);
vCoriolis.InitMatrix();
vCentrifugal.InitMatrix();
vGravity.InitMatrix();
bind();
@ -77,20 +80,11 @@ FGInertial::~FGInertial(void)
bool FGInertial::Run(void)
{
double stht, ctht, sphi, cphi;
if (!FGModel::Run()) {
gAccel = GM / (Position->GetRadius()*Position->GetRadius());
stht = sin(Rotation->GetEuler(eTht));
ctht = cos(Rotation->GetEuler(eTht));
sphi = sin(Rotation->GetEuler(ePhi));
cphi = cos(Rotation->GetEuler(ePhi));
vGravity(eX) = vForces(eX) = -gravity()*stht;
vGravity(eY) = vForces(eY) = gravity()*sphi*ctht;
vGravity(eZ) = vForces(eZ) = gravity()*cphi*ctht;
vGravity(eDown) = gAccel;
// The following equation for vOmegaLocal terms shows the angular velocity
// calculation _for_the_local_frame_ given the earth's rotation (first set)
@ -105,12 +99,17 @@ bool FGInertial::Run(void)
vOmegaLocal(eY) += -Position->GetVn() / Position->GetRadius();
vOmegaLocal(eZ) += 0.00;
// vForces = State->GetTl2b()*(-2.0*vOmegaLocal * Position->GetVel());
// Coriolis acceleration is normally written: -2w*dr/dt, but due to the axis
// conventions used here the sign is reversed: 2w*dr/dt. The same is true for
// Centrifugal acceleration.
vRadius(3) = Position->GetRadius();
vForces += State->GetTl2b()*(vOmegaLocal * (vOmegaLocal * vRadius));
vCoriolis(eEast) = 2.0*omega() * (Position->GetVd()*cos(Position->GetLatitude()) +
Position->GetVn()*sin(Position->GetLatitude()));
vForces *= MassBalance->GetMass(); // IMPORTANT !!!!!!!!!!!!!!!!!!!!!!!!!!!!
vRadius(eDown) = Position->GetRadius();
vCentrifugal(eDown) = -vOmegaLocal.Magnitude() * vOmegaLocal.Magnitude() * vRadius(eDown);
vForces = State->GetTl2b() * MassBalance->GetMass() * (vCoriolis + vCentrifugal + vGravity);
return false;
} else {

4
src/FDM/JSBSim/FGInertial.h
View file

@ -75,6 +75,8 @@ public:
bool Run(void);
FGColumnVector3& GetForces(void) {return vForces;}
FGColumnVector3& GetGravity(void) {return vGravity;}
FGColumnVector3& GetCoriolis(void) {return vCoriolis;}
FGColumnVector3& GetCentrifugal(void) {return vCentrifugal;}
double GetForces(int n) const {return vForces(n);}
bool LoadInertial(FGConfigFile* AC_cfg);
double SLgravity(void) const {return gAccelReference;}
@ -91,6 +93,8 @@ private:
FGColumnVector3 vForces;
FGColumnVector3 vRadius;
FGColumnVector3 vGravity;
FGColumnVector3 vCoriolis;
FGColumnVector3 vCentrifugal;
double gAccel;
double gAccelReference;
double RadiusReference;

1
src/FDM/JSBSim/FGInitialCondition.cpp
View file

@ -110,7 +110,6 @@ FGInitialCondition::~FGInitialCondition()
void FGInitialCondition::SetVcalibratedKtsIC(double tt) {
if(getMachFromVcas(&mach,tt*ktstofps)) {
//cout << "Mach: " << mach << endl;
lastSpeedSet=setvc;
vc=tt*ktstofps;

39
src/FDM/JSBSim/FGLGear.cpp
View file

@ -104,7 +104,7 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
FCS = Exec->GetFCS();
MassBalance = Exec->GetMassBalance();
WOW = lastWOW = false;
WOW = lastWOW = true; // should the value be initialized to true?
ReportEnable = true;
FirstContact = false;
Reported = false;
@ -123,6 +123,13 @@ FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : Exec(fdmex)
brakePct = 0.0;
maxCompLen = 0.0;
WheelSlip = lastWheelSlip = 0.0;
compressLength = 0.0;
compressSpeed = 0.0;
brakePct = 0.0;
maxCompLen = 0.0;
Debug(0);
}
@ -173,6 +180,8 @@ FGLGear::FGLGear(const FGLGear& lgear)
isRetractable = lgear.isRetractable;
GearUp = lgear.GearUp;
GearDown = lgear.GearDown;
WheelSlip = lgear.WheelSlip;
lastWheelSlip = lgear.lastWheelSlip;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -266,17 +275,17 @@ FGColumnVector3& FGLGear::Force(void)
switch (eBrakeGrp) {
case bgLeft:
SteerGain = -0.10;
SteerGain = 0.10;
BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgLeft)) +
staticFCoeff*FCS->GetBrake(bgLeft);
break;
case bgRight:
SteerGain = -0.10;
SteerGain = 0.10;
BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgRight)) +
staticFCoeff*FCS->GetBrake(bgRight);
break;
case bgCenter:
SteerGain = -0.10;
SteerGain = 0.10;
BrakeFCoeff = rollingFCoeff*(1.0 - FCS->GetBrake(bgCenter)) +
staticFCoeff*FCS->GetBrake(bgCenter);
break;
@ -289,7 +298,7 @@ FGColumnVector3& FGLGear::Force(void)
BrakeFCoeff = rollingFCoeff;
break;
case bgNone:
SteerGain = -0.10;
SteerGain = 0.0;
BrakeFCoeff = rollingFCoeff;
break;
default:
@ -326,17 +335,31 @@ FGColumnVector3& FGLGear::Force(void)
if (RollingWhlVel == 0.0 && SideWhlVel == 0.0) {
WheelSlip = 0.0;
} else if (fabs(RollingWhlVel) < 0.10) {
WheelSlip = 0.05*radtodeg*atan2(SideWhlVel, RollingWhlVel) + 0.95*WheelSlip;
} else {
WheelSlip = radtodeg*atan2(SideWhlVel, RollingWhlVel);
}
if ((WheelSlip < 0.0 && lastWheelSlip > 0.0) ||
(WheelSlip > 0.0 && lastWheelSlip < 0.0))
{
WheelSlip = 0.0;
}
lastWheelSlip = WheelSlip;
// Compute the sideforce coefficients using similar assumptions to LaRCSim for now.
// Allow a maximum of 10 degrees tire slip angle before wheel slides. At that point,
// transition from static to dynamic friction. There are more complicated formulations
// of this that avoid the discrete jump. Will fix this later.
if (fabs(WheelSlip) <= 10.0) {
FCoeff = staticFCoeff*WheelSlip/10.0;
if (fabs(WheelSlip) <= 20.0) {
FCoeff = staticFCoeff*WheelSlip/20.0;
} else if (fabs(WheelSlip) <= 40.0) {
// FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
FCoeff = (dynamicFCoeff*(fabs(WheelSlip) - 20.0)/20.0 +
staticFCoeff*(40.0 - fabs(WheelSlip))/20.0)*fabs(WheelSlip)/WheelSlip;
} else {
FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip;
}
@ -406,7 +429,7 @@ FGColumnVector3& FGLGear::Force(void)
}
if (lastWOW != WOW) {
PutMessage("GEAR_CONTACT", WOW);
PutMessage("GEAR_CONTACT: " + name, WOW);
}
lastWOW = WOW;

2
src/FDM/JSBSim/FGLGear.h
View file

@ -244,6 +244,7 @@ public:
inline double GetBodyXForce(void) { return vLocalForce(eX); }
inline double GetBodyYForce(void) { return vLocalForce(eY); }
inline double GetWheelSlipAngle(void) { return WheelSlip; }
double GetWheelVel(int axis) { return vWhlVelVec(axis);}
private:
FGColumnVector3 vXYZ;
@ -270,6 +271,7 @@ private:
double SideWhlVel, RollingWhlVel;
double RollingForce, SideForce, FCoeff;
double WheelSlip;
double lastWheelSlip;
bool WOW;
bool lastWOW;
bool FirstContact;

13
src/FDM/JSBSim/FGOutput.cpp
View file

@ -98,8 +98,9 @@ bool FGOutput::Run(void)
} else {
// Not a valid type of output
}
return false;
} else {
return true;
}
}
return false;
@ -172,7 +173,10 @@ void FGOutput::DelimitedOutput(string fname)
outstream << ", ";
outstream << "Drag, Side, Lift, ";
outstream << "L/D, ";
outstream << "Xforce, Yforce, Zforce";
outstream << "Xforce, Yforce, Zforce, ";
outstream << "xGravity, yGravity, zGravity, ";
outstream << "xCoriolis, yCoriolis, zCoriolis, ";
outstream << "xCentrifugal, yCentrifugal, zCentrifugal";
}
if (SubSystems & ssMoments) {
outstream << ", ";
@ -252,7 +256,10 @@ void FGOutput::DelimitedOutput(string fname)
outstream << ", ";
outstream << Aerodynamics->GetvFs() << ", ";
outstream << Aerodynamics->GetLoD() << ", ";
outstream << Aircraft->GetForces();
outstream << Aircraft->GetForces() << ", ";
outstream << Inertial->GetGravity() << ", ";
outstream << Inertial->GetCoriolis() << ", ";
outstream << Inertial->GetCentrifugal();
}
if (SubSystems & ssMoments) {
outstream << ", ";

34
src/FDM/JSBSim/FGPosition.cpp
View file

@ -101,7 +101,13 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGPosition";
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0;
for (int i=0;i<3;i++) {
LatitudeDot_prev[i] = 0.0;
LongitudeDot_prev[i] = 0.0;
RadiusDot_prev[i] = 0.0;
}
Longitude = Latitude = 0.0;
gamma = Vt = Vground = 0.0;
hoverbmac = hoverbcg = 0.0;
@ -143,7 +149,8 @@ Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
In FGFS, SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
*/
bool FGPosition::Run(void) {
bool FGPosition::Run(void)
{
double cosLat;
double hdot_Vt;
FGColumnVector3 vMac;
@ -160,22 +167,20 @@ bool FGPosition::Run(void) {
cosLat = cos(Latitude);
if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
LatitudeDot = vVel(eNorth) / Radius;
RadiusDot = -vVel(eDown);
Longitude += 0.5*dt*rate*(LongitudeDot + lastLongitudeDot);
Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
Longitude += State->Integrate(FGState::TRAPZ, dt*rate, LongitudeDot, LongitudeDot_prev);
Latitude += State->Integrate(FGState::TRAPZ, dt*rate, LatitudeDot, LatitudeDot_prev);
Radius += State->Integrate(FGState::TRAPZ, dt*rate, RadiusDot, RadiusDot_prev);
h = Radius - SeaLevelRadius; // Geocentric
DistanceAGL = Radius - RunwayRadius; // Geocentric
hoverbcg = DistanceAGL/b;
vMac=State->GetTb2l()*Aircraft->GetXYZrp();
vMac = State->GetTb2l()*Aircraft->GetXYZrp();
vMac *= inchtoft;
hoverbmac = (DistanceAGL + vMac(3))/b;
@ -187,10 +192,6 @@ bool FGPosition::Run(void) {
gamma = 0.0;
}
lastLatitudeDot = LatitudeDot;
lastLongitudeDot = LongitudeDot;
lastRadiusDot = RadiusDot;
return false;
} else {
@ -200,7 +201,8 @@ bool FGPosition::Run(void) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::GetState(void) {
void FGPosition::GetState(void)
{
dt = State->Getdt();
Vt = Translation->GetVt();
@ -212,7 +214,8 @@ void FGPosition::GetState(void) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::Seth(double tt) {
void FGPosition::Seth(double tt)
{
h = tt;
Radius = h + SeaLevelRadius;
DistanceAGL = Radius - RunwayRadius; // Geocentric
@ -221,7 +224,8 @@ void FGPosition::Seth(double tt) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPosition::SetDistanceAGL(double tt) {
void FGPosition::SetDistanceAGL(double tt)
{
DistanceAGL=tt;
Radius = RunwayRadius + DistanceAGL;
h = Radius - SeaLevelRadius;

3
src/FDM/JSBSim/FGPosition.h
View file

@ -79,6 +79,7 @@ public:
/** Constructor
@param Executive a pointer to the parent executive object */
FGPosition(FGFDMExec*);
/// Destructor
~FGPosition();
@ -132,7 +133,7 @@ private:
double Radius, h;
double LatitudeDot, LongitudeDot, RadiusDot;
double lastLatitudeDot, lastLongitudeDot, lastRadiusDot;
double LatitudeDot_prev[3], LongitudeDot_prev[3], RadiusDot_prev[3];
double Longitude, Latitude;
double dt;
double RunwayRadius;

58
src/FDM/JSBSim/FGPropulsion.cpp
View file

@ -82,6 +82,7 @@ FGPropulsion::FGPropulsion(FGFDMExec* exec) : FGModel(exec)
numTanks = numEngines = numThrusters = 0;
numOxiTanks = numFuelTanks = 0;
dt = 0.0;
ActiveEngine = -1; // -1: ALL, 0: Engine 1, 1: Engine 2 ...
bind();
Debug(0);
}
@ -560,13 +561,67 @@ double FGPropulsion::GetTanksIxy(const FGColumnVector3& vXYZcg)
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropulsion::SetMagnetos(int setting)
{
if (ActiveEngine == -1) {
for (unsigned i=0; i<Engines.size(); i++) {
Engines[i]->SetMagnetos(setting);
}
} else {
Engines[ActiveEngine]->SetMagnetos(setting);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropulsion::SetStarter(int setting)
{
if (ActiveEngine == -1) {
for (unsigned i=0; i<Engines.size(); i++) {
Engines[i]->SetStarter(setting);
}
} else {
if (setting == 0)
Engines[ActiveEngine]->SetStarter(false);
else
Engines[ActiveEngine]->SetStarter(true);
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropulsion::SetActiveEngine(int engine)
{
if ( unsigned(engine) > Engines.size())
ActiveEngine = -1;
else
ActiveEngine = engine;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGPropulsion::bind(void)
{
typedef double (FGPropulsion::*PMF)(int) const;
typedef int (FGPropulsion::*iPMF)(void) const;
/* PropertyManager->Tie("propulsion/num-engines", this,
&FGPropulsion::GetNumEngines);
PropertyManager->Tie("propulsion/num-tanks", this,
&FGPropulsion::GetNumTanks); */
PropertyManager->Tie("propulsion/magneto_cmd", this,
(iPMF)0,
&FGPropulsion::SetMagnetos,
true);
PropertyManager->Tie("propulsion/starter_cmd", this,
(iPMF)0,
&FGPropulsion::SetStarter,
true);
PropertyManager->Tie("propulsion/active_engine", this,
(iPMF)0,
&FGPropulsion::SetActiveEngine,
true);
PropertyManager->Tie("propulsion/num-sel-fuel-tanks", this,
&FGPropulsion::GetnumSelectedFuelTanks);
PropertyManager->Tie("propulsion/num-sel-ox-tanks", this,
@ -595,6 +650,9 @@ void FGPropulsion::unbind(void)
PropertyManager->Untie("propulsion/num-tanks"); */
PropertyManager->Untie("propulsion/num-sel-fuel-tanks");
PropertyManager->Untie("propulsion/num-sel-ox-tanks");
PropertyManager->Untie("propulsion/magneto_cmd");
PropertyManager->Untie("propulsion/starter_cmd");
PropertyManager->Untie("propulsion/active_engine");
PropertyManager->Untie("forces/fbx-prop-lbs");
PropertyManager->Untie("forces/fby-prop-lbs");
PropertyManager->Untie("forces/fbz-prop-lbs");

5
src/FDM/JSBSim/FGPropulsion.h
View file

@ -192,6 +192,10 @@ public:
double GetTanksIxz(const FGColumnVector3& vXYZcg);
double GetTanksIxy(const FGColumnVector3& vXYZcg);
void SetMagnetos(int setting);
void SetStarter(int setting);
void SetActiveEngine(int engine);
void bind();
void unbind();
@ -207,6 +211,7 @@ private:
unsigned int numEngines;
unsigned int numTanks;
unsigned int numThrusters;
int ActiveEngine;
double dt;
FGColumnVector3 vForces;
FGColumnVector3 vMoments;

17
src/FDM/JSBSim/FGRotation.cpp
View file

@ -80,8 +80,13 @@ CLASS IMPLEMENTATION
FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGRotation";
cTht=cPhi=cPsi=1.0;
sTht=sPhi=sPsi=0.0;
cTht = cPhi = cPsi = 1.0;
sTht = sPhi = sPsi = 0.0;
vPQRdot.InitMatrix();
vPQRdot_prev[0].InitMatrix();
vPQRdot_prev[1].InitMatrix();
vPQRdot_prev[2].InitMatrix();
bind();
@ -110,10 +115,12 @@ bool FGRotation::Run(void)
N1 = vMoments(eN) - (Iyy-Ixx)*vPQR(eP)*vPQR(eQ) - Ixz*vPQR(eR)*vPQR(eQ);
vPQRdot(eP) = (L2*Izz - N1*Ixz) / (Ixx*Izz - Ixz*Ixz);
vPQRdot(eQ) = (vMoments(eM) - (Ixx-Izz)*vPQR(eP)*vPQR(eR) - Ixz*(vPQR(eP)*vPQR(eP) - vPQR(eR)*vPQR(eR)))/Iyy;
vPQRdot(eQ) = (vMoments(eM) - (Ixx-Izz)*vPQR(eP)*vPQR(eR)
- Ixz*(vPQR(eP)*vPQR(eP) - vPQR(eR)*vPQR(eR)))/Iyy;
vPQRdot(eR) = (N1*Ixx + L2*Ixz) / (Ixx*Izz - Ixz*Ixz);
vPQR += dt*rate*(vlastPQRdot + vPQRdot)/2.0;
vPQR += State->Integrate(FGState::TRAPZ, dt*rate, vPQRdot, vPQRdot_prev);
vAeroPQR = vPQR + Atmosphere->GetTurbPQR();
State->IntegrateQuat(vPQR, rate);
@ -131,8 +138,6 @@ bool FGRotation::Run(void)
vEulerRates(ePsi) = (vPQR(2)*sPhi + vPQR(3)*cPhi)/cTht;
}
vlastPQRdot = vPQRdot;
if (debug_lvl > 1) Debug(2);
return false;

3
src/FDM/JSBSim/FGRotation.h
View file

@ -121,15 +121,14 @@ public:
void bind(void);
void unbind(void);
private:
FGColumnVector3 vPQR;
FGColumnVector3 vAeroPQR;
FGColumnVector3 vPQRdot;
FGColumnVector3 vPQRdot_prev[3];
FGColumnVector3 vMoments;
FGColumnVector3 vEuler;
FGColumnVector3 vEulerRates;
FGColumnVector3 vlastPQRdot;
double cTht,sTht;
double cPhi,sPhi;

30
src/FDM/JSBSim/FGState.cpp
View file

@ -64,13 +64,6 @@ MACROS
CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//
// For every term registered here there must be a corresponding handler in
// GetParameter() below that retrieves that parameter. Also, there must be an
// entry in the enum eParam definition in FGJSBBase.h. The ID is what must be used
// in any config file entry which references that item.
FGState::FGState(FGFDMExec* fdex)
{
FDMExec = fdex;
@ -78,7 +71,6 @@ FGState::FGState(FGFDMExec* fdex)
a = 1000.0;
sim_time = 0.0;
dt = 1.0/120.0;
ActiveEngine = -1;
Aircraft = FDMExec->GetAircraft();
Translation = FDMExec->GetTranslation();
@ -92,6 +84,8 @@ FGState::FGState(FGFDMExec* fdex)
Propulsion = FDMExec->GetPropulsion();
PropertyManager = FDMExec->GetPropertyManager();
for(int i=0;i<3;i++) vQdot_prev[i].InitMatrix();
InitPropertyMaps();
bind();
@ -159,7 +153,6 @@ bool FGState::Reset(string path, string acname, string fname)
resetfile >> token;
}
Position->SetLatitude(latitude*degtorad);
Position->SetLongitude(longitude*degtorad);
Position->Seth(h);
@ -336,11 +329,10 @@ void FGState::IntegrateQuat(FGColumnVector3 vPQR, int rate)
vQdot(2) = 0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ));
vQdot(3) = 0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR));
vQdot(4) = 0.5*(vQtrn(1)*vPQR(eR) + vQtrn(2)*vPQR(eQ) - vQtrn(3)*vPQR(eP));
vQtrn += 0.5*dt*rate*(vlastQdot + vQdot);
vQtrn += Integrate(TRAPZ, dt*rate, vQdot, vQdot_prev);
vQtrn.Normalize();
vlastQdot = vQdot;
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
@ -553,17 +545,17 @@ void FGState::InitPropertyMaps(void)
ParamNameToProp[ "FG_THROTTLE_POS" ]="fcs/throttle-pos-norm";
ParamNameToProp[ "FG_MIXTURE_CMD" ]="fcs/mixture-cmd-norm";
ParamNameToProp[ "FG_MIXTURE_POS" ]="fcs/mixture-pos-norm";
ParamNameToProp[ "FG_MAGNETO_CMD" ]="zero";
ParamNameToProp[ "FG_STARTER_CMD" ]="zero";
ParamNameToProp[ "FG_ACTIVE_ENGINE" ]="zero";
ParamNameToProp[ "FG_MAGNETO_CMD" ]="propulsion/magneto_cmd";
ParamNameToProp[ "FG_STARTER_CMD" ]="propulsion/starter_cmd";
ParamNameToProp[ "FG_ACTIVE_ENGINE" ]="propulsion/active_engine";
ParamNameToProp[ "FG_HOVERB" ]="aero/h_b-mac-ft";
ParamNameToProp[ "FG_PITCH_TRIM_CMD" ]="fcs/pitch-trim-cmd-norm";
ParamNameToProp[ "FG_YAW_TRIM_CMD" ]="fcs/yaw-trim-cmd-norm";
ParamNameToProp[ "FG_ROLL_TRIM_CMD" ]="fcs/roll-trim-cmd-norm";
ParamNameToProp[ "FG_LEFT_BRAKE_CMD" ]="zero";
ParamNameToProp[ "FG_CENTER_BRAKE_CMD" ]="zero";
ParamNameToProp[ "FG_RIGHT_BRAKE_CMD" ]="zero";
ParamNameToProp[ "FG_SET_LOGGING" ]="zero";
ParamNameToProp[ "FG_LEFT_BRAKE_CMD" ]="fcs/left_brake";
ParamNameToProp[ "FG_CENTER_BRAKE_CMD" ]="fcs/center_brake";
ParamNameToProp[ "FG_RIGHT_BRAKE_CMD" ]="fcs/right_brake";
ParamNameToProp[ "FG_SET_LOGGING" ]="sim/set_logging";
ParamNameToProp[ "FG_ALPHAH" ]="aero/alpha-rad";
ParamNameToProp[ "FG_ALPHAW" ]="aero/alpha-wing-rad";
ParamNameToProp[ "FG_LBARH" ]="metrics/lh-norm";

88
src/FDM/JSBSim/FGState.h
View file

@ -230,6 +230,77 @@ public:
*/
void IntegrateQuat(FGColumnVector3 vPQR, int rate);
// ======================================= General Purpose INTEGRATOR
enum iType {AB4, AB3, AB2, AM3, EULER, TRAPZ};
/** Multi-method integrator.
@param type Type of intergation scheme to use. Can be one of:
<ul>
<li>AB4 - Adams-Bashforth, fourth order</li>
<li>AB3 - Adams-Bashforth, third order</li>
<li>AB2 - Adams-Bashforth, second order</li>
<li>AM3 - Adams Moulton, third order</li>
<li>EULER - Euler</li>
<li>TRAPZ - Trapezoidal</li>
</ul>
@param delta_t the integration time step in seconds
@param vTDeriv a reference to the current value of the time derivative of
the quantity being integrated (i.e. if vUVW is being integrated
vTDeriv is the current value of vUVWdot)
@param vLastArray an array of previously calculated and saved values of
the quantity being integrated (i.e. if vUVW is being integrated
vLastArray[0] is the past value of vUVWdot, vLastArray[1] is the value of
vUVWdot prior to that, etc.)
@return the current, incremental value of the item integrated to add to the
previous value. */
template <class T> T Integrate(iType type, double delta_t, T& vTDeriv, T *vLastArray)
{
T vResult;
switch (type) {
case AB4:
vResult = (delta_t/24.0)*( 55.0 * vTDeriv
- 59.0 * vLastArray[0]
+ 37.0 * vLastArray[1]
- 9.0 * vLastArray[2] );
vLastArray[2] = vLastArray[1];
vLastArray[1] = vLastArray[0];
vLastArray[0] = vTDeriv;
break;
case AB3:
vResult = (delta_t/12.0)*( 23.0 * vTDeriv
- 16.0 * vLastArray[0]
+ 5.0 * vLastArray[1] );
vLastArray[1] = vLastArray[0];
vLastArray[0] = vTDeriv;
break;
case AB2:
vResult = (delta_t/2.0)*( 3.0 * vTDeriv - vLastArray[0] );
vLastArray[0] = vTDeriv;
break;
case AM3:
vResult = (delta_t/12.0)*( 5.0 * vTDeriv
+ 8.0 * vLastArray[0]
- 1.0 * vLastArray[1] );
vLastArray[1] = vLastArray[0];
vLastArray[0] = vTDeriv;
break;
case EULER:
vResult = delta_t * vTDeriv;
break;
case TRAPZ:
vResult = (delta_t*0.5) * (vTDeriv + vLastArray[0]);
vLastArray[0] = vTDeriv;
break;
}
return vResult;
}
// =======================================
/** Calculates Euler angles from the local-to-body matrix.
@return a reference to the vEuler column vector.
*/
@ -275,8 +346,6 @@ public:
void ReportState(void);
inline string GetPropertyName(string prm) { return ParamNameToProp[prm]; }
//inline string GetPropertyName(eParam prm) { return ParamIdxToProp[prm]; }
//inline eParam GetParam(string property) { return PropToParam[property]; }
void bind();
void unbind();
@ -292,7 +361,7 @@ private:
FGMatrix33 mTs2b;
FGMatrix33 mTb2s;
FGColumnVector4 vQtrn;
FGColumnVector4 vlastQdot;
FGColumnVector4 vQdot_prev[3];
FGColumnVector4 vQdot;
FGColumnVector3 vUVW;
FGColumnVector3 vLocalVelNED;
@ -313,22 +382,9 @@ private:
FGPropulsion* Propulsion;
FGPropertyManager* PropertyManager;
/* typedef map<string, eParam> CoeffMap;
CoeffMap coeffdef;
typedef map<eParam, string> ParamMap;
//ParamMap paramdef; */
typedef map<string,string> ParamNameMap;
ParamNameMap ParamNameToProp;
typedef map<eParam,string> ParamIdxMap;
ParamIdxMap ParamIdxToProp;
//CoeffMap PropToParam;
int ActiveEngine;
void InitPropertyMaps(void);
void Debug(int from);

12
src/FDM/JSBSim/FGTranslation.cpp
View file

@ -88,6 +88,12 @@ FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex)
Mach = 0.0;
alpha = beta = 0.0;
adot = bdot = 0.0;
vUVWdot.InitMatrix();
vUVWdot_prev[0].InitMatrix();
vUVWdot_prev[1].InitMatrix();
vUVWdot_prev[2].InitMatrix();
bind();
Debug(0);
}
@ -104,8 +110,6 @@ FGTranslation::~FGTranslation(void)
bool FGTranslation::Run(void)
{
double Tc = 0.5*State->Getdt()*rate;
if (!FGModel::Run()) {
mVel(1,1) = 0.0;
@ -120,7 +124,7 @@ bool FGTranslation::Run(void)
vUVWdot = mVel*Rotation->GetPQR() + Aircraft->GetBodyAccel();
vUVW += Tc*(vUVWdot + vlastUVWdot);
vUVW += State->Integrate(FGState::TRAPZ, State->Getdt()*rate, vUVWdot, vUVWdot_prev);
vAeroUVW = vUVW + State->GetTl2b()*Atmosphere->GetWindNED();
@ -155,8 +159,6 @@ bool FGTranslation::Run(void)
qbarUV = 0.5*Atmosphere->GetDensity()*(vAeroUVW(eU)*vAeroUVW(eU) + vAeroUVW(eV)*vAeroUVW(eV));
Mach = Vt / State->Geta();
vlastUVWdot = vUVWdot;
if (debug_lvl > 1) Debug(1);
return false;

5
src/FDM/JSBSim/FGTranslation.h
View file

@ -128,8 +128,8 @@ public:
private:
FGColumnVector3 vUVW;
FGColumnVector3 vUVWdot;
FGColumnVector3 vlastUVWdot;
FGMatrix33 mVel;
FGColumnVector3 vUVWdot_prev[3];
FGMatrix33 mVel;
FGColumnVector3 vAeroUVW;
double Vt, Mach;
@ -137,7 +137,6 @@ private:
double dt;
double alpha, beta;
double adot,bdot;
void Debug(int from);
};