fly/flightgear
1
0
Fork 0
Code Activity

5/26/2000 updated from Jon and Tony.

Browse source
This commit is contained in:
curt 2000-05-27 05:48:14 +00:00
parent b074559216
commit e12e81dd23
22 changed files with 348 additions and 207 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

51
src/FDM/JSBSim/FGAircraft.cpp
View file

@ -140,7 +140,8 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex),
vXYZcg(3),
vXYZep(3),
vEuler(3),
vFs(3) {
vFs(3)
{
Name = "FGAircraft";
AxisIdx["DRAG"] = 0;
@ -150,6 +151,8 @@ vFs(3) {
AxisIdx["PITCH"] = 4;
AxisIdx["YAW"] = 5;
GearUp = false;
numTanks = numEngines = numSelectedFuelTanks = numSelectedOxiTanks = 0;
}
@ -363,7 +366,8 @@ void FGAircraft::FMGear(void) {
}
else {
for (unsigned int i=0;i<lGear.size();i++) {
vForces += lGear[i]->Force();
vForces += lGear[i]->Force();
vMoments += lGear[i]->Moment();
}
}
}
@ -556,6 +560,10 @@ void FGAircraft::ReadOutput(FGConfigFile* AC_cfg) {
*AC_cfg >> parameter;
if (parameter == "ON") subsystems += ssCoefficients;
}
if (parameter == "GROUND_REACTIONS") {
*AC_cfg >> parameter;
if (parameter == "ON") subsystems += ssGroundReactions;
}
}
Output->SetSubsystems(subsystems);
@ -668,4 +676,43 @@ string FGAircraft::GetCoefficientValues(void) {
;
}
/******************************************************************************/
string FGAircraft::GetGroundReactionStrings(void) {
string GroundReactionStrings = "";
bool firstime = true;
for (unsigned int i=0;i<lGear.size();i++) {
if (!firstime) GroundReactionStrings += ", ";
GroundReactionStrings += (lGear[i]->GetName() + "_WOW, ");
GroundReactionStrings += (lGear[i]->GetName() + "_compressLength, ");
GroundReactionStrings += (lGear[i]->GetName() + "_compressSpeed, ");
GroundReactionStrings += (lGear[i]->GetName() + "_Force");
firstime = false;
}
return GroundReactionStrings;
}
/******************************************************************************/
string FGAircraft::GetGroundReactionValues(void) {
char buff[20];
string GroundReactionValues = "";
bool firstime = true;
for (unsigned int i=0;i<lGear.size();i++) {
if (!firstime) GroundReactionValues += ", ";
GroundReactionValues += string( lGear[i]->GetWOW()?"1":"0" ) + ", ";
GroundReactionValues += (string(gcvt(lGear[i]->GetCompLen(), 5, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i]->GetCompVel(), 6, buff)) + ", ");
GroundReactionValues += (string(gcvt(lGear[i]->GetCompForce(), 10, buff)));
firstime = false;
}
return GroundReactionValues;
}

33
src/FDM/JSBSim/FGAircraft.h
View file

@ -160,22 +160,27 @@ public:
inline float GetNlf(void) { return nlf; }
inline float GetAlphaCLMax(void) { return alphaclmax; }
inline float GetAlphaCLMin(void) { return alphaclmin; }
inline void SetAlphaCLMax(float tt) { alphaclmax=tt; }
inline void SetAlphaCLMin(float tt) { alphaclmin=tt; }
string GetCoefficientStrings(void);
string GetCoefficientValues(void);
string GetGroundReactionStrings(void);
string GetGroundReactionValues(void);
enum { ssSimulation = 1,
ssAerosurfaces = 2,
ssRates = 4,
ssVelocities = 8,
ssForces = 16,
ssMoments = 32,
ssAtmosphere = 64,
ssMassProps = 128,
ssCoefficients = 256,
ssPosition = 512 } subsystems;
enum { ssSimulation = 1,
ssAerosurfaces = 2,
ssRates = 4,
ssVelocities = 8,
ssForces = 16,
ssMoments = 32,
ssAtmosphere = 64,
ssMassProps = 128,
ssCoefficients = 256,
ssPosition = 512,
ssGroundReactions = 1024 } subsystems;
private:
void GetState(void);
@ -203,10 +208,10 @@ private:
string CFGVersion;
string AircraftName;
int numTanks;
int numEngines;
int numSelectedOxiTanks;
int numSelectedFuelTanks;
unsigned int numTanks;
unsigned int numEngines;
unsigned int numSelectedOxiTanks;
unsigned int numSelectedFuelTanks;
FGTank* Tank[MAX_TANKS];
FGEngine *Engine[MAX_ENGINES];

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

@ -108,8 +108,8 @@ bool FGAtmosphere::Run(void)
//and we know that headwinds increase the relative
//velocity, so to make a positive delta U from the
//southerly wind the sign must be switched.
vWindNED*=-1;
vWindUVW=State->GetTl2b()*vWindNED;
vWindNED *= -1;
vWindUVW = State->GetTl2b()*vWindNED;
}
soundspeed = sqrt(SHRATIO*Reng*temperature);
//cout << "Atmosphere: soundspeed: " << soundspeed << endl;

32
src/FDM/JSBSim/FGControls.cpp
View file

@ -51,36 +51,8 @@ FGControls::~FGControls() {
// $Log$
// Revision 1.10 2000/05/16 19:35:11 curt
// Updates from the Jon and Tony show.
//
// Tony submitted:
//
// JSBsim:
// Added trimming routine, it is longitudinal & in-air only at this point
// Added support for taking wind & weather data from external source
// Added support for flaps.
// Added independently settable pitch trim
// Added alphamin and max to config file, stall modeling and warning to
// follow
//
// c172.cfg:
// Flaps!
// Adjusted Cmo, model should be speed stable now
//
// FG:
// Hooked up Christian's weather code, should be using it soon.
// Hooked up the trimming routine. Note that the X-15 will not trim.
// This is not a model or trimming routine deficiency, just the
// nature of the X-15
// The trimming routine sets the pitch trim and and throttle at startup.
// The throttle is set using Norman's code for the autothrottle so the
// autothrottle is on by default. --notrim will turn it off.
// Added --vc, --mach, and --notrim switches
// (vc is airspeed in knots)
// uBody, vBody, and wBody are still supported, last one entered
// on the command line counts, i.e. you can set vc or mach or u,v,
// and w but any combination will be ignored.
// Revision 1.11 2000/05/27 03:48:15 curt
// 5/26/2000 updated from Jon and Tony.
//
// Revision 1.3 2000/04/26 10:55:57 jsb
// Made changes as required by Curt to install JSBSim into FGFS

32
src/FDM/JSBSim/FGControls.h
View file

@ -175,36 +175,8 @@ extern FGControls controls;
// $Log$
// Revision 1.9 2000/05/16 19:35:11 curt
// Updates from the Jon and Tony show.
//
// Tony submitted:
//
// JSBsim:
// Added trimming routine, it is longitudinal & in-air only at this point
// Added support for taking wind & weather data from external source
// Added support for flaps.
// Added independently settable pitch trim
// Added alphamin and max to config file, stall modeling and warning to
// follow
//
// c172.cfg:
// Flaps!
// Adjusted Cmo, model should be speed stable now
//
// FG:
// Hooked up Christian's weather code, should be using it soon.
// Hooked up the trimming routine. Note that the X-15 will not trim.
// This is not a model or trimming routine deficiency, just the
// nature of the X-15
// The trimming routine sets the pitch trim and and throttle at startup.
// The throttle is set using Norman's code for the autothrottle so the
// autothrottle is on by default. --notrim will turn it off.
// Added --vc, --mach, and --notrim switches
// (vc is airspeed in knots)
// uBody, vBody, and wBody are still supported, last one entered
// on the command line counts, i.e. you can set vc or mach or u,v,
// and w but any combination will be ignored.
// Revision 1.10 2000/05/27 03:48:15 curt
// 5/26/2000 updated from Jon and Tony.
//
// Revision 1.5 2000/05/12 22:45:35 jsb
// Removed extraneous namespace identifiers and header files

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

@ -38,7 +38,7 @@ SENTRY
#define MAX_TANKS 30
#define GRAVITY 32.174
#define INVGRAVITY 0.031081
#define EARTHRAD 20898908.00 // feet
#define EARTHRAD 20925650.00 // feet, equatorial
#define OMEGAEARTH 7.2685E-3 // rad/sec
#define EARTHRADSQRD 437882827922500.0
#define ONESECOND 4.848136811E-6

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

@ -70,12 +70,12 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) {
latitude=longitude=0;
u=v=w=0;
lastSpeedSet=setvt;
if(fdmex != NULL ) {
if(FDMExec != NULL ) {
fdmex=FDMExec;
fdmex->GetPosition()->Seth(altitude);
fdmex->GetAtmosphere()->Run();
} else {
cout << "FGInitialCondition: This class requires a pointer to an initialized FGFDMExec object" << endl;
cout << "FGInitialCondition: This class requires a pointer to an valid FGFDMExec object" << endl;
}
}
@ -130,7 +130,7 @@ void FGInitialCondition::SetMachIC(float tt) {
void FGInitialCondition::SetClimbRateFpmIC(float tt) {
if(vt != 0) {
if(vt > 0.1) {
hdot=tt/60;
gamma=asin(hdot/vt);
}
@ -159,18 +159,20 @@ void FGInitialCondition::SetAltitudeFtIC(float tt) {
altitude=tt;
fdmex->GetPosition()->Seth(altitude);
fdmex->GetAtmosphere()->Run();
//lets try to make sure the user gets what they intended
switch(lastSpeedSet) {
setvt:
case setvt:
SetVtrueKtsIC(vt);
break;
setvc:
case setvc:
SetVcalibratedKtsIC(vc);
break;
setve:
case setve:
SetVequivalentKtsIC(ve);
break;
setmach:
case setmach:
SetMachIC(mach);
break;
}
@ -211,7 +213,7 @@ float FGInitialCondition::calcVcas(float Mach) {
A = pow(((pt-p)/psl+1),0.28571);
vcas = sqrt(7*psl/rhosl*(A-1));
//cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << p << endl;
//cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << pt << endl;
return vcas;
}
@ -263,7 +265,12 @@ bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
float const relax =0.9;
int i;
bool success=false;
if(vcas < 0.1) {
Mach=0;
success=true;
return success;
}
//initializations
d=1;
if(findMachInterval(&x1,&x3,vcas)) {
@ -279,6 +286,7 @@ bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) {
//cout << "getMachFromVcas x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
d=(x3-x1)/d0;
x2=x1-d*d0*f1/(f3-f1);
f2=calcVcas(x2)-vcas;
if(f1*f2 <= 0.0) {
x3=x2;

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

@ -37,20 +37,26 @@ INCLUDES
*******************************************************************************/
#include "FGLGear.h"
#include <algorithm>
/*******************************************************************************
************************************ CODE **************************************
*******************************************************************************/
FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3), Exec(fdmex)
FGLGear::FGLGear(FGConfigFile* AC_cfg, FGFDMExec* fdmex) : vXYZ(3),
vMoment(3),
Exec(fdmex)
{
string tmp;
*AC_cfg >> tmp >> name >> vXYZ(1) >> vXYZ(2) >> vXYZ(3) >> kSpring >> bDamp
>> statFCoeff >> brakeCoeff;
State = Exec->GetState();
Aircraft = Exec->GetAircraft();
Position = Exec->GetPosition();
State = Exec->GetState();
Aircraft = Exec->GetAircraft();
Position = Exec->GetPosition();
Rotation = Exec->GetRotation();
WOW = false;
}
@ -65,10 +71,43 @@ FGLGear::~FGLGear(void)
FGColumnVector FGLGear::Force(void)
{
static FGColumnVector vForce(3);
static FGColumnVector vLocalForce(3);
static FGColumnVector vLocalGear(3);
static FGColumnVector vWhlBodyVec(3);
static FGColumnVector vWhlVelVec(3);
vWhlBodyVec = vXYZ - Aircraft->GetXYZcg();
vWhlBodyVec(eX) = -vWhlBodyVec(eX);
vWhlBodyVec(eZ) = -vWhlBodyVec(eZ);
vWhlBodyVec = vWhlBodyVec/12.0;
vLocalGear = State->GetTb2l() * vWhlBodyVec;
compressLength = vLocalGear(eZ) - Position->GetDistanceAGL();
if (compressLength > 0.00) {
WOW = true;
vWhlVelVec = State->GetTb2l() * (Rotation->GetPQR() * vWhlBodyVec);
compressSpeed = vWhlVelVec(eZ) + Position->GetVd();
vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0);
vForce = State->GetTl2b() * vLocalForce ;
// currently only aircraft body axis Z-force modeled
vMoment(eX) = vForce(eZ) * vWhlBodyVec(eY);
vMoment(eY) = -vForce(eZ) * vWhlBodyVec(eX);
vMoment(eZ) = 0.0;
} else {
WOW = false;
vForce.InitMatrix();
vMoment.InitMatrix();
}
vLocalGear = State->GetTb2l() * (vXYZ - Aircraft->GetXYZcg());
vLocalGear(3) = -vLocalGear(3);
return vForce;
}

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

@ -74,6 +74,7 @@ CLASS DECLARATION
class FGAircraft;
class FGPosition;
class FGRotation;
class FGLGear
{
@ -82,22 +83,35 @@ public:
~FGLGear(void);
FGColumnVector Force(void);
FGColumnVector Moment(void) {return vMoment;}
inline string GetName(void) {return name; }
inline bool GetWOW(void) {return WOW; }
inline float GetCompLen(void) {return compressLength;}
inline float GetCompVel(void) {return compressSpeed; }
inline float GetCompForce(void) {return Force()(3); }
private:
enum {eX=1, eY, eZ};
FGColumnVector vXYZ;
float kSpring, bDamp, compressLength;
FGColumnVector vMoment;
float kSpring, bDamp, compressLength, compressSpeed;
float statFCoeff, rollFCoeff, skidFCoeff;
float frictionForce, compForce;
float brakePct, brakeForce, brakeCoeff;
bool WOW;
string name;
FGState* State;
FGAircraft* Aircraft;
FGPosition* Position;
FGFDMExec* Exec;
FGFDMExec* Exec;
FGState* State;
FGAircraft* Aircraft;
FGPosition* Position;
FGRotation* Rotation;
};
#include "FGAircraft.h"
#include "FGPosition.h"
#include "FGRotation.h"
/******************************************************************************/
#endif

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

@ -532,6 +532,7 @@ FGColumnVector operator*(const double scalar, const FGColumnVector& C)
}
/******************************************************************************/
float FGColumnVector::Magnitude(void)
{
double num=0.0;
@ -551,6 +552,33 @@ float FGColumnVector::Magnitude(void)
FGColumnVector FGColumnVector::Normalize(void)
{
return *this/Magnitude();
double Mag = Magnitude();
for (unsigned int i=1; i<=Rows(); i++)
for (unsigned int j=1; j<=Cols(); j++)
data[i][j] = data[i][j]/Mag;
return *this;
}
/******************************************************************************/
FGColumnVector FGColumnVector::operator*(const FGColumnVector& V)
{
if (Rows() != 3 || V.Rows() != 3) {
MatrixException mE;
mE.Message = "Invalid row count in vector cross product function";
throw mE;
}
FGColumnVector Product(3);
Product(1) = data[2][1] * V(3) - data[3][1] * V(2);
Product(2) = data[3][1] * V(1) - data[1][1] * V(3);
Product(3) = data[1][1] * V(2) - data[2][1] * V(1);
return Product;
}
/******************************************************************************/

1
src/FDM/JSBSim/FGMatrix.h
View file

@ -121,6 +121,7 @@ public:
~FGColumnVector();
FGColumnVector operator*(const double scalar);
FGColumnVector operator*(const FGColumnVector& V); // Cross product operator
FGColumnVector operator/(const double scalar);
FGColumnVector operator+(const FGColumnVector& B);
FGColumnVector operator-(const FGColumnVector& B);

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

@ -186,6 +186,10 @@ void FGOutput::DelimitedOutput(void)
cout << ", ";
cout << Aircraft->GetCoefficientStrings();
}
if (SubSystems & FGAircraft::ssGroundReactions) {
cout << ", ";
cout << Aircraft->GetGroundReactionStrings();
}
cout << endl;
dFirstPass = false;
@ -251,6 +255,10 @@ void FGOutput::DelimitedOutput(void)
cout << ", ";
cout << Aircraft->GetCoefficientValues();
}
if (SubSystems & FGAircraft::ssGroundReactions) {
cout << ", ";
cout << Aircraft->GetGroundReactionValues();
}
cout << endl;
}
@ -321,6 +329,10 @@ void FGOutput::DelimitedOutput(string fname)
datafile << ", ";
datafile << Aircraft->GetCoefficientStrings();
}
if (SubSystems & FGAircraft::ssGroundReactions) {
datafile << ", ";
datafile << Aircraft->GetGroundReactionStrings();
}
datafile << endl;
sFirstPass = false;
}
@ -385,6 +397,10 @@ void FGOutput::DelimitedOutput(string fname)
datafile << ", ";
datafile << Aircraft->GetCoefficientValues();
}
if (SubSystems & FGAircraft::ssGroundReactions) {
datafile << ", ";
datafile << Aircraft->GetGroundReactionValues();
}
datafile << endl;
datafile.flush();
}

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

@ -57,11 +57,14 @@ INCLUDES
# include <simgear/compiler.h>
# ifdef FG_HAVE_STD_INCLUDES
# include <cmath>
# include <iomanip>
# else
# include <math.h>
# include <iomanip.h>
# endif
#else
# include <cmath>
# include <iomanip>
#endif
#include "FGPosition.h"
@ -91,17 +94,24 @@ FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex),
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0;
Longitude = Latitude = 0.0;
h = 0.0;
Radius = EARTHRAD + h;
gamma=Vt=0.0;
RunwayRadius = EARTHRAD;
h = gamma = Vt = 0.0;
SeaLevelRadius = EARTHRAD; // For initialization ONLY
Radius = SeaLevelRadius + h;
RunwayRadius = SeaLevelRadius;
DistanceAGL = Radius - RunwayRadius; // Geocentric
}
/******************************************************************************/
FGPosition::~FGPosition(void) {}
/******************************************************************************/
/*************************************************************************** Run
Purpose: Called on a schedule to perform Positioning algorithms
Notes: [TP] Make sure that -Vt <= hdot <= Vt, which, of course, should always
be the case
[JB] Run in standalone mode, SeaLevelRadius will be EARTHRAD. In FGFS,
SeaLevelRadius is stuffed from FGJSBSim in JSBSim.cxx each pass.
*/
bool FGPosition:: Run(void) {
double cosLat;
@ -110,7 +120,9 @@ bool FGPosition:: Run(void) {
if (!FGModel::Run()) {
GetState();
vVel = State->GetTl2b()*vUVW;
invMass = 1.0 / Aircraft->GetMass();
Radius = h + SeaLevelRadius;
invRadius = 1.0 / Radius;
cosLat = cos(Latitude);
if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
@ -122,22 +134,22 @@ bool FGPosition:: Run(void) {
Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
h = Radius - EARTHRAD; // Geocentric
h = Radius - SeaLevelRadius; // Geocentric
DistanceAGL = Radius - RunwayRadius; // Geocentric
hoverb = h/b;
hoverb = DistanceAGL/b;
if(Vt > 0) {
hdot_Vt=RadiusDot/Vt;
//make sure that -Vt <= hdot <= Vt, which, of course, should always be the case
if(fabs(hdot_Vt) <= 1) gamma= asin(hdot_Vt);
} else
gamma=0.0;
if (Vt > 0) {
hdot_Vt = RadiusDot/Vt;
if (fabs(hdot_Vt) <= 1) gamma = asin(hdot_Vt);
} else {
gamma = 0.0;
}
lastLatitudeDot = LatitudeDot;
lastLatitudeDot = LatitudeDot;
lastLongitudeDot = LongitudeDot;
lastRadiusDot = RadiusDot;
lastRadiusDot = RadiusDot;
return false;
@ -151,11 +163,10 @@ bool FGPosition:: Run(void) {
void FGPosition::GetState(void) {
dt = State->Getdt();
vUVW = Translation->GetUVW();
Vt = Translation->GetVt();
invMass = 1.0 / Aircraft->GetMass();
invRadius = 1.0 / (h + EARTHRAD);
Radius = h + EARTHRAD;
vUVW = Translation->GetUVW();
Vt = Translation->GetVt();
vVel = State->GetTb2l()*vUVW;
b = Aircraft->GetWingSpan();
}

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

@ -61,6 +61,7 @@ class FGPosition : public FGModel {
float dt;
double RunwayRadius;
double DistanceAGL;
double SeaLevelRadius;
double gamma;
double Vt;
double hoverb,b;
@ -89,6 +90,7 @@ public:
void SetLongitude(double tt) { Longitude = tt; }
void Seth(double tt) { h = tt; }
void SetRunwayRadius(double tt) { RunwayRadius = tt; }
void SetSeaLevelRadius(double tt) { SeaLevelRadius = tt;}
void SetDistanceAGL(double tt) { DistanceAGL = tt; }
bool Run(void);

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

@ -66,7 +66,8 @@ INCLUDES
FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
mTl2b(3,3),
mTs2b(3,3),
vQtrn(4) {
vQtrn(4)
{
FDMExec = fdex;
adot = bdot = 0.0;
@ -316,9 +317,15 @@ float FGState::GetParameter(int val_idx) {
case FG_ALTITUDE:
return FDMExec->GetPosition()->Geth();
case FG_BI2VEL:
return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt());
if(FDMExec->GetTranslation()->GetVt() > 0)
return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt());
else
return 0;
case FG_CI2VEL:
return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt());
if(FDMExec->GetTranslation()->GetVt() > 0)
return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt());
else
return 0;
case FG_THROTTLE_CMD:
return FDMExec->GetFCS()->GetThrottleCmd(0);
case FG_THROTTLE_POS:
@ -412,18 +419,18 @@ void FGState::CalcMatrices(void) {
Q1Q3 = vQtrn(2)*vQtrn(4);
Q2Q3 = vQtrn(3)*vQtrn(4);
mTb2l(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
mTb2l(1,2) = 2*(Q1Q2 + Q0Q3);
mTb2l(1,3) = 2*(Q1Q3 - Q0Q2);
mTb2l(2,1) = 2*(Q1Q2 - Q0Q3);
mTb2l(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
mTb2l(2,3) = 2*(Q2Q3 + Q0Q1);
mTb2l(3,1) = 2*(Q1Q3 + Q0Q2);
mTb2l(3,2) = 2*(Q2Q3 - Q0Q1);
mTb2l(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
mTl2b(1,1) = Q0Q0 + Q1Q1 - Q2Q2 - Q3Q3;
mTl2b(1,2) = 2*(Q1Q2 + Q0Q3);
mTl2b(1,3) = 2*(Q1Q3 - Q0Q2);
mTl2b(2,1) = 2*(Q1Q2 - Q0Q3);
mTl2b(2,2) = Q0Q0 - Q1Q1 + Q2Q2 - Q3Q3;
mTl2b(2,3) = 2*(Q2Q3 + Q0Q1);
mTl2b(3,1) = 2*(Q1Q3 + Q0Q2);
mTl2b(3,2) = 2*(Q2Q3 - Q0Q1);
mTl2b(3,3) = Q0Q0 - Q1Q1 - Q2Q2 + Q3Q3;
mTl2b = mTb2l;
mTl2b.T();
mTb2l = mTl2b;
mTb2l.T();
}
/******************************************************************************/
@ -449,13 +456,13 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) {
FGColumnVector FGState::CalcEuler(void) {
static FGColumnVector vEuler(3);
if (mTb2l(3,3) == 0) vEuler(ePhi) = 0.0;
else vEuler(ePhi) = atan2(mTb2l(2,3), mTb2l(3,3));
if (mTl2b(3,3) == 0) vEuler(ePhi) = 0.0;
else vEuler(ePhi) = atan2(mTl2b(2,3), mTl2b(3,3));
vEuler(eTht) = asin(-mTb2l(1,3));
vEuler(eTht) = asin(-mTl2b(1,3));
if (mTb2l(1,1) == 0.0) vEuler(ePsi) = 0.0;
else vEuler(ePsi) = atan2(mTb2l(1,2), mTb2l(1,1));
if (mTl2b(1,1) == 0.0) vEuler(ePsi) = 0.0;
else vEuler(ePsi) = atan2(mTl2b(1,2), mTl2b(1,1));
if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI;

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

@ -116,7 +116,7 @@ bool FGTranslation::Run(void) {
vUVWdot = mVel*vPQR + vForces/Mass;
vNcg=vUVWdot*INVGRAVITY;
vNcg = vUVWdot * INVGRAVITY;
vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot) + vWindUVW;
@ -132,7 +132,6 @@ bool FGTranslation::Run(void) {
Mach = Vt / State->Geta();
vlastUVWdot = vUVWdot;
} else {}
@ -151,10 +150,8 @@ void FGTranslation::GetState(void) {
Mass = Aircraft->GetMass();
rho = Atmosphere->GetDensity();
vEuler = Rotation->GetEuler();
vWindUVW = Atmosphere->GetWindUVW();
}

79
src/FDM/JSBSim/FGTrimLong.cpp
View file

@ -1,40 +1,40 @@
/*******************************************************************************
Header: FGTrimLong.cpp
Author: Tony Peden
Date started: 9/8/99
------------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) -------------
--------- Copyright (C) 1999 Anthony K. Peden (apeden@earthlink.net) ---------
This program is free software; you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
details.
You should have received a copy of the GNU General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA.
Further information about the GNU General Public License can also be found on
the world wide web at http://www.gnu.org.
HISTORY
--------------------------------------------------------------------------------
9/8/99 TP Created
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
This class takes the given set of IC's and finds the angle of attack, elevator,
and throttle setting required to fly steady level. This is currently for in-air
conditions only. It is implemented using an iterative, one-axis-at-a-time
conditions only. It is implemented using an iterative, one-axis-at-a-time
scheme. */
// !!!!!!! BEWARE ALL YE WHO ENTER HERE !!!!!!!
@ -54,10 +54,6 @@ INCLUDES
CLASS DECLARATION
*******************************************************************************/
FGTrimLong::FGTrimLong(FGFDMExec *FDMExec,FGInitialCondition *FGIC ) {
Ncycles=40;
@ -82,14 +78,13 @@ FGTrimLong::FGTrimLong(FGFDMExec *FDMExec,FGInitialCondition *FGIC ) {
trimudot=true;
axis_count=0;
}
/******************************************************************************/
FGTrimLong::~FGTrimLong(void) {}
/******************************************************************************/
void FGTrimLong::TrimStats() {
cout << endl << " Trim Statistics: " << endl;
@ -102,6 +97,8 @@ void FGTrimLong::TrimStats() {
}
}
/******************************************************************************/
void FGTrimLong::Report(void) {
cout << endl << " Trim Results" << endl;
cout << " Alpha: " << fdmex->GetTranslation()->Getalpha()*RADTODEG
@ -117,6 +114,8 @@ void FGTrimLong::Report(void) {
<< " Tolerance " << A_Tolerance << endl;
}
/******************************************************************************/
void FGTrimLong::ReportState(void) {
cout << endl << " JSBSim Trim Report" << endl;
cout << " Weight: " << fdmex->GetAircraft()->GetWeight()
@ -175,6 +174,8 @@ void FGTrimLong::ReportState(void) {
cout << " Throttle: " << fdmex->GetFCS()->GetThrottlePos(0)/100 << endl;
}
/******************************************************************************/
void FGTrimLong::setThrottlesPct(float tt) {
float tMin,tMax;
@ -187,6 +188,7 @@ void FGTrimLong::setThrottlesPct(float tt) {
}
}
/******************************************************************************/
int FGTrimLong::checkLimits(trimfp fp, float current, float min, float max) {
float lo,hi;
@ -209,12 +211,15 @@ int FGTrimLong::checkLimits(trimfp fp, float current, float min, float max) {
return result;
}
bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float eps, float min, float max, int max_iterations, int *actual_its) {
/******************************************************************************/
bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result,
float eps, float min, float max, int max_iterations, int *actual_its) {
float x1,x2,x3,f1,f2,f3,d,d0;
float const relax =0.9;
x1=x3=0;
int i;
x1 = x3 = x2 = 0;
d=1;
bool success=false;
//initializations
@ -233,8 +238,8 @@ bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float
i=0;
while ((fabs(d) > eps) && (i < max_iterations)) {
if(Debug > 1)
cout << "FGTrimLong::solve i,x1,x2,x3: " << i << ", " << x1 << ", " << x2 << ", " << x3 << endl;
cout << "FGTrimLong::solve i,x1,x2,x3: " << i << ", " << x1
<< ", " << x2 << ", " << x3 << endl;
d=(x3-x1)/d0;
x2=x1-d*d0*f1/(f3-f1);
// if(x2 < min)
@ -265,8 +270,10 @@ bool FGTrimLong::solve(trimfp fp,float guess,float desired, float *result, float
return success;
}
bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,float desired,int max_iterations) {
/******************************************************************************/
bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,
float desired,int max_iterations) {
int i=0;
bool found=false;
float flo,fhi,fguess;
@ -292,36 +299,52 @@ bool FGTrimLong::findInterval(trimfp fp, float *lo, float *hi,float guess,float
}
}
if(Debug > 1)
cout << "FGTrimLong::findInterval: i=" << i << " Lo= " << xlo << " Hi= " << xhi << " flo*fhi: " << flo*fhi << endl;
cout << "FGTrimLong::findInterval: i=" << i << " Lo= " << xlo
<< " Hi= " << xhi << " flo*fhi: " << flo*fhi << endl;
} while((found == 0) && (i <= max_iterations));
*lo=xlo;
*hi=xhi;
return found;
}
/******************************************************************************/
float FGTrimLong::udot_func(float x) {
setThrottlesPct(x);
fdmex->RunIC(fgic);
return fdmex->GetTranslation()->GetUVWdot()(1);
}
/******************************************************************************/
float FGTrimLong::wdot_func(float x) {
fgic->SetAlphaDegIC(x);
fdmex->RunIC(fgic);
return fdmex->GetTranslation()->GetUVWdot()(3);
}
/******************************************************************************/
float FGTrimLong::qdot_func(float x) {
fdmex->GetFCS()->SetPitchTrimCmd(x);
fdmex->RunIC(fgic);
return fdmex->GetRotation()->GetPQRdot()(2);
}
/******************************************************************************/
bool FGTrimLong::DoTrim(void) {
int k=0,j=0,sum=0,trim_failed=0,jmax=Naxis;
int its;
float step,temp,min,max;
if(fgic->GetVtrueKtsIC() < 1) {
cout << "Trim failed, on-ground trimming not yet implemented." << endl;
cout << "Or did you *really* mean to start in-air"
<< " with less than 1 knot airspeed?" << endl;
return false;
}
trimfp fp;
fgic -> SetAlphaDegIC((alphaMin+alphaMax)/2);
@ -421,5 +444,5 @@ bool FGTrimLong::DoTrim(void) {
}
//YOU WERE WARNED, BUT YOU DID IT ANYWAY.

3
src/FDM/JSBSim/FGUtility.cpp
View file

@ -69,9 +69,6 @@ INCLUDES
FGUtility::FGUtility()
{
// Move constant stuff to here (if any) so it won't take CPU time
// in the methods below.
SeaLevelR = EARTHRAD * ECCENT;
}

1
src/FDM/JSBSim/FGUtility.h
View file

@ -58,7 +58,6 @@ public:
protected:
private:
float SeaLevelR;
FGState* State;
FGFDMExec* FDMExec;
};

1
src/FDM/JSBSim/filtersjb/FGFCSComponent.cpp
View file

@ -1,4 +1,3 @@
/*******************************************************************************
Module: FGFCSComponent.cpp

12
src/FDM/JSBSim/filtersjb/FGFCSComponent.h
View file

@ -44,19 +44,17 @@ INCLUDES
#ifdef FGFS
# include <simgear/compiler.h>
# include STL_STRING
FG_USING_STD(string);
#else
# include <string>
#endif
#include <string>
/*******************************************************************************
DEFINES
*******************************************************************************/
class FGFCS;
using std::string;
using namespace std;
class FGFCS;
/*******************************************************************************
CLASS DECLARATION
@ -89,7 +87,7 @@ public:
inline string GetName(void) {return Name;}
};
#include <FDM/JSBsim/FGFCS.h>
#include "../FGFCS.h"
#endif

52
src/FDM/JSBSim/filtersjb/FGSummer.cpp
View file

@ -1,4 +1,3 @@
/*******************************************************************************
Module: FGSummer.cpp
@ -38,7 +37,7 @@ COMMENTS, REFERENCES, and NOTES
INCLUDES
*******************************************************************************/
#include "FGSummer.h"
#include "FGSummer.h"
/*******************************************************************************
************************************ CODE **************************************
@ -66,12 +65,16 @@ FGSummer::FGSummer(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
while ((token = AC_cfg->GetValue()) != "/COMPONENT") {
*AC_cfg >> token;
if (token == "ID") {
*AC_cfg >> ID;
cout << " ID: " << ID << endl;
cout << " ID: " << ID << endl;
} else if (token == "INPUT") {
token = AC_cfg->GetValue("INPUT");
cout << " INPUT: " << token << endl;
cout << " INPUT: " << token << endl;
if (token.find("FG_") != token.npos) {
*AC_cfg >> token;
tmpInputIndex = fcs->GetState()->GetParameterIndex(token);
@ -82,17 +85,20 @@ FGSummer::FGSummer(FGFCS* fcs, FGConfigFile* AC_cfg) : FGFCSComponent(fcs),
InputIndices.push_back(tmpInputIndex);
InputTypes.push_back(itFCS);
}
} else if (token == "CLIPTO") {
*AC_cfg >> clipmin >> clipmax;
if(clipmax > clipmin) {
clip=true;
cout << " CLIPTO: " << clipmin << ", " << clipmax << endl;
}
} else if (token == "CLIPTO") {
*AC_cfg >> clipmin >> clipmax;
if (clipmax > clipmin) {
clip = true;
cout << " CLIPTO: " << clipmin << ", " << clipmax << endl;
}
} else if (token == "OUTPUT") {
IsOutput = true;
*AC_cfg >> sOutputIdx;
cout << " OUTPUT: " <<sOutputIdx << endl;
cout << " OUTPUT: " <<sOutputIdx << endl;
OutputIdx = fcs->GetState()->GetParameterIndex(sOutputIdx);
}
}
@ -109,9 +115,10 @@ bool FGSummer::Run(void )
unsigned int idx;
// The Summer takes several inputs, so do not call the base class Run()
// FGFCSComponent::Run();
// FGFCSComponent::Run();
Output = 0.0;
for (idx=0; idx<InputIndices.size(); idx++) {
switch (InputTypes[idx]) {
case itPilotAC:
@ -119,18 +126,17 @@ bool FGSummer::Run(void )
break;
case itFCS:
Output += fcs->GetComponentOutput(InputIndices[idx]);
break;
}
}
if(clip) {
if(Output > clipmax)
Output=clipmax;
else if(Output < clipmin)
Output=clipmin;
}
if (IsOutput) { SetOutput(); }
//cout << "Out FGSummer::Run" << endl;
if (clip) {
if (Output > clipmax) Output = clipmax;
else if (Output < clipmin) Output = clipmin;
}
if (IsOutput) SetOutput();
return true;
}