}
// copy control positions into the JSBsim structure
- FDMExec.GetFCS()->SetDa( controls.get_aileron());
- FDMExec.GetFCS()->SetDe( controls.get_elevator()
+ FDMExec.GetFCS()->SetDaCmd( controls.get_aileron());
+ FDMExec.GetFCS()->SetDeCmd( controls.get_elevator()
+ controls.get_elevator_trim() );
- FDMExec.GetFCS()->SetDr( controls.get_rudder());
- FDMExec.GetFCS()->SetDf( 0.0 );
- FDMExec.GetFCS()->SetDs( 0.0 );
- FDMExec.GetFCS()->SetThrottle( FGControls::ALL_ENGINES,
+ FDMExec.GetFCS()->SetDrCmd( controls.get_rudder());
+ FDMExec.GetFCS()->SetDfCmd( 0.0 );
+ // FDMExec.GetFCS()->SetDsCmd( 0.0 );
+ FDMExec.GetFCS()->SetThrottleCmd( FGControls::ALL_ENGINES,
controls.get_throttle( 0 ) * 100.0 );
// FCS->SetBrake( controls.get_brake( 0 ) );
int FGJSBsim::copy_from_JSBsim() {
// Velocities
- set_Velocities_Local( FDMExec.GetPosition()->GetVn(),
- FDMExec.GetPosition()->GetVe(),
- FDMExec.GetPosition()->GetVd() );
+ // set_Velocities_Local( FDMExec.GetPosition()->GetVn(),
+ // FDMExec.GetPosition()->GetVe(),
+ // FDMExec.GetPosition()->GetVd() );
// set_Velocities_Ground( V_north_rel_ground, V_east_rel_ground,
// V_down_rel_ground );
// set_Velocities_Local_Airmass( V_north_airmass, V_east_airmass,
//set_V_calibrated( FDMExec.GetAuxiliary()->GetVcalibratedFPS() );
set_V_calibrated_kts( FDMExec.GetAuxiliary()->GetVcalibratedKTS() );
- set_Omega_Body( FDMExec.GetRotation()->GetP(),
- FDMExec.GetRotation()->GetQ(),
- FDMExec.GetRotation()->GetR() );
+ set_Omega_Body( FDMExec.GetState()->GetParameter(FG_ROLLRATE),
+ FDMExec.GetState()->GetParameter(FG_PITCHRATE),
+ FDMExec.GetState()->GetParameter(FG_YAWRATE) );
// set_Omega_Local( P_local, Q_local, R_local );
// set_Omega_Total( P_total, Q_total, R_total );
-
+
set_Euler_Rates( FDMExec.GetRotation()->Getphi(),
FDMExec.GetRotation()->Gettht(),
FDMExec.GetRotation()->Getpsi() );
+
// ***FIXME*** set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot );
set_Mach_number( FDMExec.GetState()->GetMach());
set_Geocentric_Position( lat_geoc, lon,
sl_radius2 * METER_TO_FEET + alt );
set_Geodetic_Position( lat_geod, lon, alt );
- set_Euler_Angles( FDMExec.GetRotation()->Getphi(),
+ set_Euler_Angles( FDMExec.GetRotation()->Getphi(),
FDMExec.GetRotation()->Gettht(),
FDMExec.GetRotation()->Getpsi() );
05/03/99 JSB Changed (for the better?) the way configurations are read in.
9/17/99 TP Combined force and moment functions. Added aero reference
point to config file. Added calculations for moments due to
- difference in cg and aero reference point
+ difference in cg and aero reference point
********************************************************************************
COMMENTS, REFERENCES, and NOTES
INCLUDES
*******************************************************************************/
-#include <sys/types.h>
#include <sys/stat.h>
+#include <sys/types.h>
#ifdef FGFS
# ifndef __BORLANDC__
************************************ CODE **************************************
*******************************************************************************/
-FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex)
+FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex),
+ vMoments(3),
+ vForces(3),
+ vXYZrp(3),
+ vbaseXYZcg(3),
+ vXYZcg(3),
+ vXYZep(3),
+ vEuler(3)
{
- int i;
-
Name = "FGAircraft";
- for (i=0;i<6;i++) coeff_ctr[i] = 0;
+ AxisIdx["LIFT"] = 0;
+ AxisIdx["SIDE"] = 1;
+ AxisIdx["DRAG"] = 2;
+ AxisIdx["ROLL"] = 3;
+ AxisIdx["PITCH"] = 4;
+ AxisIdx["YAW"] = 5;
}
+/******************************************************************************/
+
+
FGAircraft::~FGAircraft(void)
{
}
+/******************************************************************************/
+
bool FGAircraft::LoadAircraft(string aircraft_path, string engine_path, string fname)
{
string path;
- string fullpath;
string filename;
- string aircraftDef;
- string tag;
- string holding_string;
- char scratch[128];
- ifstream coeffInFile;
- streampos gpos(0);
- int axis;
- string axis_descript;
- bool readAeroRp=false;
-
- axis = -1;
-#ifdef MACOS
- aircraftDef = aircraft_path + ":" + fname + ":" + fname + ".cfg";
-#else
- aircraftDef = aircraft_path + "/" + fname + "/" + fname + ".cfg";
-#endif
- ifstream aircraftfile(aircraftDef.c_str());
- cout << "Reading Aircraft Configuration File: " << aircraftDef << endl;
- // Output->SocketStatusOutput("Reading Aircraft Configuration File: " + aircraftDef);
-
- numTanks = numEngines = 0;
- numSelectedOxiTanks = numSelectedFuelTanks = 0;
-
- Xcg=Ycg=Zcg=0; //protection for no cg specified in file
-
- while (!aircraftfile.fail()) {
- holding_string.erase();
- aircraftfile >> holding_string;
-#if defined(__BORLANDC__) || defined(FG_HAVE_NATIVE_SGI_COMPILERS) || defined(_MSC_VER) || defined(__MWERKS__)
- if (holding_string.compare(0, 2, "//") != 0) {
-#else
- if (holding_string.compare("//",0,2) != 0) {
-#endif
- if (holding_string == "CFG_VERSION") {
- aircraftfile >> CFGVersion;
- cout << "Config file version: " << CFGVersion << endl;
- if (CFGVersion < NEEDED_CFG_VERSION) {
- cout << endl << "YOU HAVE AN OLD CFG FILE FOR THIS AIRCRAFT."
- " RESULTS WILL BE UNPREDICTABLE !!" << endl << endl;
- }
- } else if (holding_string == "AIRCRAFT") {
- cout << "Reading in Aircraft parameters ..." << endl;
- } else if (holding_string == "AERODYNAMICS") {
- cout << "Reading in Aerodynamic parameters ..." << endl;
- } else if (holding_string == "AC_NAME") {
- aircraftfile >> AircraftName; // String with no embedded spaces
- cout << "Aircraft Name: " << AircraftName << endl;
- } else if (holding_string == "AC_WINGAREA") {
- aircraftfile >> WingArea;
- cout << "Aircraft Wing Area: " << WingArea << endl;
- } else if (holding_string == "AC_WINGSPAN") {
- aircraftfile >> WingSpan;
- cout << "Aircraft WingSpan: " << WingSpan << endl;
- } else if (holding_string == "AC_CHORD") {
- aircraftfile >> cbar;
- cout << "Aircraft Chord: " << cbar << endl;
- } else if (holding_string == "AC_IXX") {
- aircraftfile >> baseIxx;
- cout << "Aircraft Base Ixx: " << baseIxx << endl;
- } else if (holding_string == "AC_IYY") {
- aircraftfile >> baseIyy;
- cout << "Aircraft Base Iyy: " << baseIyy << endl;
- } else if (holding_string == "AC_IZZ") {
- aircraftfile >> baseIzz;
- cout << "Aircraft Base Izz: " << baseIzz << endl;
- } else if (holding_string == "AC_IXZ") {
- aircraftfile >> baseIxz;
- cout << "Aircraft Base Ixz: " << baseIxz << endl;
- } else if (holding_string == "AC_EMPTYWT") {
- aircraftfile >> EmptyWeight;
- EmptyMass = EmptyWeight / GRAVITY;
- cout << "Aircraft Empty Weight: " << EmptyWeight << endl;
- } else if (holding_string == "AC_AERORP") {
- aircraftfile >> Xrp >> Yrp >> Zrp;
- readAeroRp=true;
- cout << "Aerodynamic Reference Point: " << Xrp << " " << Yrp << " " << Zrp << endl;
- } else if (holding_string == "AC_CGLOC") {
- aircraftfile >> baseXcg >> baseYcg >> baseZcg;
- cout << "Aircraft Base C.G.: " << baseXcg << " " << baseYcg << " " << baseZcg << endl;
- } else if (holding_string == "AC_EYEPTLOC") {
- aircraftfile >> Xep >> Yep >> Zep;
- cout << "Pilot Eyepoint: " << Xep << " " << Yep << " " << Zep << endl;
- } else if (holding_string == "AC_TANK") {
- Tank[numTanks] = new FGTank(aircraftfile);
- switch(Tank[numTanks]->GetType()) {
- case FGTank::ttFUEL:
- numSelectedFuelTanks++;
- cout << "Reading in Fuel Tank #" << numSelectedFuelTanks << " parameters ..." << endl;
- break;
- case FGTank::ttOXIDIZER:
- numSelectedOxiTanks++;
- cout << "Reading in Oxidizer Tank #" << numSelectedOxiTanks << " parameters ..." << endl;
- break;
- }
- numTanks++;
-
- } else if (holding_string == "AC_GEAR") {
-
- lGear.push_back(new FGLGear(aircraftfile));
-
- } else if (holding_string == "AC_ENGINE") {
-
- aircraftfile >> tag;
- cout << "Reading in " << tag << " Engine parameters ..." << endl;
- Engine[numEngines] = new FGEngine(FDMExec, engine_path, tag, numEngines);
- numEngines++;
-
- } else if (holding_string == "}") {
-
- } else if (holding_string == "{") {
-
- } else if (holding_string == "LIFT") {
-
- axis_descript = " Lift Coefficients ...";
- axis = LiftCoeff;
-
- } else if (holding_string == "DRAG") {
-
- axis_descript = " Drag Coefficients ...";
- axis = DragCoeff;
-
- } else if (holding_string == "SIDE") {
-
- axis_descript = " Side Coefficients ...";
- axis = SideCoeff;
-
- } else if (holding_string == "ROLL") {
-
- axis_descript = " Roll Coefficients ...";
- axis = RollCoeff;
-
- } else if (holding_string == "PITCH") {
-
- axis_descript = " Pitch Coefficients ...";
- axis = PitchCoeff;
-
- } else if (holding_string == "YAW") {
-
- axis_descript = " Yaw Coefficients ...";
- axis = YawCoeff;
-
- }
-
- if (axis >= 0) {
- cout << axis_descript << endl;
- aircraftfile >> tag;
- gpos = aircraftfile.tellg();
- aircraftfile >> tag;
- if ( !(tag == "}") ) {
- while ( !(tag == "}") ) {
- aircraftfile.seekg(gpos);
- Coeff[axis][coeff_ctr[axis]] = new FGCoefficient(FDMExec, aircraftfile);
- coeff_ctr[axis]++;
- aircraftfile >> tag;
- gpos = aircraftfile.tellg();
- aircraftfile >> tag;
- }
- } else {
- cout << " None found ..." << endl;
- }
- }
- axis = -1;
-
- } else {
- aircraftfile.getline(scratch, 127);
+ string aircraftCfgFileName;
+ string token;
+
+ AircraftPath = aircraft_path;
+ EnginePath = engine_path;
+
+ aircraftCfgFileName = AircraftPath + "/" + fname + "/" + fname + ".cfg";
+
+ FGConfigFile AC_cfg(aircraftCfgFileName);
+
+ ReadPrologue(&AC_cfg);
+
+ while ((AC_cfg.GetNextConfigLine() != "EOF") &&
+ (token = AC_cfg.GetValue()) != "/FDM_CONFIG")
+ {
+ if (token == "METRICS") {
+ cout << " Reading Metrics" << endl;
+ ReadMetrics(&AC_cfg);
+ } else if (token == "AERODYNAMICS") {
+ cout << " Reading Aerodynamics" << endl;
+ ReadAerodynamics(&AC_cfg);
+ } else if (token == "UNDERCARRIAGE") {
+ cout << " Reading Landing Gear" << endl;
+ ReadUndercarriage(&AC_cfg);
+ } else if (token == "PROPULSION") {
+ cout << " Reading Propulsion" << endl;
+ ReadPropulsion(&AC_cfg);
+ } else if (token == "FLIGHT_CONTROL") {
+ cout << " Reading Flight Control" << endl;
+ ReadFlightControls(&AC_cfg);
}
}
-
- if (!readAeroRp) {
- Xrp = Xcg;
- Yrp = Ycg;
- Zrp = Zcg;
- cout << "Aerodynamic reference point not found, set to empty weight cg location" << endl;
- }
-
- cout << "End of Configuration File Parsing." << endl;
+
return true;
}
+/******************************************************************************/
bool FGAircraft::Run(void)
{
if (!FGModel::Run()) { // if false then execute this Run()
GetState();
- for (int i = 0; i < 3; i++) Forces[i] = Moments[i] = 0.0;
+ for (int i = 1; i <= 3; i++) vForces(i) = vMoments(i) = 0.0;
MassChange();
- FMProp(); FMAero(); FMGear(); FMMass();
-
- PutState();
+ FMProp();
+ FMAero();
+ FMGear();
+ FMMass();
} else { // skip Run() execution this time
}
return false;
}
+/******************************************************************************/
void FGAircraft::MassChange()
{
- float Xt, Xw, Yt, Yw, Zt, Zw, Tw;
+ static FGColumnVector vXYZtank(3);
+ float Tw;
float IXXt, IYYt, IZZt, IXZt;
int t;
+ unsigned int axis_ctr;
+
+ for (axis_ctr=1; axis_ctr<=3; axis_ctr++) vXYZtank(axis_ctr) = 0.0;
// UPDATE TANK CONTENTS
//
// Calculate new CG here.
- Xt = Yt = Zt = Tw = 0;
- Xw = Yw = Zw = 0;
+ Tw = 0;
for (t=0; t<numTanks; t++) {
- Xt += Tank[t]->GetX()*Tank[t]->GetContents();
- Yt += Tank[t]->GetY()*Tank[t]->GetContents();
- Zt += Tank[t]->GetZ()*Tank[t]->GetContents();
+ vXYZtank(eX) += Tank[t]->GetX()*Tank[t]->GetContents();
+ vXYZtank(eY) += Tank[t]->GetY()*Tank[t]->GetContents();
+ vXYZtank(eZ) += Tank[t]->GetZ()*Tank[t]->GetContents();
Tw += Tank[t]->GetContents();
}
- Xcg = (Xt + EmptyWeight*baseXcg) / (Tw + EmptyWeight);
- Ycg = (Yt + EmptyWeight*baseYcg) / (Tw + EmptyWeight);
- Zcg = (Zt + EmptyWeight*baseZcg) / (Tw + EmptyWeight);
+ vXYZcg = (vXYZtank + EmptyWeight*vbaseXYZcg) / (Tw + EmptyWeight);
// Calculate new moments of inertia here
IXXt = IYYt = IZZt = IXZt = 0.0;
for (t=0; t<numTanks; t++) {
- IXXt += ((Tank[t]->GetX()-Xcg)/12.0)*((Tank[t]->GetX() - Xcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
- IYYt += ((Tank[t]->GetY()-Ycg)/12.0)*((Tank[t]->GetY() - Ycg)/12.0)*Tank[t]->GetContents()/GRAVITY;
- IZZt += ((Tank[t]->GetZ()-Zcg)/12.0)*((Tank[t]->GetZ() - Zcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
- IXZt += ((Tank[t]->GetX()-Xcg)/12.0)*((Tank[t]->GetZ() - Zcg)/12.0)*Tank[t]->GetContents()/GRAVITY;
+ IXXt += ((Tank[t]->GetX()-vXYZcg(eX))/12.0)*((Tank[t]->GetX() - vXYZcg(eX))/12.0)*Tank[t]->GetContents()/GRAVITY;
+ IYYt += ((Tank[t]->GetY()-vXYZcg(eY))/12.0)*((Tank[t]->GetY() - vXYZcg(eY))/12.0)*Tank[t]->GetContents()/GRAVITY;
+ IZZt += ((Tank[t]->GetZ()-vXYZcg(eZ))/12.0)*((Tank[t]->GetZ() - vXYZcg(eZ))/12.0)*Tank[t]->GetContents()/GRAVITY;
+ IXZt += ((Tank[t]->GetX()-vXYZcg(eX))/12.0)*((Tank[t]->GetZ() - vXYZcg(eZ))/12.0)*Tank[t]->GetContents()/GRAVITY;
}
Ixx = baseIxx + IXXt;
}
+/******************************************************************************/
void FGAircraft::FMAero(void)
{
- float F[3];
- float dxcg,dycg,dzcg;
- float ca, cb, sa, sb;
- int axis_ctr,ctr;
- F[0] = F[1] = F[2] = 0.0;
+ static FGColumnVector vFs(3);
+ static FGColumnVector vDXYZcg(3);
+ unsigned int axis_ctr,ctr;
+
+ for (axis_ctr=1; axis_ctr<=3; axis_ctr++) vFs(axis_ctr) = 0.0;
for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
- for (ctr=0; ctr < coeff_ctr[axis_ctr]; ctr++) {
- F[axis_ctr] += Coeff[axis_ctr][ctr]->TotalValue();
-// Coeff[axis_ctr][ctr]->DumpSD();
+ for (ctr=0; ctr < Coeff[axis_ctr].size(); ctr++) {
+ vFs(axis_ctr+1) += Coeff[axis_ctr][ctr].TotalValue();
}
}
- ca = cos(alpha);
- sa = sin(alpha);
- cb = cos(beta);
- sb = sin(beta);
-
- Forces[0] += - F[DragCoeff]*ca*cb
- - F[SideCoeff]*ca*sb
- + F[LiftCoeff]*sa;
- Forces[1] += F[DragCoeff]*sb
- + F[SideCoeff]*cb;
- Forces[2] += - F[DragCoeff]*sa*cb
- - F[SideCoeff]*sa*sb
- - F[LiftCoeff]*ca;
+ vForces += State->GetTs2b(alpha, beta)*vFs;
// The d*cg distances below, given in inches, are the distances FROM the c.g.
// TO the reference point. Since the c.g. and ref point are given in inches in
// is given with X positive out the nose, the dxcg and dzcg values are
// *rotated* 180 degrees about the Y axis.
- dxcg = -(Xrp - Xcg)/12; //cg and rp values are in inches
- dycg = (Yrp - Ycg)/12;
- dzcg = -(Zrp - Zcg)/12;
+ vDXYZcg(eX) = -(vXYZrp(eX) - vXYZcg(eX))/12.0; //cg and rp values are in inches
+ vDXYZcg(eY) = (vXYZrp(eY) - vXYZcg(eY))/12.0;
+ vDXYZcg(eZ) = -(vXYZrp(eZ) - vXYZcg(eZ))/12.0;
- Moments[0] += Forces[2]*dycg - Forces[1]*dzcg; //rolling moment
- Moments[1] += Forces[0]*dzcg - Forces[2]*dxcg; //pitching moment
- Moments[2] += -Forces[0]*dycg + Forces[1]*dxcg; //yawing moment
+ vMoments(eL) += vForces(eZ)*vDXYZcg(eY) - vForces(eY)*vDXYZcg(eZ); // rolling moment
+ vMoments(eM) += vForces(eX)*vDXYZcg(eZ) - vForces(eZ)*vDXYZcg(eX); // pitching moment
+ vMoments(eN) += vForces(eX)*vDXYZcg(eY) - vForces(eY)*vDXYZcg(eX); // yawing moment
for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
- for (ctr = 0; ctr < coeff_ctr[axis_ctr+3]; ctr++) {
- Moments[axis_ctr] += Coeff[axis_ctr+3][ctr]->TotalValue();
-// Coeff[axis_ctr+3][ctr]->DumpSD();
+ for (ctr = 0; ctr < Coeff[axis_ctr+3].size(); ctr++) {
+ vMoments(axis_ctr+1) += Coeff[axis_ctr+3][ctr].TotalValue();
}
}
}
+/******************************************************************************/
void FGAircraft::FMGear(void)
{
if (GearUp) {
// crash routine
} else {
- for (int i=0;i<lGear.size();i++) {
+ for (unsigned int i=0;i<lGear.size();i++) {
// lGear[i].
}
}
}
+/******************************************************************************/
void FGAircraft::FMMass(void)
{
- Forces[0] += -GRAVITY*sin(tht) * Mass;
- Forces[1] += GRAVITY*sin(phi)*cos(tht) * Mass;
- Forces[2] += GRAVITY*cos(phi)*cos(tht) * Mass;
+ vForces(eX) += -GRAVITY*sin(vEuler(eTht)) * Mass;
+ vForces(eY) += GRAVITY*sin(vEuler(ePhi))*cos(vEuler(eTht)) * Mass;
+ vForces(eZ) += GRAVITY*cos(vEuler(ePhi))*cos(vEuler(eTht)) * Mass;
}
+/******************************************************************************/
void FGAircraft::FMProp(void)
{
for (int i=0;i<numEngines;i++) {
- Forces[0] += Engine[i]->CalcThrust();
+ vForces(eX) += Engine[i]->CalcThrust();
}
}
+/******************************************************************************/
+
void FGAircraft::GetState(void)
{
dt = State->Getdt();
alpha = Translation->Getalpha();
beta = Translation->Getbeta();
- phi = Rotation->Getphi();
- tht = Rotation->Gettht();
- psi = Rotation->Getpsi();
+ vEuler = Rotation->GetEuler();
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadMetrics(FGConfigFile* AC_cfg)
+{
+ string token = "";
+ string parameter;
+
+ AC_cfg->GetNextConfigLine();
+
+ while ((token = AC_cfg->GetValue()) != "/METRICS") {
+ *AC_cfg >> parameter;
+ if (parameter == "AC_WINGAREA") *AC_cfg >> WingArea;
+ else if (parameter == "AC_WINGSPAN") *AC_cfg >> WingSpan;
+ else if (parameter == "AC_CHORD") *AC_cfg >> cbar;
+ else if (parameter == "AC_IXX") *AC_cfg >> baseIxx;
+ else if (parameter == "AC_IYY") *AC_cfg >> baseIyy;
+ else if (parameter == "AC_IZZ") *AC_cfg >> baseIzz;
+ else if (parameter == "AC_IXZ") *AC_cfg >> baseIxz;
+ else if (parameter == "AC_EMPTYWT") *AC_cfg >> EmptyWeight;
+ else if (parameter == "AC_CGLOC") *AC_cfg >> vbaseXYZcg(eX) >> vbaseXYZcg(eY) >> vbaseXYZcg(eZ);
+ else if (parameter == "AC_EYEPTLOC") *AC_cfg >> vXYZep(eX) >> vXYZep(eY) >> vXYZep(eZ);
+ else if (parameter == "AC_AERORP") *AC_cfg >> vXYZrp(eX) >> vXYZrp(eY) >> vXYZrp(eZ);
+ }
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadPropulsion(FGConfigFile* AC_cfg)
+{
+ string token;
+ string engine_name;
+ string parameter;
+
+ AC_cfg->GetNextConfigLine();
+
+ while ((token = AC_cfg->GetValue()) != "/PROPULSION") {
+ *AC_cfg >> parameter;
+
+ if (parameter == "AC_ENGINE") {
+
+ *AC_cfg >> engine_name;
+ Engine[numEngines] = new FGEngine(FDMExec, EnginePath, engine_name, numEngines);
+ numEngines++;
+
+ } else if (parameter == "AC_TANK") {
+
+ Tank[numTanks] = new FGTank(AC_cfg);
+ switch(Tank[numTanks]->GetType()) {
+ case FGTank::ttFUEL:
+ numSelectedFuelTanks++;
+ break;
+ case FGTank::ttOXIDIZER:
+ numSelectedOxiTanks++;
+ break;
+ }
+ numTanks++;
+ }
+ }
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadFlightControls(FGConfigFile* AC_cfg)
+{
+ string token;
+
+ FCS->LoadFCS(AC_cfg);
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadAerodynamics(FGConfigFile* AC_cfg)
+{
+ string token, axis;
+
+ AC_cfg->GetNextConfigLine();
+
+ Coeff.push_back(*(new CoeffArray()));
+ Coeff.push_back(*(new CoeffArray()));
+ Coeff.push_back(*(new CoeffArray()));
+ Coeff.push_back(*(new CoeffArray()));
+ Coeff.push_back(*(new CoeffArray()));
+ Coeff.push_back(*(new CoeffArray()));
+
+ while ((token = AC_cfg->GetValue()) != "/AERODYNAMICS") {
+ if (token == "AXIS") {
+ axis = AC_cfg->GetValue("NAME");
+ AC_cfg->GetNextConfigLine();
+ while ((token = AC_cfg->GetValue()) != "/AXIS") {
+ Coeff[AxisIdx[axis]].push_back(*(new FGCoefficient(FDMExec, AC_cfg)));
+ DisplayCoeffFactors(Coeff[AxisIdx[axis]].back().Getmultipliers());
+ }
+ AC_cfg->GetNextConfigLine();
+ }
+ }
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadUndercarriage(FGConfigFile* AC_cfg)
+{
+ string token;
+
+ AC_cfg->GetNextConfigLine();
+
+ while ((token = AC_cfg->GetValue()) != "/UNDERCARRIAGE") {
+ lGear.push_back(new FGLGear(AC_cfg));
+ }
+}
+
+/******************************************************************************/
+
+void FGAircraft::ReadPrologue(FGConfigFile* AC_cfg)
+{
+ string token = AC_cfg->GetValue();
+
+ AircraftName = AC_cfg->GetValue("NAME");
+ cout << "Reading Aircraft Configuration File: " << AircraftName << endl;
+ CFGVersion = strtod(AC_cfg->GetValue("VERSION").c_str(),NULL);
+ cout << " Version: " << CFGVersion << endl;
+
+ if (CFGVersion < NEEDED_CFG_VERSION) {
+ cout << endl << "YOU HAVE AN OLD CFG FILE FOR THIS AIRCRAFT."
+ " RESULTS WILL BE UNPREDICTABLE !!" << endl;
+ cout << "Current version needed is: " << NEEDED_CFG_VERSION << endl;
+ cout << " You have version: " << CFGVersion << endl << endl;
+ exit(-1);
+ }
}
+/******************************************************************************/
-void FGAircraft::PutState(void)
+void FGAircraft::DisplayCoeffFactors(int multipliers)
{
+ cout << " Non-Dimensionalized by: ";
+
+ if (multipliers & FG_QBAR) cout << "qbar ";
+ if (multipliers & FG_WINGAREA) cout << "S ";
+ if (multipliers & FG_WINGSPAN) cout << "b ";
+ if (multipliers & FG_CBAR) cout << "c ";
+ if (multipliers & FG_ALPHA) cout << "alpha ";
+ if (multipliers & FG_ALPHADOT) cout << "alphadot ";
+ if (multipliers & FG_BETA) cout << "beta ";
+ if (multipliers & FG_BETADOT) cout << "betadot ";
+ if (multipliers & FG_PITCHRATE) cout << "q ";
+ if (multipliers & FG_ROLLRATE) cout << "p ";
+ if (multipliers & FG_YAWRATE) cout << "r ";
+
+ if (multipliers & FG_ELEVATOR_CMD) cout << "De cmd ";
+ if (multipliers & FG_AILERON_CMD) cout << "Da cmd ";
+ if (multipliers & FG_RUDDER_CMD) cout << "Dr cmd ";
+ if (multipliers & FG_FLAPS_CMD) cout << "Df cmd ";
+ if (multipliers & FG_SPOILERS_CMD) cout << "Dsp cmd ";
+ if (multipliers & FG_SPDBRAKE_CMD) cout << "Dsb cmd ";
+
+ if (multipliers & FG_ELEVATOR_POS) cout << "De ";
+ if (multipliers & FG_AILERON_POS) cout << "Da ";
+ if (multipliers & FG_RUDDER_POS) cout << "Dr ";
+ if (multipliers & FG_FLAPS_POS) cout << "Df ";
+ if (multipliers & FG_SPOILERS_POS) cout << "Dsp ";
+ if (multipliers & FG_SPDBRAKE_POS) cout << "Dsb ";
+
+ if (multipliers & FG_MACH) cout << "Mach ";
+ if (multipliers & FG_ALTITUDE) cout << "h ";
+ if (multipliers & FG_BI2VEL) cout << "b /(2*Vt) ";
+ if (multipliers & FG_CI2VEL) cout << "c /(2*Vt) ";
+
+ cout << endl;
}
+/******************************************************************************/
+
+
#ifdef FGFS
# include <simgear/compiler.h>
# ifdef FG_HAVE_STD_INCLUDES
-# include <fstream>
# include <vector>
+# include <map>
# else
-# include <fstream.h>
# include <vector.h>
+# include <map.h>
# endif
#else
-# include <fstream>
# include <vector>
+# include <map>
#endif
#include "FGModel.h"
#include "FGEngine.h"
#include "FGTank.h"
#include "FGLGear.h"
+#include "FGConfigFile.h"
+#include "FGMatrix.h"
/*******************************************************************************
DEFINITIONS
class FGAircraft : public FGModel
{
+ enum {eL=1, eM, eN};
+ enum {eX=1, eY, eZ};
+ enum {eP=1, eQ, eR};
+ enum {ePhi=1, eTht, ePsi};
public:
FGAircraft(FGFDMExec*);
~FGAircraft(void);
inline FGTank* GetTank(int tt) {return Tank[tt];}
inline float GetWeight(void) {return Weight;}
inline float GetMass(void) {return Mass;}
- inline float GetL(void) {return Moments[0];}
- inline float GetM(void) {return Moments[1];}
- inline float GetN(void) {return Moments[2];}
- inline float GetFx(void) {return Forces[0];}
- inline float GetFy(void) {return Forces[1];}
- inline float GetFz(void) {return Forces[2];}
+ inline FGColumnVector GetMoments(void) {return vMoments;}
+ inline FGColumnVector GetForces(void) {return vForces;}
inline float GetIxx(void) {return Ixx;}
inline float GetIyy(void) {return Iyy;}
inline float GetIzz(void) {return Izz;}
inline float GetIxz(void) {return Ixz;}
- inline float GetXcg(void) {return Xcg;}
inline int GetNumEngines(void) {return numEngines;}
+ inline FGColumnVector GetXYZcg(void) {return vXYZcg;}
private:
void GetState(void);
- void PutState(void);
void FMAero(void);
void FMGear(void);
void FMMass(void);
void FMProp(void);
void MassChange(void);
- float Moments[3];
- float Forces[3];
- string AircraftName;
+ FGColumnVector vMoments;
+ FGColumnVector vForces;
+ FGColumnVector vXYZrp;
+ FGColumnVector vbaseXYZcg;
+ FGColumnVector vXYZcg;
+ FGColumnVector vXYZep;
+ FGColumnVector vEuler;
float baseIxx, baseIyy, baseIzz, baseIxz, EmptyMass, Mass;
float Ixx, Iyy, Izz, Ixz;
- float Xrp, Yrp, Zrp;
- float baseXcg, baseYcg, baseZcg;
- float Xcg, Ycg, Zcg;
- float Xep, Yep, Zep;
float rho, qbar, Vt;
float alpha, beta;
float WingArea, WingSpan, cbar;
- float phi, tht, psi;
float Weight, EmptyWeight;
float dt;
- float CFGVersion;
+ double CFGVersion;
+ string AircraftName;
int numTanks;
int numEngines;
FGTank* Tank[MAX_TANKS];
FGEngine *Engine[MAX_ENGINES];
- FGCoefficient *Coeff[6][10];
- int coeff_ctr[6];
+ typedef map<string,int> AxisIndex;
+ AxisIndex AxisIdx;
+
+ typedef vector<FGCoefficient> CoeffArray;
+ typedef vector<CoeffArray> CoeffVector;
+
+ CoeffVector Coeff;
+
+ void DisplayCoeffFactors(int multipliers);
bool GearUp;
string Axis[6];
vector <FGLGear*> lGear;
+ string AircraftPath;
+ string EnginePath;
+ void ReadMetrics(FGConfigFile*);
+ void ReadPropulsion(FGConfigFile*);
+ void ReadFlightControls(FGConfigFile*);
+ void ReadAerodynamics(FGConfigFile*);
+ void ReadUndercarriage(FGConfigFile*);
+ void ReadPrologue(FGConfigFile*);
protected:
Name = "FGAtmosphere";
h = 0;
Calculate(h);
- temperature = T;
- pressure = p;
- density = rho;
- soundspeed = a;
+ SLtemperature = temperature;
+ SLpressure = pressure;
+ SLdensity = density;
+ SLsoundspeed = sqrt(SHRATIO*Reng*temperature);
+ useExternal=false;
}
bool FGAtmosphere::Run(void)
{
if (!FGModel::Run()) { // if false then execute this Run()
- h = State->Geth();
-
- Calculate(h);
-
- temperature = T;
- pressure = p;
- density = rhos;
- soundspeed = a;
+ if (!useExternal) {
+ h = State->Geth();
+ Calculate(h);
+ } else {
+ density = exDensity;
+ pressure = exPressure;
+ temperature = exTemperature;
+ }
+ soundspeed = sqrt(SHRATIO*Reng*temperature);
State->Seta(soundspeed);
} else { // skip Run() execution this time
}
return false;
}
-
-float FGAtmosphere::CalcRho(float altitude)
-{
- //return (0.00237 - 7.0E-08*altitude
- // + 7.0E-13*altitude*altitude
- // - 2.0E-18*altitude*altitude*altitude);
- return GetDensity(altitude);
-}
-
-
void FGAtmosphere::Calculate(float altitude)
{
//see reference [1]
- float slope,reftemp,refpress,refdens;
- int i=0;
- float htab[]={0,36089,82020,154198,173882,259183,295272,344484}; //ft.
-
- if (altitude <= htab[0]) {
- altitude=0;
- } else if (altitude >= htab[7]){
- i = 7;
- altitude = htab[7];
- } else {
- while (htab[i+1] < altitude) {
- i++;
- }
- }
-
- switch(i) {
- case 0: // sea level
- slope = -0.0035662; // R/ft.
- reftemp = 518.688; // R
- refpress = 2116.17; // psf
- refdens = 0.0023765; // slugs/cubic ft.
- break;
- case 1: // 36089 ft.
- slope = 0;
- reftemp = 389.988;
- refpress = 474.1;
- refdens = 0.0007078;
- break;
- case 2: // 82020 ft.
- slope = 0.00164594;
- reftemp = 389.988;
- refpress = 52.7838;
- refdens = 7.8849E-5;
- break;
- case 3: // 154198 ft.
- slope = 0;
- reftemp = 508.788;
- refpress = 2.62274;
- refdens = 3.01379E-6;
- break;
- case 4: // 173882 ft.
- slope = -0.00246891;
- reftemp = 508.788;
- refpress = 1.28428;
- refdens = 1.47035e-06;
- break;
- case 5: // 259183 ft.
- slope = 0;
- reftemp = 298.188;
- refpress = 0.0222008;
- refdens = 4.33396e-08;
- break;
- case 6: // 295272 ft.
- slope = 0.00219459;
- reftemp = 298.188;
- refpress = 0.00215742;
- refdens = 4.21368e-09;
- break;
- case 7: // 344484 ft.
- slope = 0;
- reftemp = 406.188;
- refpress = 0.000153755;
- refdens = 2.20384e-10;
- break;
- }
-
-
- if (slope == 0) {
- T = reftemp;
- p = refpress*exp(-GRAVITY/(reftemp*Reng)*(altitude-htab[i]));
- rhos = refdens*exp(-GRAVITY/(reftemp*Reng)*(altitude-htab[i]));
- } else {
- T = reftemp+slope*(altitude-htab[i]);
- p = refpress*pow(T/reftemp,-GRAVITY/(slope*Reng));
- rhos = refdens*pow(T/reftemp,-(GRAVITY/(slope*Reng)+1));
- }
- a = sqrt(SHRATIO*Reng*T);
-
-}
-
-
-float FGAtmosphere::GetTemperature(float altitude)
-{
- Calculate(altitude);
- return T;
-}
+ float slope,reftemp,refpress,refdens;
+ int i=0;
+ float htab[]={0,36089,82020,154198,173882,259183,295272,344484}; //ft.
+// cout << "Atmosphere: h=" << altitude << " rho= " << density << endl;
+ if (altitude <= htab[0]) {
+ altitude=0;
+ } else if (altitude >= htab[7]){
+ i = 7;
+ altitude = htab[7];
+ } else {
+ while (htab[i+1] < altitude) {
+ i++;
+ }
+ }
+ switch(i) {
+ case 0: // sea level
+ slope = -0.0035662; // R/ft.
+ reftemp = 518.688; // R
+ refpress = 2116.17; // psf
+ refdens = 0.0023765; // slugs/cubic ft.
+ break;
+ case 1: // 36089 ft.
+ slope = 0;
+ reftemp = 389.988;
+ refpress = 474.1;
+ refdens = 0.0007078;
+ break;
+ case 2: // 82020 ft.
+ slope = 0.00164594;
+ reftemp = 389.988;
+ refpress = 52.7838;
+ refdens = 7.8849E-5;
+ break;
+ case 3: // 154198 ft.
+ slope = 0;
+ reftemp = 508.788;
+ refpress = 2.62274;
+ refdens = 3.01379E-6;
+ break;
+ case 4: // 173882 ft.
+ slope = -0.00246891;
+ reftemp = 508.788;
+ refpress = 1.28428;
+ refdens = 1.47035e-06;
+ break;
+ case 5: // 259183 ft.
+ slope = 0;
+ reftemp = 298.188;
+ refpress = 0.0222008;
+ refdens = 4.33396e-08;
+ break;
+ case 6: // 295272 ft.
+ slope = 0.00219459;
+ reftemp = 298.188;
+ refpress = 0.00215742;
+ refdens = 4.21368e-09;
+ break;
+ case 7: // 344484 ft.
+ slope = 0;
+ reftemp = 406.188;
+ refpress = 0.000153755;
+ refdens = 2.20384e-10;
+ break;
+ }
-float FGAtmosphere::GetPressure(float altitude)
-{
- Calculate(altitude);
- return p;
-}
-float FGAtmosphere::GetDensity(float altitude)
-{
- Calculate(altitude);
- return rhos;
-}
+ if (slope == 0) {
+ temperature = reftemp;
+ pressure = refpress*exp(-GRAVITY/(reftemp*Reng)*(altitude-htab[i]));
+ density = refdens*exp(-GRAVITY/(reftemp*Reng)*(altitude-htab[i]));
+ } else {
+ temperature = reftemp+slope*(altitude-htab[i]);
+ pressure = refpress*pow(temperature/reftemp,-GRAVITY/(slope*Reng));
+ density = refdens*pow(temperature/reftemp,-(GRAVITY/(slope*Reng)+1));
+ }
+ //cout << "Atmosphere: h=" << altitude << " rho= " << density << endl;
-float FGAtmosphere::GetSoundSpeed(float altitude)
-{
- Calculate(altitude);
- return a;
}
~FGAtmosphere(void);
bool Run(void);
- inline float Getrho(void) {return density;}
- float CalcRho(float altitude);
-
inline float GetTemperature(void){return temperature;}
inline float GetDensity(void) {return density;} // use only after Run() has been called
inline float GetPressure(void) {return pressure;}
inline float GetSoundSpeed(void) {return soundspeed;}
- float GetTemperature(float altitude); //Rankine, altitude in feet
- float GetDensity(float altitude); //slugs/ft^3
- float GetPressure(float altitude); //lbs/ft^2
- float GetSoundSpeed(float altitude); //ft/s
+ inline float GetTemperatureSL(void) { return SLtemperature; } //Rankine, altitude in feet
+ inline float GetDensitySL(void) { return SLdensity; } //slugs/ft^3
+ inline float GetPressureSL(void) { return SLpressure; } //lbs/ft^2
+ inline float GetSoundSpeedSL(void) { return SLsoundspeed; } //ft/s
+
+ inline float GetPressureRatio(void) { return temperature/SLtemperature; }
+ inline float GetDensityRatio(void) { return density/SLdensity; }
+ inline float GetTemperatureRatio(void) { return pressure/SLpressure; }
+ inline float GetSoundSpeedRatio(void) { return soundspeed/SLsoundspeed; }
+
+ inline void UseExternal(void) { useExternal=true; }
+ inline void UseInternal(void) { useExternal=false; } //this is the default
+
+ inline void SetExTemperature(float t) { exTemperature=t; }
+ inline void SetExDensity(float d) { exDensity=d; }
+ inline void SetExPressure(float p) { exPressure=p; }
protected:
float rho;
float h;
- float temperature,T;
- float pressure,p;
- float density,rhos;
- float soundspeed,a;
+ float SLtemperature,SLdensity,SLpressure,SLsoundspeed;
+ float temperature,density,pressure,soundspeed;
+ bool useExternal;
+ float exTemperature,exDensity,exPressure;
void Calculate(float altitude);
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
-This class calculates various auxiliary parameters, mostly used by the visual
-system
+This class calculates various auxiliary parameters.
REFERENCES
- Anderson, John D. "Introduction to Flight", 3rd Edition, McGraw-Hill, 1989
- pgs. 112-126
+ Anderson, John D. "Introduction to Flight", 3rd Edition, McGraw-Hill, 1989
+ pgs. 112-126
HISTORY
--------------------------------------------------------------------------------
01/26/99 JSB Created
FGAuxiliary::FGAuxiliary(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGAuxiliary";
- vcas=veas=mach=qbar=pt=0;
- psl=rhosl=1;
+ vcas = veas = mach = qbar = pt = 0;
+ psl = rhosl = 1;
}
bool FGAuxiliary::Run()
{
float A,B,D;
+
if (!FGModel::Run()) {
- GetState();
- if(mach < 1)
- //calculate total pressure assuming isentropic flow
- pt=p*pow((1 + 0.2*mach*mach),3.5);
- else
- {
- // shock in front of pitot tube, we'll assume its normal and use
- // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
- // pressure behind the shock to the static pressure in front
- B=5.76*mach*mach/(5.6*mach*mach - 0.8);
- // The denominator above is zero for Mach ~ 0.38, for which
- // we'll never be here, so we're safe
- D=(2.8*mach*mach-0.4)*0.4167;
- pt=p*pow(B,3.5)*D;
- }
- A=pow(((pt-p)/psl+1),0.28571);
- vcas=sqrt(7*psl/rhosl*(A-1));
- veas=sqrt(2*qbar/rhosl);
+ GetState();
+ if(mach < 1) //calculate total pressure assuming isentropic flow
+ pt=p*pow((1 + 0.2*mach*mach),3.5);
+ else
+ {
+ // shock in front of pitot tube, we'll assume its normal and use
+ // the Rayleigh Pitot Tube Formula, i.e. the ratio of total
+ // pressure behind the shock to the static pressure in front
+
+ B = 5.76*mach*mach/(5.6*mach*mach - 0.8);
+
+ // The denominator above is zero for Mach ~ 0.38, for which
+ // we'll never be here, so we're safe
+
+ D = (2.8*mach*mach-0.4)*0.4167;
+ pt = p*pow(B,3.5)*D;
+ }
+
+ A = pow(((pt-p)/psl+1),0.28571);
+ vcas = sqrt(7*psl/rhosl*(A-1));
+ veas = sqrt(2*qbar/rhosl);
+
} else {
+
}
+
return false;
}
void FGAuxiliary::GetState(void)
{
- qbar=State->Getqbar();
- mach=State->GetMach();
- p=Atmosphere->GetPressure();
- rhosl=Atmosphere->GetDensity(0);
- psl=Atmosphere->GetPressure(0);
-}
+ qbar = State->Getqbar();
+ mach = State->GetMach();
+ p = Atmosphere->GetPressure();
+ rhosl = Atmosphere->GetDensitySL();
+ psl = Atmosphere->GetPressureSL();
+}
void FGAuxiliary::PutState(void){}
*******************************************************************************/
#include "FGCoefficient.h"
-#include "FGAtmosphere.h"
#include "FGState.h"
#include "FGFDMExec.h"
-#include "FGFCS.h"
-#include "FGAircraft.h"
-#include "FGTranslation.h"
-#include "FGRotation.h"
-#include "FGPosition.h"
-#include "FGAuxiliary.h"
-#include "FGOutput.h"
/*******************************************************************************
************************************ CODE **************************************
*******************************************************************************/
-FGCoefficient::FGCoefficient(FGFDMExec* fdex, ifstream& coeffDefFile)
+FGCoefficient::FGCoefficient(FGFDMExec* fdex, FGConfigFile* AC_cfg)
{
int r, c, start, end, n;
float ftrashcan;
- string strashcan;
-
- coeffdef["FG_QBAR"] = 1;
- coeffdef["FG_WINGAREA"] = 2;
- coeffdef["FG_WINGSPAN"] = 4;
- coeffdef["FG_CBAR"] = 8;
- coeffdef["FG_ALPHA"] = 16;
- coeffdef["FG_ALPHADOT"] = 32;
- coeffdef["FG_BETA"] = 64;
- coeffdef["FG_BETADOT"] = 128;
- coeffdef["FG_PITCHRATE"] = 256;
- coeffdef["FG_ROLLRATE"] = 512;
- coeffdef["FG_YAWRATE"] = 1024;
- coeffdef["FG_ELEVATOR"] = 2048;
- coeffdef["FG_AILERON"] = 4096;
- coeffdef["FG_RUDDER"] = 8192;
- coeffdef["FG_MACH"] = 16384;
- coeffdef["FG_ALTITUDE"] = 32768L;
- coeffdef["FG_BI2VEL"] = 65536L;
- coeffdef["FG_CI2VEL"] = 131072L;
+ string multparms;
FDMExec = fdex;
State = FDMExec->GetState();
- Atmosphere = FDMExec->GetAtmosphere();
- FCS = FDMExec->GetFCS();
- Aircraft = FDMExec->GetAircraft();
- Translation = FDMExec->GetTranslation();
- Rotation = FDMExec->GetRotation();
- Position = FDMExec->GetPosition();
- Auxiliary = FDMExec->GetAuxiliary();
- Output = FDMExec->GetOutput();
-
- if (coeffDefFile) {
- if (!coeffDefFile.fail()) {
- coeffDefFile >> name;
- cout << " " << name << endl;
- coeffDefFile >> strashcan;
- coeffDefFile >> description;
- cout << " " << description << endl;
- coeffDefFile >> method;
- cout << " " << method << endl;
-
- if (method == "EQUATION") type = EQUATION;
- else if (method == "TABLE") type = TABLE;
- else if (method == "VECTOR") type = VECTOR;
- else if (method == "VALUE") type = VALUE;
- else type = UNKNOWN;
-
- if (type == VECTOR || type == TABLE) {
- coeffDefFile >> rows;
- cout << " Rows: " << rows << " ";
- if (type == TABLE) {
- coeffDefFile >> columns;
- cout << "Cols: " << columns;
- }
- cout << endl;
+ if (AC_cfg) {
+ name = AC_cfg->GetValue("NAME");
+ method = AC_cfg->GetValue("TYPE");
- coeffDefFile >> strashcan;
- if (strashcan.substr(0,1) == "F") {
- LookupR = coeffdef[strashcan.c_str()];
- cout << " Row indexing parameter: " << strashcan << endl;
- } else {
- LookupR = atoi(strashcan.c_str());
- cout << " Row indexing parameter: " << LookupR << endl;
- }
+ AC_cfg->GetNextConfigLine();
+ *AC_cfg >> description;
- }
+ cout << " " << name << endl;
+ cout << " " << description << endl;
+ cout << " " << method << endl;
+ if (method == "EQUATION") type = EQUATION;
+ else if (method == "TABLE") type = TABLE;
+ else if (method == "VECTOR") type = VECTOR;
+ else if (method == "VALUE") type = VALUE;
+ else type = UNKNOWN;
+
+ if (type == VECTOR || type == TABLE) {
+ *AC_cfg >> rows;
+ cout << " Rows: " << rows << " ";
if (type == TABLE) {
- coeffDefFile >> strashcan;
- if (strashcan.substr(0,1) == "F") {
- LookupC = coeffdef[strashcan.c_str()];
- cout << " Column indexing parameter: " << strashcan << endl;
- } else {
- LookupC = atoi(strashcan.c_str());
- cout << " Column indexing parameter: " << LookupC << endl;
- }
+ *AC_cfg >> columns;
+ cout << "Cols: " << columns;
}
- coeffDefFile >> strashcan;
-
- end = strashcan.length();
- n = strashcan.find("|");
- start = 0;
- multipliers = 0;
- if (strashcan.substr(0,1) == "F") {
- while(n < end && n >= 0) {
- n -= start;
- multipliers += coeffdef[strashcan.substr(start,n).c_str()];
- start += n+1;
- n = strashcan.find("|",start);
- }
- multipliers += coeffdef[strashcan.substr(start,end).c_str()];
+ cout << endl;
+
+ *AC_cfg >> multparms;
+ if (multparms.substr(0,1) == "F") {
+ LookupR = State->GetParameterIndex(multparms);
+ cout << " Row indexing parameter: " << multparms << endl;
} else {
- multipliers = atoi(strashcan.c_str());
+ LookupR = atoi(multparms.c_str());
+ cout << " Row indexing parameter: " << LookupR << endl;
}
- cout << " Non-Dimensionalized by: ";
+ }
- mult_count = 0;
- if (multipliers & FG_QBAR) {
- mult_idx[mult_count] = FG_QBAR;
- mult_count++;
- cout << "qbar ";
- }
- if (multipliers & FG_WINGAREA) {
- mult_idx[mult_count] = FG_WINGAREA;
- mult_count++;
- cout << "S ";
- }
- if (multipliers & FG_WINGSPAN) {
- mult_idx[mult_count] = FG_WINGSPAN;
- mult_count++;
- cout << "b ";
- }
- if (multipliers & FG_CBAR) {
- mult_idx[mult_count] = FG_CBAR;
- mult_count++;
- cout << "c ";
- }
- if (multipliers & FG_ALPHA) {
- mult_idx[mult_count] = FG_ALPHA;
- mult_count++;
- cout << "alpha ";
- }
- if (multipliers & FG_ALPHADOT) {
- mult_idx[mult_count] = FG_ALPHADOT;
- mult_count++;
- cout << "alphadot ";
- }
- if (multipliers & FG_BETA) {
- mult_idx[mult_count] = FG_BETA;
- mult_count++;
- cout << "beta ";
- }
- if (multipliers & FG_BETADOT) {
- mult_idx[mult_count] = FG_BETADOT;
- mult_count++;
- cout << "betadot ";
- }
- if (multipliers & FG_PITCHRATE) {
- mult_idx[mult_count] = FG_PITCHRATE;
- mult_count++;
- cout << "q ";
- }
- if (multipliers & FG_ROLLRATE) {
- mult_idx[mult_count] = FG_ROLLRATE;
- mult_count++;
- cout << "p ";
- }
- if (multipliers & FG_YAWRATE) {
- mult_idx[mult_count] = FG_YAWRATE;
- mult_count++;
- cout << "r ";
- }
- if (multipliers & FG_ELEVATOR) {
- mult_idx[mult_count] = FG_ELEVATOR;
- mult_count++;
- cout << "De ";
- }
- if (multipliers & FG_AILERON) {
- mult_idx[mult_count] = FG_AILERON;
- mult_count++;
- cout << "Da ";
- }
- if (multipliers & FG_RUDDER) {
- mult_idx[mult_count] = FG_RUDDER;
- mult_count++;
- cout << "Dr ";
- }
- if (multipliers & FG_MACH) {
- mult_idx[mult_count] = FG_MACH;
- mult_count++;
- cout << "Mach ";
- }
- if (multipliers & FG_ALTITUDE) {
- mult_idx[mult_count] = FG_ALTITUDE;
- mult_count++;
- cout << "h ";
- }
- if (multipliers & FG_BI2VEL) {
- mult_idx[mult_count] = FG_BI2VEL;
- mult_count++;
- cout << "b /(2*Vt) ";
- }
- if (multipliers & FG_CI2VEL) {
- mult_idx[mult_count] = FG_CI2VEL;
- mult_count++;
- cout << "c /(2*Vt) ";
+ if (type == TABLE) {
+ *AC_cfg >> multparms;
+ if (multparms.substr(0,1) == "F") {
+ LookupR = State->GetParameterIndex(multparms);
+ cout << " Column indexing parameter: " << multparms << endl;
+ } else {
+ LookupC = atoi(multparms.c_str());
+ cout << " Column indexing parameter: " << LookupC << endl;
}
- cout << endl;
+ }
- switch(type) {
- case VALUE:
- coeffDefFile >> StaticValue;
- cout << " Value = " << StaticValue << endl;
- break;
- case VECTOR:
- Allocate(rows,2);
+ // Here, read in the line of the form (e.g.) FG_MACH|FG_QBAR|FG_ALPHA
+ // where each non-dimensionalizing parameter for this coefficient is
+ // separated by a | character
- for (r=1;r<=rows;r++) {
- coeffDefFile >> Table3D[r][0];
- coeffDefFile >> Table3D[r][1];
- }
+ *AC_cfg >> multparms;
- for (r=0;r<=rows;r++) {
- cout << " ";
- for (c=0;c<=columns;c++) {
- cout << Table3D[r][c] << " ";
- }
- cout << endl;
- }
+ end = multparms.length();
+ n = multparms.find("|");
+ start = mult_count = multipliers = 0;
- break;
- case TABLE:
- Allocate(rows, columns);
-
- Table3D[0][0] = 0.0;
- for (c=1;c<=columns;c++) {
- coeffDefFile >> Table3D[0][c];
- for (r=1;r<=rows;r++) {
- if ( c==1 ) coeffDefFile >> Table3D[r][0];
- else coeffDefFile >> ftrashcan;
- coeffDefFile >> Table3D[r][c];
- }
+ while(n < end && n >= 0) {
+ n -= start;
+ mult_idx[mult_count] = State->GetParameterIndex(multparms.substr(start,n));
+ multipliers += mult_idx[mult_count];
+ mult_count++;
+ start += n+1;
+ n = multparms.find("|",start);
+ }
+ mult_idx[mult_count] = State->GetParameterIndex(multparms.substr(start,n));
+ multipliers += mult_idx[mult_count];
+ mult_count++;
+
+ // End of non-dimensionalizing parameter read-in
+
+ switch(type) {
+ case VALUE:
+ *AC_cfg >> StaticValue;
+ cout << " Value = " << StaticValue << endl;
+ break;
+ case VECTOR:
+ Allocate(rows,2);
+
+ for (r=1;r<=rows;r++) {
+ *AC_cfg >> Table3D[r][0];
+ *AC_cfg >> Table3D[r][1];
+ }
+
+ for (r=1;r<=rows;r++) {
+ cout << " ";
+ for (c=0;c<columns;c++) {
+ cout << Table3D[r][c] << " ";
}
+ cout << endl;
+ }
- for (r=0;r<=rows;r++) {
- cout << " ";
- for (c=0;c<=columns;c++) {
- cout << Table3D[r][c] << " ";
- }
- cout << endl;
+ break;
+ case TABLE:
+ Allocate(rows, columns);
+
+ Table3D[0][0] = 0.0;
+ for (c=1;c<=columns;c++) {
+ *AC_cfg >> Table3D[0][c];
+ for (r=1;r<=rows;r++) {
+ if ( c==1 ) *AC_cfg >> Table3D[r][0];
+ else *AC_cfg >> ftrashcan;
+ *AC_cfg >> Table3D[r][c];
}
+ }
- break;
+ for (r=0;r<=rows;r++) {
+ cout << " ";
+ for (c=0;c<=columns;c++) {
+ cout << Table3D[r][c] << " ";
+ }
+ cout << endl;
}
- } else {
- cerr << "Empty file" << endl;
+
+ break;
}
+ AC_cfg->GetNextConfigLine();
}
}
+/******************************************************************************/
FGCoefficient::~FGCoefficient(void)
{
}
+/******************************************************************************/
bool FGCoefficient::Allocate(int r, int c)
{
return true;
}
+/******************************************************************************/
bool FGCoefficient::Allocate(int n)
{
return true;
}
+/******************************************************************************/
float FGCoefficient::Value(float rVal, float cVal)
{
SD = Value = col1temp + cFactor*(col2temp - col1temp);
for (midx=0;midx<mult_count;midx++) {
- Value *= GetCoeffVal(mult_idx[midx]);
+ Value *= State->GetParameter(mult_idx[midx]);
}
return Value;
}
+/******************************************************************************/
float FGCoefficient::Value(float Val)
{
SD = Value = Factor*(Table3D[r][1] - Table3D[r-1][1]) + Table3D[r-1][1];
for (midx=0;midx<mult_count;midx++) {
- Value *= GetCoeffVal(mult_idx[midx]);
+ Value *= State->GetParameter(mult_idx[midx]);
}
return Value;
}
+/******************************************************************************/
float FGCoefficient::Value(void)
{
float Value;
int midx;
-
+
SD = Value = StaticValue;
for (midx=0;midx<mult_count;midx++) {
- Value *= GetCoeffVal(mult_idx[midx]);
+ Value *= State->GetParameter(mult_idx[midx]);
}
return Value;
}
+/******************************************************************************/
+
float FGCoefficient::TotalValue()
{
switch(type) {
case 1:
return (Value());
case 2:
- return (Value(GetCoeffVal(LookupR)));
+ return (Value(State->GetParameter(LookupR)));
case 3:
- return (Value(GetCoeffVal(LookupR),GetCoeffVal(LookupC)));
+ return (Value(State->GetParameter(LookupR),State->GetParameter(LookupC)));
case 4:
return 0.0;
}
return 0;
}
-float FGCoefficient::GetCoeffVal(int val_idx)
-{
- switch(val_idx) {
- case FG_QBAR:
- return State->Getqbar();
- case FG_WINGAREA:
- return Aircraft->GetWingArea();
- case FG_WINGSPAN:
- return Aircraft->GetWingSpan();
- case FG_CBAR:
- return Aircraft->Getcbar();
- case FG_ALPHA:
- return Translation->Getalpha();
- case FG_ALPHADOT:
- return State->Getadot();
- case FG_BETA:
- return Translation->Getbeta();
- case FG_BETADOT:
- return State->Getbdot();
- case FG_PITCHRATE:
- return Rotation->GetQ();
- case FG_ROLLRATE:
- return Rotation->GetP();
- case FG_YAWRATE:
- return Rotation->GetR();
- case FG_ELEVATOR:
- return FCS->GetDe();
- case FG_AILERON:
- return FCS->GetDa();
- case FG_RUDDER:
- return FCS->GetDr();
- case FG_MACH:
- return State->GetMach();
- case FG_ALTITUDE:
- return State->Geth();
- case FG_BI2VEL:
- return Aircraft->GetWingSpan()/(2.0 * State->GetVt());
- case FG_CI2VEL:
- return Aircraft->Getcbar()/(2.0 * State->GetVt());
- }
- return 0;
-}
-
+/******************************************************************************/
void FGCoefficient::DumpSD(void)
{
- cout << " " << name << " = " << SD << endl;
+ cout << " " << name << ": " << SD << endl;
}
+
+/******************************************************************************/
+
#ifdef FGFS
# include <simgear/compiler.h>
# include STL_STRING
-# ifdef FG_HAVE_STD_INCLUDES
-# include <fstream>
-# else
-# include <fstream.h>
-# endif
FG_USING_STD(string);
#else
# include <string>
-# include <fstream>
#endif
#include <map>
+#include "FGConfigFile.h"
+#include "FGDefs.h"
/*******************************************************************************
DEFINES
using namespace std;
-#define FG_QBAR 1
-#define FG_WINGAREA 2
-#define FG_WINGSPAN 4
-#define FG_CBAR 8
-#define FG_ALPHA 16
-#define FG_ALPHADOT 32
-#define FG_BETA 64
-#define FG_BETADOT 128
-#define FG_PITCHRATE 256
-#define FG_ROLLRATE 512
-#define FG_YAWRATE 1024
-#define FG_ELEVATOR 2048
-#define FG_AILERON 4096
-#define FG_RUDDER 8192
-#define FG_MACH 16384
-#define FG_ALTITUDE 32768L
-#define FG_BI2VEL 65536L
-#define FG_CI2VEL 131072L
-
/*******************************************************************************
FORWARD DECLARATIONS
*******************************************************************************/
This class models the stability derivative coefficient lookup tables or
equations. Note that the coefficients need not be calculated each delta-t.
-FG_QBAR 1
-FG_WINGAREA 2
-FG_WINGSPAN 4
-FG_CBAR 8
-FG_ALPHA 16
-FG_ALPHADOT 32
-FG_BETA 64
-FG_BETADOT 128
-FG_PITCHRATE 256
-FG_ROLLRATE 512
-FG_YAWRATE 1024
-FG_ELEVATOR 2048
-FG_AILERON 4096
-FG_RUDDER 8192
-FG_MACH 16384
-FG_ALTITUDE 32768L
-FG_BI2VEL 65536L
-FG_CI2VEL 131072L
+FG_QBAR 1
+FG_WINGAREA 2
+FG_WINGSPAN 4
+FG_CBAR 8
+FG_ALPHA 16
+FG_ALPHADOT 32
+FG_BETA 64
+FG_BETADOT 128
+FG_PITCHRATE 256
+FG_ROLLRATE 512
+FG_YAWRATE 1024
+FG_MACH 2048
+FG_ALTITUDE 4096
+FG_BI2VEL 8192
+FG_CI2VEL 16384
+FG_ELEVATOR_POS 32768L
+FG_AILERON_POS 65536L
+FG_RUDDER_POS 131072L
+FG_SPDBRAKE_POS 262144L
+FG_FLAPS_POS 524288L
+FG_ELEVATOR_CMD 1048576L
+FG_AILERON_CMD 2097152L
+FG_RUDDER_CMD 4194304L
+FG_SPDBRAKE_CMD 8388608L
+FG_FLAPS_CMD 16777216L
+FG_SPARE1 33554432L
+FG_SPARE2 67108864L
+FG_SPARE3 134217728L
+FG_SPARE4 268435456L
+FG_SPARE5 536870912L
+FG_SPARE6 1073741824L
+
+The above definitions are found in FGDefs.h
********************************************************************************
CLASS DECLARATION
class FGCoefficient
{
public:
- FGCoefficient(FGFDMExec*, ifstream&);
+ FGCoefficient(FGFDMExec*, FGConfigFile*);
~FGCoefficient(void);
bool Allocate(int);
bool Allocate(int, int);
float TotalValue(void);
inline float GetSDValue(void) {return SD;}
inline void SetSDValue(float tt) {SD = tt;}
+ inline long int Getmultipliers(void) {return multipliers;}
void DumpSD(void);
enum Type {UNKNOWN, VALUE, VECTOR, TABLE, EQUATION};
protected:
private:
- typedef map<string, long> CoeffMap;
- CoeffMap coeffdef;
+ int numInstances;
string filename;
string description;
string name;
float *Table2D;
float **Table3D;
float LookupR, LookupC;
+ long int multipliers;
long int mult_idx[10];
int rows, columns;
Type type;
- int multipliers;
int mult_count;
float SD; // Actual stability derivative (or other coefficient) value
- float GetCoeffVal(int);
-
FGFDMExec* FDMExec;
FGState* State;
FGAtmosphere* Atmosphere;
--- /dev/null
+/*******************************************************************************
+
+ Header: FGConfigFile.h
+ Author: Jon Berndt
+ Date started: 03/29/00
+ Purpose: Config file read-in class
+ Called by: FGAircraft
+
+FUNCTIONAL DESCRIPTION
+--------------------------------------------------------------------------------
+
+HISTORY
+--------------------------------------------------------------------------------
+03/16/2000 JSB Created
+
+********************************************************************************
+INCLUDES
+*******************************************************************************/
+
+#include "FGConfigFile.h"
+#include <stdlib.h>
+#include <math.h>
+
+/*******************************************************************************
+************************************ CODE **************************************
+*******************************************************************************/
+
+FGConfigFile::FGConfigFile(string cfgFileName)
+{
+ cfgfile.open(cfgFileName.c_str());
+ CommentsOn = false;
+ CurrentIndex = 0;
+ GetNextConfigLine();
+}
+
+
+FGConfigFile::~FGConfigFile(void)
+{
+ cfgfile.close();
+}
+
+
+string FGConfigFile::GetNextConfigLine(void)
+{
+ do {
+ CurrentLine = GetLine();
+ if (CurrentLine.find("<COMMENT>") != CurrentLine.npos) CommentsOn = true;
+ if (CurrentLine.find("</COMMENT>") != CurrentLine.npos) {
+ CommentsOn = false;
+ GetNextConfigLine();
+ }
+ } while (IsCommentLine());
+ if (CurrentLine.length() == 0) GetNextConfigLine();
+ CurrentIndex = 0;
+ return CurrentLine;
+}
+
+
+string FGConfigFile::GetValue(string val)
+{
+ unsigned int pos, p1, p2, ptest;
+
+ if (val == "") { // this call is to return the tag value
+ pos = CurrentLine.find("<");
+ if (pos != CurrentLine.npos) { // beginning brace found "<"
+ p1 = CurrentLine.find_first_not_of(" ",pos+1);
+ if (p1 != CurrentLine.npos) { // found first character of tag
+ p2 = CurrentLine.find_first_of(" >",p1+1);
+ if (p2 == CurrentLine.npos) p2 = p1+1;
+ return CurrentLine.substr(p1,p2-p1);
+ }
+ } else { // beginning brace "<" not found; this is a regular data line
+ pos = CurrentLine.find_first_not_of(" ");
+ if (pos != CurrentLine.npos) { // first character in line found
+ p2 = CurrentLine.find_first_of(" ",pos+1);
+ if (p2 != CurrentLine.npos) {
+ return CurrentLine.substr(pos,p2-pos);
+ } else {
+ return CurrentLine.substr(pos,CurrentLine.length()-pos);
+ }
+ }
+ }
+ } else { // return a value for a specific tag
+ pos = CurrentLine.find(val);
+ if (pos != CurrentLine.npos) {
+ pos = CurrentLine.find("=",pos);
+ if (pos != CurrentLine.npos) {
+ ptest = CurrentLine.find_first_not_of(" ",pos+1);
+ if (ptest != CurrentLine.npos) {
+ p1 = ptest + 1;
+ if (CurrentLine[ptest] == '"') { // quoted
+ p2 = CurrentLine.find_first_of("\"",p1);
+ } else { // not quoted
+ p2 = CurrentLine.find_first_of(" ",p1);
+ }
+ if (p2 != CurrentLine.npos) {
+ return CurrentLine.substr(p1,p2-p1);
+ }
+ }
+ } else { // "=" not found
+ pos = CurrentLine.find(val);
+ pos = CurrentLine.find_first_of(" ",pos+1);
+ ptest = CurrentLine.find_first_not_of(" ",pos+1);
+ if (ptest != CurrentLine.npos) {
+ if (CurrentLine[ptest] == '"') { // quoted
+ p1 = ptest + 1;
+ p2 = CurrentLine.find_first_of("\"",p1);
+ } else { // not quoted
+ p1 = ptest;
+ p2 = CurrentLine.find_first_of(" ",p1);
+ }
+ if (p2 != CurrentLine.npos) {
+ return CurrentLine.substr(p1,p2-p1);
+ } else {
+ p2 = CurrentLine.length();
+ return CurrentLine.substr(p1,p2-p1);
+ }
+ }
+ }
+ }
+ }
+
+ return string("");
+}
+
+
+string FGConfigFile::GetValue(void)
+{
+ return GetValue("");
+}
+
+
+bool FGConfigFile::IsCommentLine(void)
+{
+ if (CurrentLine[0] == '/' && CurrentLine[1] == '/') return true;
+ if (CommentsOn) return true;
+
+ return false;
+}
+
+
+string FGConfigFile::GetLine(void)
+{
+ string scratch = "";
+ unsigned int test;
+
+ while ((test = cfgfile.get()) != EOF) {
+ if (test >= 0x20) {
+ scratch += (char)test;
+ } else {
+ if ((test = cfgfile.get()) != EOF) {
+ if (test >= 0x20) cfgfile.unget();
+ break;
+ }
+ }
+ }
+ if (cfgfile.eof()) return string("EOF");
+ return scratch;
+}
+
+FGConfigFile& FGConfigFile::operator>>(double& val)
+{
+ unsigned int pos, end;
+
+ pos = CurrentLine.find_first_not_of(", ",CurrentIndex);
+ if (pos == CurrentLine.npos) pos = CurrentLine.length();
+ end = CurrentLine.find_first_of(", ",pos+1);
+ if (end == CurrentLine.npos) end = CurrentLine.length();
+ string str = CurrentLine.substr(pos, end - pos);
+ val = strtod(str.c_str(),NULL);
+ CurrentIndex = end+1;
+ // EXPERIMENTAL
+ if (CurrentIndex >= CurrentLine.length())
+ GetNextConfigLine();
+ // END EXPERIMENTAL
+ return *this;
+}
+
+FGConfigFile& FGConfigFile::operator>>(float& val)
+{
+ unsigned int pos, end;
+
+ pos = CurrentLine.find_first_not_of(", ",CurrentIndex);
+ if (pos == CurrentLine.npos) pos = CurrentLine.length();
+ end = CurrentLine.find_first_of(", ",pos+1);
+ if (end == CurrentLine.npos) end = CurrentLine.length();
+ string str = CurrentLine.substr(pos, end - pos);
+ val = strtod(str.c_str(),NULL);
+ CurrentIndex = end+1;
+ // EXPERIMENTAL
+ if (CurrentIndex >= CurrentLine.length())
+ GetNextConfigLine();
+ // END EXPERIMENTAL
+ return *this;
+}
+
+FGConfigFile& FGConfigFile::operator>>(int& val)
+{
+ unsigned int pos, end;
+
+ pos = CurrentLine.find_first_not_of(", ",CurrentIndex);
+ if (pos == CurrentLine.npos) pos = CurrentLine.length();
+ end = CurrentLine.find_first_of(", ",pos+1);
+ if (end == CurrentLine.npos) end = CurrentLine.length();
+ string str = CurrentLine.substr(pos, end - pos);
+ val = atoi(str.c_str());
+ CurrentIndex = end+1;
+ // EXPERIMENTAL
+ if (CurrentIndex >= CurrentLine.length())
+ GetNextConfigLine();
+ // END EXPERIMENTAL
+ return *this;
+}
+
+FGConfigFile& FGConfigFile::operator>>(string& str)
+{
+ unsigned int pos, end;
+
+ pos = CurrentLine.find_first_not_of(", ",CurrentIndex);
+ if (pos == CurrentLine.npos) pos = CurrentLine.length();
+ end = CurrentLine.find_first_of(", ",pos+1);
+ if (end == CurrentLine.npos) end = CurrentLine.length();
+ str = CurrentLine.substr(pos, end - pos);
+ CurrentIndex = end+1;
+ // EXPERIMENTAL
+ if (CurrentIndex >= CurrentLine.length())
+ GetNextConfigLine();
+ // END EXPERIMENTAL
+ return *this;
+}
+
+
+void FGConfigFile::ResetLineIndexToZero(void)
+{
+ CurrentIndex = 0;
+}
+
--- /dev/null
+/*******************************************************************************
+
+ Header: FGConfigFile.h
+ Author: Jon Berndt
+ Date started: 03/29/00
+
+ ------------- Copyright (C) 1999 Jon S. Berndt (jsb@hal-pc.org) -------------
+
+ 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
+--------------------------------------------------------------------------------
+03/29/00 JSB Created
+
+********************************************************************************
+SENTRY
+*******************************************************************************/
+
+#ifndef FGCONFIGFILE_H
+#define FGCONFIGFILE_H
+
+/*******************************************************************************
+INCLUDES
+*******************************************************************************/
+
+#ifdef FGFS
+# include <simgear/compiler.h>
+# include STL_STRING
+# ifdef FG_HAVE_STD_INCLUDES
+# include <fstream>
+# else
+# include <fstream.h>
+# endif
+ FG_USING_STD(string);
+#else
+# include <string>
+# include <fstream>
+#endif
+
+/*******************************************************************************
+DEFINES
+*******************************************************************************/
+
+#ifndef FGFS
+using namespace std;
+#endif
+
+/*******************************************************************************
+CLASS DECLARATION
+*******************************************************************************/
+
+class FGConfigFile
+{
+public:
+ FGConfigFile(string);
+ ~FGConfigFile(void);
+
+ string GetLine(void);
+ string GetNextConfigLine(void);
+ string GetValue(string);
+ string GetValue(void);
+ bool IsCommentLine(void);
+ FGConfigFile& operator>>(double&);
+ FGConfigFile& operator>>(float&);
+ FGConfigFile& operator>>(int&);
+ FGConfigFile& operator>>(string&);
+ void ResetLineIndexToZero(void);
+
+protected:
+
+private:
+ ifstream cfgfile;
+ string CurrentLine;
+ bool CommentsOn;
+ unsigned int CurrentIndex;
+};
+
+/******************************************************************************/
+#endif
// $Log$
+// Revision 1.8 2000/04/24 21:49:06 curt
+// Updated JSBsim code.
+//
+// Revision 1.2 2000/04/15 13:16:54 jsb
+// In good shape, now, changes to Coefficient and aircraft, mostly, with new commands added and inputs and outputs separated.
+//
// Revision 1.7 1999/12/30 17:01:59 curt
// Here is a wrap-up of the latest changes to JSBSim. It still is flaky, but
// much less so due to returning the aero reference point stuff to the config
// $Log$
+// Revision 1.7 2000/04/24 21:49:07 curt
+// Updated JSBsim code.
+//
+// Revision 1.3 2000/04/15 13:16:54 jsb
+// In good shape, now, changes to Coefficient and aircraft, mostly, with new commands added and inputs and outputs separated.
+//
// Revision 1.6 1999/09/07 21:15:45 curt
// Updates to get engine working.
//
#define INVECCENTSQRD 1.0067395
#define INVECCENTSQRDM1 0.0067395
#define EPS 0.081819221
-#define Reng 1716 //Specific Gas Constant,ft^2/(sec^2*R)
-#define SHRATIO 1.4 //Specific Heat Ratio
+#define Reng 1716 //Specific Gas Constant,ft^2/(sec^2*R)
+#define SHRATIO 1.4 //Specific Heat Ratio
#define RADTODEG 57.29578
#define DEGTORAD 1.745329E-2
#define KTSTOFPS 1.68781
#define FPSTOKTS 0.592484
-#define NEEDED_CFG_VERSION 1.10
+#define NEEDED_CFG_VERSION 1.30
#define HPTOFTLBSSEC 550
#define METERS_TO_FEET 3.2808
+#define FG_QBAR 1
+#define FG_WINGAREA 2
+#define FG_WINGSPAN 4
+#define FG_CBAR 8
+#define FG_ALPHA 16
+#define FG_ALPHADOT 32
+#define FG_BETA 64
+#define FG_BETADOT 128
+#define FG_PITCHRATE 256
+#define FG_ROLLRATE 512
+#define FG_YAWRATE 1024
+#define FG_MACH 2048
+#define FG_ALTITUDE 4096
+#define FG_BI2VEL 8192
+#define FG_CI2VEL 16384
+#define FG_ELEVATOR_POS 32768L
+#define FG_AILERON_POS 65536L
+#define FG_RUDDER_POS 131072L
+#define FG_SPDBRAKE_POS 262144L
+#define FG_SPOILERS_POS 524288L
+#define FG_FLAPS_POS 1048576L
+#define FG_ELEVATOR_CMD 2097152L
+#define FG_AILERON_CMD 4194304L
+#define FG_RUDDER_CMD 8388608L
+#define FG_SPDBRAKE_CMD 16777216L
+#define FG_SPOILERS_CMD 33554432L
+#define FG_FLAPS_CMD 67108864L
+#define FG_SPARE3 134217728L
+#define FG_SPARE4 268435456L
+#define FG_SPARE5 536870912L
+#define FG_SPARE6 1073741824L
/******************************************************************************/
#endif
+
Output = FDMExec->GetOutput();
Name = engineName;
-#ifdef MACOS
- fullpath = enginePath + ":" + engineName + ".dat";
-#else
fullpath = enginePath + "/" + engineName + ".dat";
-#endif
ifstream enginefile(fullpath.c_str());
if (enginefile) {
{
float lastThrust;
- Throttle = FCS->GetThrottle(EngineNumber);
+ Throttle = FCS->GetThrottlePos(EngineNumber);
lastThrust = Thrust; // last actual thrust
if (Throttle < MinThrottle || Starved) {
Flameout = true;
} else {
PctPower = Throttle / MaxThrottle;
- Thrust = PctPower*((1.0 - Atmosphere->Getrho() / 0.002378)*(VacThrustMax - SLThrustMax) +
+ Thrust = PctPower*((1.0 - Atmosphere->GetDensityRatio())*(VacThrustMax - SLThrustMax) +
SLThrustMax); // desired thrust
Flameout = false;
}
{
float v,h,pa;
- Throttle = FCS->GetThrottle(EngineNumber);
+ Throttle = FCS->GetThrottlePos(EngineNumber);
Throttle /= 100;
v=State->GetVt();
#include "FGAuxiliary.h"
#include "FGOutput.h"
+#include "filtersjb/FGFilter.h"
+#include "filtersjb/FGDeadBand.h"
+#include "filtersjb/FGGain.h"
+#include "filtersjb/FGGradient.h"
+#include "filtersjb/FGSwitch.h"
+#include "filtersjb/FGSummer.h"
+
/*******************************************************************************
************************************ CODE **************************************
*******************************************************************************/
Name = "FGFCS";
}
+/******************************************************************************/
FGFCS::~FGFCS(void)
{
}
+/******************************************************************************/
bool FGFCS::Run(void)
{
if (!FGModel::Run()) {
-
+
+ for (unsigned int i=0; i<Components.size(); i++) Components[i]->Run();
+
} else {
}
return false;
}
+/******************************************************************************/
+
+void FGFCS::SetThrottleCmd(int engineNum, float setting)
+{
+ if (engineNum < 0) {
+ for (int ctr=0;ctr<Aircraft->GetNumEngines();ctr++) ThrottleCmd[ctr] = setting;
+ } else {
+ ThrottlePos[engineNum] = setting;
+ }
+}
+
+/******************************************************************************/
-void FGFCS::SetThrottle(int engineNum, float setting)
+void FGFCS::SetThrottlePos(int engineNum, float setting)
{
if (engineNum < 0) {
- for (int ctr=0;ctr<Aircraft->GetNumEngines();ctr++) Throttle[ctr] = setting;
+ for (int ctr=0;ctr<Aircraft->GetNumEngines();ctr++)
+ ThrottlePos[ctr] = ThrottleCmd[ctr];
} else {
- Throttle[engineNum] = setting;
+ ThrottlePos[engineNum] = setting;
+ }
+}
+
+/******************************************************************************/
+
+bool FGFCS::LoadFCS(FGConfigFile* AC_cfg)
+{
+ string token;
+
+ FCSName = AC_cfg->GetValue("NAME");
+ AC_cfg->GetNextConfigLine();
+ while ((token = AC_cfg->GetValue()) != "/FLIGHT_CONTROL") {
+ if (token == "COMPONENT") {
+
+ if (((token = AC_cfg->GetValue("TYPE")) == "LAG_FILTER") ||
+ (token == "RECT_LAG_FILTER") ||
+ (token == "LEAD_LAG_FILTER") ||
+ (token == "SECOND_ORDER_FILTER") ||
+ (token == "WASHOUT_FILTER") ||
+ (token == "INTEGRATOR") )
+ {
+ Components.push_back(new FGFilter(this, AC_cfg));
+ } else if ((token == "PURE_GAIN") ||
+ (token == "SCHEDULED_GAIN") ||
+ (token == "AEROSURFACE_SCALE") )
+ {
+ Components.push_back(new FGGain(this, AC_cfg));
+ } else if (token == "SUMMER") {
+ Components.push_back(new FGSummer(this, AC_cfg));
+ } else if (token == "DEADBAND") {
+ Components.push_back(new FGDeadBand(this, AC_cfg));
+ } else if (token == "GRADIENT") {
+ Components.push_back(new FGGradient(this, AC_cfg));
+ } else if (token == "SWITCH") {
+ Components.push_back(new FGSwitch(this, AC_cfg));
+ }
+ AC_cfg->GetNextConfigLine();
+ }
}
+ return true;
+}
+
+/******************************************************************************/
+
+float FGFCS::GetComponentOutput(int idx)
+{
+ return Components[idx]->GetOutput();
+}
+
+/******************************************************************************/
+
+string FGFCS::GetComponentName(int idx)
+{
+ return Components[idx]->GetName();
}
+
INCLUDES
*******************************************************************************/
+#ifdef FGFS
+# include <simgear/compiler.h>
+# include STL_STRING
+ FG_USING_STD(string);
+# ifdef FG_HAVE_STD_INCLUDES
+# include <vector>
+# else
+# include <vector.h>
+# endif
+#else
+# include <vector>
+# include <string>
+#endif
+
+#include "filtersjb/FGFCSComponent.h"
#include "FGModel.h"
+#include "FGConfigFile.h"
+
/*******************************************************************************
CLASS DECLARATION
class FGFCS : public FGModel
{
+private:
+ float DaCmd, DeCmd, DrCmd, DfCmd, DsbCmd, DspCmd;
+ float DaPos, DePos, DrPos, DfPos, DsbPos, DspPos;
+ float ThrottleCmd[MAX_ENGINES];
+ float ThrottlePos[MAX_ENGINES];
+
+ vector <FGFCSComponent*> Components;
+
public:
FGFCS(FGFDMExec*);
~FGFCS(void);
bool Run(void);
-
- inline float GetDa(void) {return Da;}
- inline float GetDe(void) {return De;}
- inline float GetDr(void) {return Dr;}
- inline float GetDf(void) {return Df;}
- inline float GetDs(void) {return Ds;}
- inline float GetThrottle(int ii) {return Throttle[ii];}
-
- inline void SetDa(float tt) {Da = tt*0.30;}
- inline void SetDe(float tt) {De = tt*0.60;}
- inline void SetDr(float tt) {Dr = -1*tt*0.50;}
- inline void SetDf(float tt) {Df = tt;}
- inline void SetDs(float tt) {Ds = tt;}
- void SetThrottle(int ii, float tt);
-
-protected:
-private:
- float Da, De, Dr, Df, Ds;
- float Throttle[MAX_ENGINES];
+ inline float GetDaCmd(void) {return DaCmd;}
+ inline float GetDeCmd(void) {return DeCmd;}
+ inline float GetDrCmd(void) {return DrCmd;}
+ inline float GetDfCmd(void) {return DfCmd;}
+ inline float GetDsbCmd(void) {return DsbCmd;}
+ inline float GetDspCmd(void) {return DspCmd;}
+ inline float GetThrottleCmd(int ii) {return ThrottleCmd[ii];}
+
+ inline float GetDaPos(void) {return DaPos;}
+ inline float GetDePos(void) {return DePos;}
+ inline float GetDrPos(void) {return DrPos;}
+ inline float GetDfPos(void) {return DfPos;}
+ inline float GetDsbPos(void) {return DsbPos;}
+ inline float GetDspPos(void) {return DspPos;}
+ inline float GetThrottlePos(int ii) {return ThrottlePos[ii];}
+
+ inline FGState* GetState(void) {return State;}
+ float GetComponentOutput(int idx);
+ string GetComponentName(int idx);
+
+ inline void SetDaCmd(float tt) {DaCmd = tt;}
+ inline void SetDeCmd(float tt) {DeCmd = tt;}
+ inline void SetDrCmd(float tt) {DrCmd = tt;}
+ inline void SetDfCmd(float tt) {DfCmd = tt;}
+ inline void SetDsbCmd(float tt) {DsbCmd = tt;}
+ inline void SetDspCmd(float tt) {DspCmd = tt;}
+ void SetThrottleCmd(int ii, float tt);
+
+ inline void SetDaPos(float tt) {DaPos = tt;}
+ inline void SetDePos(float tt) {DePos = tt;}
+ inline void SetDrPos(float tt) {DrPos = tt;}
+ inline void SetDfPos(float tt) {DfPos = tt;}
+ inline void SetDsbPos(float tt) {DsbPos = tt;}
+ inline void SetDspPos(float tt) {DspPos = tt;}
+ void SetThrottlePos(int ii, float tt);
+
+ bool LoadFCS(FGConfigFile* AC_cfg);
+ string FCSName;
};
-/******************************************************************************/
+#include "FGState.h"
+
#endif
Schedule(Translation, 1);
Schedule(Position, 1);
Schedule(Auxiliary, 1);
- Schedule(Output, 60);
+ Schedule(Output, 1);
terminate = false;
frozen = false;
11/17/98 JSB Created
7/31/99 TP Added RunIC function that runs the sim so that every frame
begins with the IC values from the given FGInitialCondition
- object and dt=0.
+ object and dt=0.
********************************************************************************
SENTRY
class FGFDMExec
{
public:
- FGFDMExec(void);
- ~FGFDMExec(void);
+ FGFDMExec::FGFDMExec(void);
+ FGFDMExec::~FGFDMExec(void);
FGModel* FirstModel;
FGInitialCondition::~FGInitialCondition(void) {};
-void FGInitialCondition::SetVcalibratedKtsIC(float tt)
+/* void FGInitialCondition::SetVcalibratedKtsIC(float tt)
{
vc=tt*KTSTOFPS;
- cout << "ic.vc: " << vc << endl;
- cout << "ic.rhosl: " << atm->GetDensity(0) << endl;
- cout << "ic.rho: " << atm->GetDensity(altitude) << endl;
- vt=sqrt(atm->GetDensity(0)/atm->GetDensity(altitude)*vc*vc);
- cout << "ic.vt: " << vt << endl;
+
+ vt=sqrt(atm->GetDensity(0)/atm->GetDensity(altitude)*vc*vc);
+
//mach=vt*sqrt(SHRATIO*Reng*atm->GetTemperature(altitude));
}
+ */
void FGInitialCondition::SetVtrueKtsIC(float tt)
}
-void FGInitialCondition::SetMachIC(float tt)
+/* void FGInitialCondition::SetMachIC(float tt)
{
mach=tt;
vt=mach*sqrt(SHRATIO*Reng*atm->GetTemperature(altitude));
//vc=sqrt(atm->GetDensity(altitude)/atm->GetDensity(0)*vt*vt);
-}
+} */
+
void FGInitialCondition::SetAltitudeFtIC(float tt)
FGInitialCondition(FGFDMExec *fdmex);
~FGInitialCondition(void);
- void SetVcalibratedKtsIC(float tt);
+ /* void SetVcalibratedKtsIC(float tt); */
+
void SetVtrueKtsIC(float tt);
- void SetMachIC(float tt);
+ /* void SetMachIC(float tt); */
+
void SetAltitudeFtIC(float tt);
void SetFlightPathAngleDegIC(float tt); //"vertical" flight path, solve for alpha using speed
//void SetClimbRateFpmIC(float tt); //overwrite gamma
*******************************************************************************/
-FGLGear::FGLGear(ifstream& acfile)
+FGLGear::FGLGear(FGConfigFile* AC_cfg)
{
- acfile >> name >> X >> Y >> Z >> kSpring >> bDamp >> statFCoeff >> brakeCoeff;
+ string tmp;
+ *AC_cfg >> tmp >> name >> X >> Y >> Z >> kSpring >> bDamp >> statFCoeff >> brakeCoeff;
}
********************************************************************************
[1] Cooke, Zyda, Pratt, and McGhee, "NPSNET: Flight Simulation Dynamic Modeling
- Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
- School, January 1994
+ Using Quaternions", Presence, Vol. 1, No. 4, pp. 404-420 Naval Postgraduate
+ School, January 1994
[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices",
- JSC 12960, July 1977
+ JSC 12960, July 1977
[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at
- NASA-Ames", NASA CR-2497, January 1975
+ NASA-Ames", NASA CR-2497, January 1975
[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
- Wiley & Sons, 1979 ISBN 0-471-03032-5
+ Wiley & Sons, 1979 ISBN 0-471-03032-5
[5] Bernard Etkin, "Dynamics of Flight, Stability and Control", Wiley & Sons,
- 1982 ISBN 0-471-08936-2
+ 1982 ISBN 0-471-08936-2
********************************************************************************
INCLUDES
#ifdef FGFS
# include <simgear/compiler.h>
# include STL_STRING
-# ifdef FG_HAVE_STD_INCLUDES
-# include <fstream>
-# else
-# include <fstream.h>
-# endif
FG_USING_STD(string);
#else
-# include <fstream>
# include <string>
#endif
+#include "FGConfigFile.h"
+
/*******************************************************************************
DEFINITIONS
*******************************************************************************/
class FGLGear
{
public:
- FGLGear(ifstream&);
+ FGLGear(FGConfigFile*);
~FGLGear(void);
float Force(void);
/*******************************************************************************
Module: FGMatrix.cpp
-Author: Tony Peden [formatted here by JSB]
-Date started: ??
+Author: Originally by Tony Peden [formatted here (and broken??) by JSB]
+Date started: 1998
Purpose: FGMatrix class
Called by: Various
FUNCTIONAL DESCRIPTION
--------------------------------------------------------------------------------
-
-ARGUMENTS
---------------------------------------------------------------------------------
-
-
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
********************************************************************************
INCLUDES
*******************************************************************************/
-#include <stdlib.h>
#include "FGMatrix.h"
-#include <iostream.h>
-#include <iomanip.h>
-#include <fstream.h>
-
-/*******************************************************************************
-DEFINES
-*******************************************************************************/
-
-#pragma warn -use
-
-/*******************************************************************************
-CONSTANTS
-*******************************************************************************/
-
-
-/*******************************************************************************
-TYPEDEFS
-*******************************************************************************/
-
-
-/*******************************************************************************
-GLOBALS
-*******************************************************************************/
-
/*******************************************************************************
************************************ CODE **************************************
*******************************************************************************/
-double** alloc(int rows,int cols)
+double** FGalloc(int rows, int cols)
{
double **A;
A = new double *[rows+1];
- if (!A) return NULL;
+ if (!A) return NULL;
- for (int i=0;i<=rows;i++){
- A[i]=new double[cols+1];
+ for (int i=0; i <= rows; i++){
+ A[i] = new double [cols+1];
if (!A[i]) return NULL;
}
return A;
}
+/******************************************************************************/
-void dealloc(double **A, int rows, int cols)
+void dealloc(double **A, int rows)
{
- for(int i=0;i<=rows;i++){
- delete[] A[i];
- }
-
+ for(int i=0;i<= rows;i++) delete[] A[i];
delete[] A;
}
+/******************************************************************************/
-FGMatrix::FGMatrix(unsigned rows, unsigned cols)
+FGMatrix::FGMatrix(const unsigned int r, const unsigned int c) : rows(r), cols(c)
{
- this->rows=rows;
- this->cols=cols;
- keep=false;
- data=alloc(rows,cols);
+ data = FGalloc(rows,cols);
+ rowCtr = colCtr = 1;
}
+/******************************************************************************/
-FGMatrix::FGMatrix(const FGMatrix& A)
+FGMatrix::FGMatrix(const FGMatrix& M)
{
- data=NULL;
- *this=A;
+ data = NULL;
+ rowCtr = colCtr = 1;
+ *this = M;
}
+/******************************************************************************/
FGMatrix::~FGMatrix(void)
{
- if (keep == false) {
- dealloc(data,rows,cols);
- rows=cols=0;
- }
-}
-
-
-FGMatrix& FGMatrix::operator=(const FGMatrix& A)
-{
- if (&A != this) {
- if (data != NULL) dealloc(data,rows,cols);
-
- width = A.width;
- prec = A.prec;
- delim = A.delim;
- origin = A.origin;
- rows = A.rows;
- cols = A.cols;
- keep = false;
-
- if (A.keep == true) {
- data = A.data;
- } else {
- data = alloc(rows,cols);
- for (unsigned int i=0; i<=rows; i++) {
- for (unsigned int j=0; j<=cols; j++) {
- data[i][j] = A.data[i][j];
- }
- }
+ dealloc(data,rows);
+ rowCtr = colCtr = 1;
+ rows = cols = 0;
+}
+
+/******************************************************************************/
+
+ostream& operator<<(ostream& os, const FGMatrix& M)
+{
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ if (i == M.Rows() && j == M.Cols())
+ os << M.data[i][j];
+ else
+ os << M.data[i][j] << ", ";
}
}
+ return os;
+}
+
+/******************************************************************************/
+
+FGMatrix& FGMatrix::operator<<(const float ff)
+{
+ data[rowCtr][colCtr] = ff;
+ if (++colCtr > Cols()) {
+ colCtr = 1;
+ if (++rowCtr > Rows())
+ rowCtr = 1;
+ }
return *this;
}
+/******************************************************************************/
-double& FGMatrix::operator()(unsigned row, unsigned col)
+istream& operator>>(istream& is, FGMatrix& M)
{
- return data[row][col];
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ is >> M.data[i][j];
+ }
+ }
+ return is;
}
+/******************************************************************************/
-unsigned FGMatrix::Rows(void) const
+FGMatrix& FGMatrix::operator=(const FGMatrix& M)
+{
+ if (&M != this) {
+ if (data != NULL) dealloc(data,rows);
+
+ width = M.width;
+ prec = M.prec;
+ delim = M.delim;
+ origin = M.origin;
+ rows = M.rows;
+ cols = M.cols;
+
+ data = FGalloc(rows,cols);
+ for (unsigned int i=0; i<=rows; i++) {
+ for (unsigned int j=0; j<=cols; j++) {
+ data[i][j] = M.data[i][j];
+ }
+ }
+ }
+ return *this;
+}
+
+/******************************************************************************/
+
+unsigned int FGMatrix::Rows(void) const
{
return rows;
}
+/******************************************************************************/
-unsigned FGMatrix::Cols(void) const
+unsigned int FGMatrix::Cols(void) const
{
return cols;
}
+/******************************************************************************/
void FGMatrix::SetOParams(char delim,int width,int prec,int origin)
{
- FGMatrix::delim=delim;
- FGMatrix::width=width;
- FGMatrix::prec=prec;
- FGMatrix::origin=origin;
+ FGMatrix::delim = delim;
+ FGMatrix::width = width;
+ FGMatrix::prec = prec;
+ FGMatrix::origin = origin;
}
+/******************************************************************************/
void FGMatrix::InitMatrix(double value)
{
if (data) {
for (unsigned int i=0;i<=rows;i++) {
- for (unsigned int j=0;j<=cols;j++) {
- operator()(i,j) = value;
- }
- }
- }
+ for (unsigned int j=0;j<=cols;j++) {
+ operator()(i,j) = value;
+ }
+ }
+ }
}
+/******************************************************************************/
void FGMatrix::InitMatrix(void)
{
- this->InitMatrix(0);
+ this->InitMatrix(0);
}
+// *****************************************************************************
+// binary operators ************************************************************
+// *****************************************************************************
-FGMatrix operator-(FGMatrix& A, FGMatrix& B)
+FGMatrix FGMatrix::operator-(const FGMatrix& M)
{
- if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
- cout << endl << "FGMatrix::operator-" << endl << '\t';
- cout << "Subtraction not defined for matrices of different sizes";
- cout << endl;
- exit(1);
- }
+ if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator -";
+ throw mE;
+ }
- FGMatrix Diff(A.Rows(),A.Cols());
- Diff.keep=true;
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- Diff(i,j)=A(i,j)-B(i,j);
- }
- }
- return Diff;
+ FGMatrix Diff(Rows(), Cols());
+
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ Diff(i,j) = (*this)(i,j) - M(i,j);
+ }
+ }
+ return Diff;
}
+/******************************************************************************/
-void operator-=(FGMatrix &A,FGMatrix &B)
+void FGMatrix::operator-=(const FGMatrix &M)
{
- if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
- cout << endl << "FGMatrix::operator-" << endl << '\t';
- cout << "Subtraction not defined for matrices of different sizes";
- cout << endl;
- exit(1);
- }
+ if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator -=";
+ throw mE;
+ }
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- A(i,j)-=B(i,j);
- }
- }
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ (*this)(i,j) -= M(i,j);
+ }
+ }
}
+/******************************************************************************/
-FGMatrix operator+(FGMatrix& A, FGMatrix& B)
+FGMatrix FGMatrix::operator+(const FGMatrix& M)
{
- if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
- cout << endl << "FGMatrix::operator+" << endl << '\t';
- cout << "Addition not defined for matrices of different sizes";
- cout << endl;
- exit(1);
- }
+ if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator +";
+ throw mE;
+ }
+
+ FGMatrix Sum(Rows(), Cols());
- FGMatrix Sum(A.Rows(),A.Cols());
- Sum.keep = true;
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- Sum(i,j)=A(i,j)+B(i,j);
- }
- }
- return Sum;
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ Sum(i,j) = (*this)(i,j) + M(i,j);
+ }
+ }
+ return Sum;
}
+/******************************************************************************/
-void operator+=(FGMatrix &A,FGMatrix &B)
+void FGMatrix::operator+=(const FGMatrix &M)
{
- if ((A.Rows() != B.Rows()) || (A.Cols() != B.Cols())) {
- cout << endl << "FGMatrix::operator+" << endl << '\t';
- cout << "Addition not defined for matrices of different sizes";
- cout << endl;
- exit(1);
- }
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- A(i,j)+=B(i,j);
- }
- }
+ if ((Rows() != M.Rows()) || (Cols() != M.Cols())) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator +=";
+ throw mE;
+ }
+
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ (*this)(i,j)+=M(i,j);
+ }
+ }
}
+/******************************************************************************/
-FGMatrix operator*(double scalar,FGMatrix &A)
+FGMatrix operator*(double scalar, FGMatrix &M)
{
- FGMatrix Product(A.Rows(),A.Cols());
- Product.keep = true;
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- Product(i,j) = scalar*A(i,j);
+ FGMatrix Product(M.Rows(), M.Cols());
+
+ for (unsigned int i=1; i<=M.Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ Product(i,j) = scalar*M(i,j);
}
- }
- return Product;
+ }
+ return Product;
}
+/******************************************************************************/
-void operator*=(FGMatrix &A,double scalar)
+void FGMatrix::operator*=(const double scalar)
{
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- A(i,j)*=scalar;
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ (*this)(i,j) *= scalar;
}
}
}
+/******************************************************************************/
-FGMatrix operator*(FGMatrix &Left, FGMatrix &Right)
+FGMatrix FGMatrix::operator*(const FGMatrix& M)
{
- if (Left.Cols() != Right.Rows()) {
- cout << endl << "FGMatrix::operator*" << endl << '\t';
- cout << "The number of rows in the right matrix must match the number";
- cout << endl << '\t' << "of columns in the left." << endl;
- cout << '\t' << "Multiplication not defined." << endl;
- exit(1);
- }
+ if (Cols() != M.Rows()) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator *";
+ throw mE;
+ }
- FGMatrix Product(Left.Rows(),Right.Cols());
- Product.keep = true;
- for (unsigned int i=1;i<=Left.Rows();i++) {
- for (unsigned int j=1;j<=Right.Cols();j++) {
- Product(i,j) = 0;
- for (unsigned int k=1;k<=Left.Cols();k++) {
- Product(i,j)+=Left(i,k)*Right(k,j);
+ FGMatrix Product(Rows(), M.Cols());
+
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ Product(i,j) = 0;
+ for (unsigned int k=1; k<=Cols(); k++) {
+ Product(i,j) += (*this)(i,k) * M(k,j);
}
- }
- }
- return Product;
+ }
+ }
+ return Product;
}
+/******************************************************************************/
-void operator*=(FGMatrix &Left,FGMatrix &Right)
+void FGMatrix::operator*=(const FGMatrix& M)
{
- if (Left.Cols() != Right.Rows()) {
- cout << endl << "FGMatrix::operator*" << endl << '\t';
- cout << "The number of rows in the right matrix must match the number";
- cout << endl << '\t' << "of columns in the left." << endl;
- cout << '\t' << "Multiplication not defined." << endl;
- exit(1);
- }
+ if (Cols() != M.Rows()) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Matrix operator *=";
+ throw mE;
+ }
- double **prod;
+ double **prod;
- prod=alloc(Left.Rows(),Right.Cols());
- for (unsigned int i=1;i<=Left.Rows();i++) {
- for (unsigned int j=1;j<=Right.Cols();j++) {
- prod[i][j] = 0;
- for (unsigned int k=1;k<=Left.Cols();k++) {
- prod[i][j]+=Left(i,k)*Right(k,j);
- }
- }
- }
- dealloc(Left.data,Left.Rows(),Left.Cols());
- Left.data=prod;
- Left.cols=Right.cols;
+ prod = FGalloc(Rows(), M.Cols());
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=M.Cols(); j++) {
+ prod[i][j] = 0;
+ for (unsigned int k=1; k<=Cols(); k++) {
+ prod[i][j] += (*this)(i,k) * M(k,j);
+ }
+ }
+ }
+ dealloc(data, Rows());
+ data = prod;
+ cols = M.cols;
}
+/******************************************************************************/
-FGMatrix operator/(FGMatrix& A, double scalar)
+FGMatrix FGMatrix::operator/(const double scalar)
{
- FGMatrix Quot(A.Rows(),A.Cols());
- A.keep = true;
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- Quot(i,j)=A(i,j)/scalar;
- }
- }
- return Quot;
+ FGMatrix Quot(Rows(), Cols());
+
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ Quot(i,j) = (*this)(i,j)/scalar;
+ }
+ }
+ return Quot;
}
+/******************************************************************************/
-void operator/=(FGMatrix &A,double scalar)
+void FGMatrix::operator/=(const double scalar)
{
- for (unsigned int i=1;i<=A.Rows();i++) {
- for (unsigned int j=1;j<=A.Cols();j++) {
- A(i,j)/=scalar;
- }
- }
+ for (unsigned int i=1; i<=Rows(); i++) {
+ for (unsigned int j=1; j<=Cols(); j++) {
+ (*this)(i,j)/=scalar;
+ }
+ }
}
+/******************************************************************************/
void FGMatrix::T(void)
{
- if (rows==cols)
- TransposeSquare();
- else
- TransposeNonSquare();
+ if (rows==cols)
+ TransposeSquare();
+ else
+ TransposeNonSquare();
+}
+
+/******************************************************************************/
+
+FGColumnVector FGMatrix::operator*(const FGColumnVector& Col)
+{
+ FGColumnVector Product(Col.Rows());
+
+ if (Cols() != Col.Rows()) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Column Vector operator *";
+ throw mE;
+ }
+
+ for (unsigned int i=1;i<=Rows();i++) {
+ Product(i) = 0.00;
+ for (unsigned int j=1;j<=Cols();j++) {
+ Product(i) += Col(j)*data[i][j];
+ }
+ }
+ return Product;
}
+/******************************************************************************/
void FGMatrix::TransposeSquare(void)
{
- for (unsigned int i=1;i<=rows;i++) {
- for (unsigned int j=i+1;j<=cols;j++) {
- double tmp=data[i][j];
- data[i][j]=data[j][i];
- data[j][i]=tmp;
- }
- }
+ for (unsigned int i=1; i<=rows; i++) {
+ for (unsigned int j=i+1; j<=cols; j++) {
+ double tmp = data[i][j];
+ data[i][j] = data[j][i];
+ data[j][i] = tmp;
+ }
+ }
}
+/******************************************************************************/
void FGMatrix::TransposeNonSquare(void)
{
- double **tran;
+ double **tran;
- tran=alloc(rows,cols);
- for (unsigned int i=1;i<=rows;i++) {
- for (unsigned int j=1;j<=cols;j++) {
- tran[j][i]=data[i][j];
- }
- }
- dealloc(data,rows,cols);
+ tran = FGalloc(rows,cols);
- data=tran;
- unsigned m=rows;
- rows=cols;
- cols=m;
+ for (unsigned int i=1; i<=rows; i++) {
+ for (unsigned int j=1; j<=cols; j++) {
+ tran[j][i] = data[i][j];
+ }
+ }
+
+ dealloc(data,rows);
+
+ data = tran;
+ unsigned int m = rows;
+ rows = cols;
+ cols = m;
}
+/******************************************************************************/
FGColumnVector::FGColumnVector(void):FGMatrix(3,1) { }
FGColumnVector::FGColumnVector(int m):FGMatrix(m,1) { }
-FGColumnVector::FGColumnVector(FGColumnVector& b):FGMatrix(b) { }
+FGColumnVector::FGColumnVector(const FGColumnVector& b):FGMatrix(b) { }
FGColumnVector::~FGColumnVector() { }
-double& FGColumnVector::operator()(int m)
+
+/******************************************************************************/
+
+double& FGColumnVector::operator()(int m) const
+{
+ return FGMatrix::operator()(m,1);
+}
+
+/******************************************************************************/
+
+FGColumnVector operator*(const FGMatrix& Mat, const FGColumnVector& Col)
+{
+ FGColumnVector Product(Col.Rows());
+
+ if (Mat.Cols() != Col.Rows()) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Column Vector operator *";
+ throw mE;
+ }
+
+ for (unsigned int i=1;i<=Mat.Rows();i++) {
+ Product(i) = 0.00;
+ for (unsigned int j=1;j<=Mat.Cols();j++) {
+ Product(i) += Col(j)*Mat(i,j);
+ }
+ }
+
+ return Product;
+}
+
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::operator+(const FGColumnVector& C)
+{
+ if (Rows() != C.Rows()) {
+ MatrixException mE;
+ mE.Message = "Invalid row/column match in Column Vector operator *";
+ throw mE;
+ }
+
+ FGColumnVector Sum(C.Rows());
+
+ for (unsigned int i=1; i<=C.Rows(); i++) {
+ Sum(i) = C(i) + data[i][1];
+ }
+
+ return Sum;
+}
+
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::operator*(const double scalar)
+{
+ FGColumnVector Product(Rows());
+
+ for (unsigned int i=1; i<=Rows(); i++) Product(i) = scalar * data[i][1];
+
+ return Product;
+}
+
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::operator/(const double scalar)
+{
+ FGColumnVector Quotient(Rows());
+
+ for (unsigned int i=1; i<=Rows(); i++) Quotient(i) = data[i][1] / scalar;
+
+ return Quotient;
+}
+
+/******************************************************************************/
+
+FGColumnVector operator*(const double scalar, const FGColumnVector& C)
+{
+ FGColumnVector Product(C.Rows());
+
+ for (unsigned int i=1; i<=C.Rows(); i++) {
+ Product(i) = scalar * C(i);
+ }
+
+ return Product;
+}
+
+/******************************************************************************/
+float FGColumnVector::Magnitude(void)
+{
+ double num=0.0;
+
+ if ((data[1][1] == 0.00) &&
+ (data[2][1] == 0.00) &&
+ (data[3][1] == 0.00))
+ {
+ return 0.00;
+ } else {
+ for (unsigned int i = 1; i<=Rows(); i++) num += data[i][1]*data[i][1];
+ return sqrt(num);
+ }
+}
+
+/******************************************************************************/
+
+FGColumnVector FGColumnVector::Normalize(void)
{
- return FGMatrix::operator()(m,1);
+ return *this/Magnitude();
}
/*******************************************************************************
Header: FGMatrix.h
-Author: Tony Peden [formatted here by Jon Berndt]
+Author: Originally by Tony Peden [formatted and adapted here by Jon Berndt]
Date started: Unknown
HISTORY
--------------------------------------------------------------------------------
??/??/?? TP Created
+03/16/2000 JSB Added exception throwing
-/*******************************************************************************
+********************************************************************************
SENTRY
*******************************************************************************/
*******************************************************************************/
#include <stdlib.h>
-#include <iostream.h>
-#include <fstream.h>
+#ifdef FGFS
+# include <simgear/compiler.h>
+# include STL_STRING
+# ifdef FG_HAVE_STD_INCLUDES
+# include <fstream>
+# else
+# include <fstream.h>
+# endif
+ FG_USING_STD(string);
+#else
+# include <string>
+# include <fstream>
+#endif
/*******************************************************************************
-DEFINES
+FORWARD DECLARATIONS
*******************************************************************************/
+class FGColumnVector;
/*******************************************************************************
-CONSTANTS
+DECLARATION: MatrixException
*******************************************************************************/
+using namespace std;
-/*******************************************************************************
-TYPEDEFS
-*******************************************************************************/
-
+class MatrixException /* : public exception */
+{
+public:
+ string Message;
+};
/*******************************************************************************
-GLOBALS
+DECLARATION: FGMatrix
*******************************************************************************/
class FGMatrix
{
-private:
+protected:
double **data;
- unsigned rows,cols;
- bool keep;
+
+private:
+ unsigned int rows,cols;
char delim;
int width,prec,origin;
void TransposeSquare(void);
void TransposeNonSquare(void);
+ unsigned int rowCtr, colCtr;
public:
- FGMatrix(unsigned rows, unsigned cols);
+ FGMatrix(unsigned int r, unsigned int c);
FGMatrix(const FGMatrix& A);
~FGMatrix(void);
- FGMatrix& FGMatrix::operator=(const FGMatrix& A);
- double& FGMatrix::operator()(unsigned row, unsigned col);
+ FGMatrix& operator=(const FGMatrix& A);
+ inline double& operator()(unsigned int row, unsigned int col) const {return data[row][col];}
- unsigned FGMatrix::Rows(void) const;
- unsigned FGMatrix::Cols(void) const;
+ FGColumnVector operator*(const FGColumnVector& Col);
- void FGMatrix::T(void);
+ unsigned int Rows(void) const;
+ unsigned int Cols(void) const;
+
+ void T(void);
void InitMatrix(void);
void InitMatrix(double value);
- friend FGMatrix operator-(FGMatrix& A, FGMatrix& B);
- friend FGMatrix operator+(FGMatrix& A, FGMatrix& B);
- friend FGMatrix operator*(double scalar,FGMatrix& A);
- friend FGMatrix operator*(FGMatrix& Left, FGMatrix& Right);
- friend FGMatrix operator/(FGMatrix& A, double scalar);
+ FGMatrix operator-(const FGMatrix& B);
+ FGMatrix operator+(const FGMatrix& B);
+ FGMatrix operator*(const FGMatrix& B);
+ FGMatrix operator/(const double scalar);
+ FGMatrix& operator<<(const float ff);
+
+ friend ostream& operator<<(ostream& os, const FGMatrix& M);
+ friend istream& operator>>(istream& is, FGMatrix& M);
- friend void operator-=(FGMatrix &A,FGMatrix &B);
- friend void operator+=(FGMatrix &A,FGMatrix &B);
- friend void operator*=(FGMatrix &A,FGMatrix &B);
- friend void operator*=(FGMatrix &A,double scalar);
- friend void operator/=(FGMatrix &A,double scalar);
+ void operator-=(const FGMatrix &B);
+ void operator+=(const FGMatrix &B);
+ void operator*=(const FGMatrix &B);
+ void operator*=(const double scalar);
+ void operator/=(const double scalar);
+
+ friend FGMatrix operator*(double scalar,FGMatrix& A);
void SetOParams(char delim,int width,int prec, int origin=0);
};
+/*******************************************************************************
+DECLARATION: FGColumnVector
+*******************************************************************************/
+
class FGColumnVector : public FGMatrix
{
public:
FGColumnVector(void);
FGColumnVector(int m);
- FGColumnVector(FGColumnVector& b);
+ FGColumnVector(const FGColumnVector& b);
~FGColumnVector();
- double& operator()(int m);
+ FGColumnVector operator*(const double scalar);
+ FGColumnVector operator/(const double scalar);
+ FGColumnVector operator+(const FGColumnVector& B);
+ float Magnitude(void);
+ FGColumnVector Normalize(void);
+
+ friend FGColumnVector operator*(const double scalar, const FGColumnVector& A);
+
+ double& operator()(int m) const;
};
/******************************************************************************/
void SetRate(int tt) {rate = tt;};
protected:
+ enum {eU=1, eV, eW};
+ enum {eNorth=1, eEast, eDown};
+ enum {eP=1, eQ, eR};
+ enum {eL=1, eM, eN};
+ enum {eX=1, eY, eZ};
+ enum {ePhi=1, eTht, ePsi};
+
int exe_ctr;
int rate;
#endif
}
+/******************************************************************************/
FGOutput::~FGOutput(void)
{
if (socket) delete socket;
}
+/******************************************************************************/
bool FGOutput::Run(void)
{
return false;
}
+/******************************************************************************/
void FGOutput::DelimitedOutput(void)
{
if (dFirstPass) {
cout << "Time,";
- cout << "Altitude,";
- cout << "Phi,";
- cout << "Tht,";
- cout << "Psi,";
- cout << "Rho,";
- cout << "Vtotal,";
- cout << "U,";
- cout << "V,";
- cout << "W,";
- cout << "Vn,";
- cout << "Ve,";
- cout << "Vd,";
- cout << "Udot,";
- cout << "Vdot,";
- cout << "Wdot,";
- cout << "P,";
- cout << "Q,";
- cout << "R,";
- cout << "PDot,";
- cout << "QDot,";
- cout << "RDot,";
- cout << "Fx,";
- cout << "Fy,";
- cout << "Fz,";
- cout << "Latitude,";
- cout << "Longitude,";
cout << "QBar,";
- cout << "Alpha,";
- cout << "L,";
- cout << "M,";
- cout << "N,";
+ cout << "Vtotal,";
cout << "Throttle,";
cout << "Aileron,";
cout << "Elevator,";
cout << "Rudder,";
+ cout << "Rho,";
cout << "Ixx,";
cout << "Iyy,";
cout << "Izz,";
cout << "Ixz,";
cout << "Mass,";
- cout << "X CG";
+ cout << "Xcg, Ycg, Zcg, ";
+ cout << "Xforce, Yforce, Zforce,";
+ cout << "L, M, N, ";
+ cout << "Altitude,";
+ cout << "Phi, Tht, Psi,";
+ cout << "P, Q, R, ";
+ cout << "U, V, W, ";
+ cout << "Alpha,";
+ cout << "Vn, Ve, Vd, ";
+ cout << "Latitude,";
+ cout << "Longitude";
cout << endl;
dFirstPass = false;
}
cout << State->Getsim_time() << ",";
- cout << State->Geth() << ",";
- cout << Rotation->Getphi() << ",";
- cout << Rotation->Gettht() << ",";
- cout << Rotation->Getpsi() << ",";
- cout << Atmosphere->GetDensity() << ",";
- cout << State->GetVt() << ",";
- cout << Translation->GetU() << ",";
- cout << Translation->GetV() << ",";
- cout << Translation->GetW() << ",";
- cout << Position->GetVn() << ",";
- cout << Position->GetVe() << ",";
- cout << Position->GetVd() << ",";
- cout << Translation->GetUdot() << ",";
- cout << Translation->GetVdot() << ",";
- cout << Translation->GetWdot() << ",";
- cout << Rotation->GetP() << ",";
- cout << Rotation->GetQ() << ",";
- cout << Rotation->GetR() << ",";
- cout << Rotation->GetPdot() << ",";
- cout << Rotation->GetQdot() << ",";
- cout << Rotation->GetRdot() << ",";
- cout << Aircraft->GetFx() << ",";
- cout << Aircraft->GetFy() << ",";
- cout << Aircraft->GetFz() << ",";
- cout << State->Getlatitude() << ",";
- cout << State->Getlongitude() << ",";
cout << State->Getqbar() << ",";
- cout << Translation->Getalpha() << ",";
- cout << Aircraft->GetL() << ",";
- cout << Aircraft->GetM() << ",";
- cout << Aircraft->GetN() << ",";
- cout << FCS->GetThrottle(0) << ",";
- cout << FCS->GetDa() << ",";
- cout << FCS->GetDe() << ",";
- cout << FCS->GetDr() << ",";
+ cout << State->GetVt() << ",";
+ cout << FCS->GetThrottlePos(0) << ",";
+ cout << FCS->GetDaPos() << ",";
+ cout << FCS->GetDePos() << ",";
+ cout << FCS->GetDrPos() << ",";
+ cout << Atmosphere->GetDensity() << ",";
cout << Aircraft->GetIxx() << ",";
cout << Aircraft->GetIyy() << ",";
cout << Aircraft->GetIzz() << ",";
cout << Aircraft->GetIxz() << ",";
cout << Aircraft->GetMass() << ",";
- cout << Aircraft->GetXcg() << "";
+ cout << Aircraft->GetXYZcg() << ",";
+ cout << Aircraft->GetForces() << ",";
+ cout << Aircraft->GetMoments() << ",";
+ cout << State->Geth() << ",";
+ cout << Rotation->GetEuler() << ",";
+ cout << Rotation->GetPQR() << ",";
+ cout << Translation->GetUVW() << ",";
+ cout << Translation->Getalpha() << ",";
+ cout << Position->GetVel() << ",";
+ cout << State->Getlatitude() << ",";
+ cout << State->Getlongitude();
cout << endl;
-
}
+/******************************************************************************/
void FGOutput::DelimitedOutput(string fname)
{
if (sFirstPass) {
datafile.open(fname.c_str());
datafile << "Time,";
- datafile << "Altitude,";
- datafile << "Phi,";
- datafile << "Tht,";
- datafile << "Psi,";
- datafile << "Rho,";
- datafile << "Vtotal,";
- datafile << "U,";
- datafile << "V,";
- datafile << "W,";
- datafile << "Vn,";
- datafile << "Ve,";
- datafile << "Vd,";
- datafile << "Udot,";
- datafile << "Vdot,";
- datafile << "Wdot,";
- datafile << "P,";
- datafile << "Q,";
- datafile << "R,";
- datafile << "PDot,";
- datafile << "QDot,";
- datafile << "RDot,";
- datafile << "Fx,";
- datafile << "Fy,";
- datafile << "Fz,";
- datafile << "Latitude,";
- datafile << "Longitude,";
datafile << "QBar,";
- datafile << "Alpha,";
- datafile << "L,";
- datafile << "M,";
- datafile << "N,";
+ datafile << "Vtotal,";
datafile << "Throttle,";
datafile << "Aileron,";
datafile << "Elevator,";
datafile << "Rudder,";
+ datafile << "Rho,";
datafile << "Ixx,";
datafile << "Iyy,";
datafile << "Izz,";
datafile << "Ixz,";
datafile << "Mass,";
- datafile << "X CG";
+ datafile << "Xcg, Ycg, Zcg, ";
+ datafile << "Xforce, Yforce, Zforce, ";
+ datafile << "L, M, N, ";
+ datafile << "Altitude,";
+ datafile << "Phi, Tht, Psi,";
+ datafile << "P, Q, R, ";
+ datafile << "U, V, W, ";
+ datafile << "Alpha,";
+ datafile << "Vn, Ve, Vd, ";
+ datafile << "Latitude,";
+ datafile << "Longitude";
datafile << endl;
sFirstPass = false;
}
datafile << State->Getsim_time() << ",";
- datafile << State->Geth() << ",";
- datafile << Rotation->Getphi() << ",";
- datafile << Rotation->Gettht() << ",";
- datafile << Rotation->Getpsi() << ",";
- datafile << Atmosphere->GetDensity() << ",";
- datafile << State->GetVt() << ",";
- datafile << Translation->GetU() << ",";
- datafile << Translation->GetV() << ",";
- datafile << Translation->GetW() << ",";
- datafile << Position->GetVn() << ",";
- datafile << Position->GetVe() << ",";
- datafile << Position->GetVd() << ",";
- datafile << Translation->GetUdot() << ",";
- datafile << Translation->GetVdot() << ",";
- datafile << Translation->GetWdot() << ",";
- datafile << Rotation->GetP() << ",";
- datafile << Rotation->GetQ() << ",";
- datafile << Rotation->GetR() << ",";
- datafile << Rotation->GetPdot() << ",";
- datafile << Rotation->GetQdot() << ",";
- datafile << Rotation->GetRdot() << ",";
- datafile << Aircraft->GetFx() << ",";
- datafile << Aircraft->GetFy() << ",";
- datafile << Aircraft->GetFz() << ",";
- datafile << State->Getlatitude() << ",";
- datafile << State->Getlongitude() << ",";
datafile << State->Getqbar() << ",";
- datafile << Translation->Getalpha() << ",";
- datafile << Aircraft->GetL() << ",";
- datafile << Aircraft->GetM() << ",";
- datafile << Aircraft->GetN() << ",";
- datafile << FCS->GetThrottle(0) << ",";
- datafile << FCS->GetDa() << ",";
- datafile << FCS->GetDe() << ",";
- datafile << FCS->GetDr() << ",";
+ datafile << State->GetVt() << ",";
+ datafile << FCS->GetThrottlePos(0) << ",";
+ datafile << FCS->GetDaPos() << ",";
+ datafile << FCS->GetDePos() << ",";
+ datafile << FCS->GetDrPos() << ",";
+ datafile << Atmosphere->GetDensity() << ",";
datafile << Aircraft->GetIxx() << ",";
datafile << Aircraft->GetIyy() << ",";
datafile << Aircraft->GetIzz() << ",";
datafile << Aircraft->GetIxz() << ",";
datafile << Aircraft->GetMass() << ",";
- datafile << Aircraft->GetXcg() << "";
+ datafile << Aircraft->GetXYZcg() << ",";
+ datafile << Aircraft->GetForces() << ",";
+ datafile << Aircraft->GetMoments() << ",";
+ datafile << State->Geth() << ",";
+ datafile << Rotation->GetEuler() << ",";
+ datafile << Rotation->GetPQR() << ",";
+ datafile << Translation->GetUVW() << ",";
+ datafile << Translation->Getalpha() << ",";
+ datafile << Position->GetVel() << ",";
+ datafile << State->Getlatitude() << ",";
+ datafile << State->Getlongitude();
datafile << endl;
datafile.flush();
}
+/******************************************************************************/
+
void FGOutput::SocketOutput(void)
{
string asciiData;
-
-#ifdef MACOS
- if (socket == 0) return;
-#else
+ /*
if (socket <= 0) return;
-#endif
socket->Clear();
if (sFirstPass) {
socket->Append(FCS->GetDa());
socket->Append(FCS->GetDe());
socket->Append(FCS->GetDr());
- socket->Send();
+ socket->Send(); */
}
+/******************************************************************************/
void FGOutput::SocketStatusOutput(string out_str)
{
socket->Send();
}
+/******************************************************************************/
+
*******************************************************************************/
-FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex)
+FGPosition::FGPosition(FGFDMExec* fdmex) : FGModel(fdmex),
+ vUVW(3),
+ vVel(3)
{
Name = "FGPosition";
- AccelN = AccelE = AccelD = 0.0;
LongitudeDot = LatitudeDot = RadiusDot = 0.0;
+ lastLongitudeDot = lastLatitudeDot = lastRadiusDot = 0.0;
}
+/******************************************************************************/
FGPosition::~FGPosition(void)
{
}
+/******************************************************************************/
bool FGPosition:: Run(void)
{
- float tanLat, cosLat;
+ float cosLat;
if (!FGModel::Run()) {
GetState();
- T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3; // Page A-11
- T[1][2] = 2*(Q1*Q2 + Q0*Q3); // From
- T[1][3] = 2*(Q1*Q3 - Q0*Q2); // Reference [2]
- T[2][1] = 2*(Q1*Q2 - Q0*Q3);
- T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
- T[2][3] = 2*(Q2*Q3 + Q0*Q1);
- T[3][1] = 2*(Q1*Q3 + Q0*Q2);
- T[3][2] = 2*(Q2*Q3 - Q0*Q1);
- T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
-
- Fn = T[1][1]*Fx + T[2][1]*Fy + T[3][1]*Fz; // Eqn. 3.5
- Fe = T[1][2]*Fx + T[2][2]*Fy + T[3][2]*Fz; // From
- Fd = T[1][3]*Fx + T[2][3]*Fy + T[3][3]*Fz; // Reference [3]
-
- tanLat = tan(Latitude); // I made this up
- cosLat = cos(Latitude);
- lastAccelN = AccelN;
- lastAccelE = AccelE;
- lastAccelD = AccelD;
+ vVel = State->GetTb2l()*vUVW;
- Vn = T[1][1]*U + T[2][1]*V + T[3][1]*W;
- Ve = T[1][2]*U + T[2][2]*V + T[3][2]*W;
- Vd = T[1][3]*U + T[2][3]*V + T[3][3]*W;
+ cosLat = cos(Latitude);
+ if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat);
-/* AccelN = invMass * Fn + invRadius * (Vn*Vd - Ve*Ve*tanLat); // Eqn. 3.6
- AccelE = invMass * Fe + invRadius * (Ve*Vd + Vn*Ve*tanLat); // From
- AccelD = invMass * Fd - invRadius * (Vn*Vn + Ve*Ve); // Reference [3]
+ LatitudeDot = vVel(eNorth) * invRadius;
+ RadiusDot = -vVel(eDown);
- Vn += 0.5*dt*rate*(3.0*AccelN - lastAccelN); // Eqn. 3.7
- Ve += 0.5*dt*rate*(3.0*AccelE - lastAccelE); // From
- Vd += 0.5*dt*rate*(3.0*AccelD - lastAccelD); // Reference [3]
+ Longitude += 0.5*dt*rate*(LongitudeDot + lastLongitudeDot);
+ Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
+ Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
- Vee = Ve - OMEGAEARTH * (Radius) * cosLat; // From Eq. 3.8
-*/ // Reference [3]
lastLatitudeDot = LatitudeDot;
lastLongitudeDot = LongitudeDot;
lastRadiusDot = RadiusDot;
- if (cosLat != 0) LongitudeDot = Ve / (Radius * cosLat);
- LatitudeDot = Vn * invRadius;
- RadiusDot = -Vd;
-
- Longitude += 0.5*dt*rate*(LongitudeDot + lastLongitudeDot);
- Latitude += 0.5*dt*rate*(LatitudeDot + lastLatitudeDot);
- Radius += 0.5*dt*rate*(RadiusDot + lastRadiusDot);
-
PutState();
return false;
+
} else {
return true;
}
}
+/******************************************************************************/
void FGPosition::GetState(void)
{
dt = State->Getdt();
- Q0 = Rotation->GetQ0();
- Q1 = Rotation->GetQ1();
- Q2 = Rotation->GetQ2();
- Q3 = Rotation->GetQ3();
-
- Fx = Aircraft->GetFx();
- Fy = Aircraft->GetFy();
- Fz = Aircraft->GetFz();
-
- U = Translation->GetU();
- V = Translation->GetV();
- W = Translation->GetW();
+ vUVW = Translation->GetUVW();
Latitude = State->Getlatitude();
Longitude = State->Getlongitude();
Radius = State->Geth() + EARTHRAD;
}
+/******************************************************************************/
void FGPosition::PutState(void)
{
State->Seth(Radius - EARTHRAD);
}
+/******************************************************************************/
+
/*******************************************************************************
INCLUDES
*******************************************************************************/
+
#include "FGModel.h"
+#include "FGMatrix.h"
using namespace std;
class FGPosition : public FGModel
{
-public:
- FGPosition(FGFDMExec*);
- ~FGPosition(void);
-
- inline float GetFn(void) {return Fn;}
- inline float GetFe(void) {return Fe;}
- inline float GetFd(void) {return Fd;}
-
- inline float GetVn(void) {return Vn;}
- inline float GetVe(void) {return Ve;}
- inline float GetVd(void) {return Vd;}
-
- inline float GetT(int r, int c) {return T[r][c];}
- inline void SetT(float t1, float t2, float t3, float t4, float t5, float t6,
- float t7, float t8, float t9)
- {T[1][1]=t1; T[1][2]=t2 ;T[1][3]=t3;
- T[2][1]=t4; T[2][2]=t5 ;T[2][3]=t6;
- T[3][1]=t7; T[3][2]=t8 ;T[3][3]=t9;}
-
- bool Run(void);
+ FGColumnVector vUVW;
+ FGColumnVector vVel;
-protected:
-
-private:
- float T[4][4];
- float Q0, Q1, Q2, Q3;
- float Fn, Fe, Fd;
- float Fx, Fy, Fz;
- float U, V, W;
- float Vn, Ve, Vd, Vee;
- float invMass, invRadius;
+ float Vee, invMass, invRadius;
double Radius;
- float AccelN, AccelE, AccelD;
- float lastAccelN, lastAccelE, lastAccelD;
float LatitudeDot, LongitudeDot, RadiusDot;
float lastLatitudeDot, lastLongitudeDot, lastRadiusDot;
float Longitude, Latitude;
void GetState(void);
void PutState(void);
+
+public:
+ FGPosition(FGFDMExec*);
+ ~FGPosition(void);
+
+ inline FGColumnVector GetVel(void) {return vVel;}
+ inline FGColumnVector GetUVW(void) {return vUVW;}
+ inline FGColumnVector GetVn(void) {return vVel(1);}
+ inline FGColumnVector GetVe(void) {return vVel(2);}
+ inline FGColumnVector GetVd(void) {return vVel(3);}
+
+ bool Run(void);
};
/******************************************************************************/
#ifndef M_PI
/* get a definition for pi */
-#include <simgear/constants.h>
+#include <Include/fg_constants.h>
#define M_PI FG_PI
#endif
*******************************************************************************/
-FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex)
+FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex),
+ vPQR(3),
+ vEuler(3),
+ vMoments(3)
{
Name = "FGRotation";
- Q0dot = Q1dot = Q2dot = Q3dot = 0.0;
- Pdot = Qdot = Rdot = 0.0;
}
+/******************************************************************************/
FGRotation::~FGRotation(void)
{
}
+/******************************************************************************/
bool FGRotation::Run(void)
{
- float L2, N1, iQtot, sum;
+ float L2, N1;
+ static FGColumnVector vlastPQRdot(3);
+ static FGColumnVector vPQRdot(3);
if (!FGModel::Run()) {
GetState();
- lastPdot = Pdot;
- lastQdot = Qdot;
- lastRdot = Rdot;
+ L2 = vMoments(eL) + Ixz*vPQR(eP)*vPQR(eQ) - (Izz-Iyy)*vPQR(eR)*vPQR(eQ);
+ N1 = vMoments(eN) - (Iyy-Ixx)*vPQR(eP)*vPQR(eQ) - Ixz*vPQR(eR)*vPQR(eQ);
- L2 = L + Ixz*P*Q - (Izz-Iyy)*R*Q;
- N1 = N - (Iyy-Ixx)*P*Q - Ixz*R*Q;
+ 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(eR) = (N1*Ixx + L2*Ixz) / (Ixx*Izz - Ixz*Ixz);
- Pdot = (L2*Izz - N1*Ixz) / (Ixx*Izz - Ixz*Ixz);
- Qdot = (M - (Ixx-Izz)*P*R - Ixz*(P*P - R*R))/Iyy;
- Rdot = (N1*Ixx + L2*Ixz) / (Ixx*Izz - Ixz*Ixz);
+ vPQR += dt*rate*(vlastPQRdot + vPQRdot)/2.0;
- P += dt*rate*(lastPdot + Pdot)/2.0;
- Q += dt*rate*(lastQdot + Qdot)/2.0;
- R += dt*rate*(lastRdot + Rdot)/2.0;
+ State->IntegrateQuat(vPQR, rate);
+ State->CalcMatrices();
+ vEuler = State->CalcEuler();
- lastQ0dot = Q0dot;
- lastQ1dot = Q1dot;
- lastQ2dot = Q2dot;
- lastQ3dot = Q3dot;
+ vlastPQRdot = vPQRdot;
- Q0dot = -0.5*(Q1*P + Q2*Q + Q3*R);
- Q1dot = 0.5*(Q0*P + Q2*R - Q3*Q);
- Q2dot = 0.5*(Q0*Q + Q3*P - Q1*R);
- Q3dot = 0.5*(Q0*R + Q1*Q - Q2*P);
-
- Q0 += 0.5*dt*rate*(lastQ0dot + Q0dot);
- Q1 += 0.5*dt*rate*(lastQ1dot + Q1dot);
- Q2 += 0.5*dt*rate*(lastQ2dot + Q2dot);
- Q3 += 0.5*dt*rate*(lastQ3dot + Q3dot);
-
- sum = Q0*Q0 + Q1*Q1 + Q2*Q2 + Q3*Q3;
-
- iQtot = 1.0 / sqrt(sum);
-
- Q0 *= iQtot;
- Q1 *= iQtot;
- Q2 *= iQtot;
- Q3 *= iQtot;
-
- if (T[3][3] == 0)
- phi = 0.0;
- else
- phi = atan2(T[2][3], T[3][3]);
-
- tht = asin(-T[1][3]);
-
- if (T[1][1] == 0.0)
- psi = 0.0;
- else
- psi = atan2(T[1][2], T[1][1]);
-
- if (psi < 0.0) psi += 2*M_PI;
-
- PutState();
} else {
}
return false;
}
+/******************************************************************************/
void FGRotation::GetState(void)
{
dt = State->Getdt();
- L = Aircraft->GetL();
- M = Aircraft->GetM();
- N = Aircraft->GetN();
+ vMoments = Aircraft->GetMoments();
Ixx = Aircraft->GetIxx();
Iyy = Aircraft->GetIyy();
Izz = Aircraft->GetIzz();
Ixz = Aircraft->GetIxz();
-
- for (int r=1;r<=3;r++)
- for (int c=1;c<=3;c++)
- T[r][c] = Position->GetT(r,c);
}
-
-void FGRotation::PutState(void)
-{
-}
+/******************************************************************************/
#endif
#include "FGModel.h"
+#include "FGMatrix.h"
using namespace std;
class FGRotation : public FGModel
{
+ FGColumnVector vPQR;
+ FGColumnVector vMoments;
+ FGColumnVector vEuler;
+
+ float Ixx, Iyy, Izz, Ixz;
+ float dt;
+
+ void GetState(void);
+
public:
FGRotation(FGFDMExec*);
~FGRotation(void);
bool Run(void);
- inline float GetP(void) {return P;}
- inline float GetQ(void) {return Q;}
- inline float GetR(void) {return R;}
-
- inline float GetPdot(void) {return Pdot;}
- inline float GetQdot(void) {return Qdot;}
- inline float GetRdot(void) {return Rdot;}
-
- inline float Getphi(void) {return phi;}
- inline float Gettht(void) {return tht;}
- inline float Getpsi(void) {return psi;}
-
- inline float GetQ0(void) {return Q0;}
- inline float GetQ1(void) {return Q1;}
- inline float GetQ2(void) {return Q2;}
- inline float GetQ3(void) {return Q3;}
-
- inline void SetP(float tt) {P = tt;}
- inline void SetQ(float tt) {Q = tt;}
- inline void SetR(float tt) {R = tt;}
-
- inline void SetPQR(float t1, float t2, float t3) {P=t1;
- Q=t2;
- R=t3;}
-
- inline void Setphi(float tt) {phi = tt;}
- inline void Settht(float tt) {tht = tt;}
- inline void Setpsi(float tt) {psi = tt;}
-
- inline void SetEuler(float t1, float t2, float t3) {phi=t1;
- tht=t2;
- psi=t3;}
-
- inline void SetQ0123(float t1, float t2, float t3, float t4) {Q0=t1;
- Q1=t2;
- Q2=t3;
- Q3=t4;}
-
-protected:
-
-private:
- float P, Q, R;
- float L, M, N;
- float Ixx, Iyy, Izz, Ixz;
- float Q0, Q1, Q2, Q3;
- float phi, tht, psi;
- float Pdot, Qdot, Rdot;
- float Q0dot, Q1dot, Q2dot, Q3dot;
- float lastPdot, lastQdot, lastRdot;
- float lastQ0dot, lastQ1dot, lastQ2dot, lastQ3dot;
- float dt;
- float T[4][4];
-
- void GetState(void);
- void PutState(void);
+ inline FGColumnVector GetPQR(void) {return vPQR;}
+ inline FGColumnVector GetEuler(void) {return vEuler;}
+ inline void SetPQR(FGColumnVector tt) {vPQR = tt;}
+ inline void SetEuler(FGColumnVector tt) {vEuler = tt;}
+ inline float Getphi(void) {return vEuler(1);}
+ inline float Gettht(void) {return vEuler(2);}
+ inline float Getpsi(void) {return vEuler(3);}
};
/******************************************************************************/
*******************************************************************************/
-FGState::FGState(FGFDMExec* fdex)
+FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
+ mTl2b(3,3),
+ mTs2b(3,3),
+ vQtrn(4)
{
FDMExec = fdex;
qbar = 0.0;
sim_time = 0.0;
dt = 1.0/120.0;
+
+ coeffdef["FG_QBAR"] = 1 ;
+ coeffdef["FG_WINGAREA"] = 2 ;
+ coeffdef["FG_WINGSPAN"] = 4 ;
+ coeffdef["FG_CBAR"] = 8 ;
+ coeffdef["FG_ALPHA"] = 16 ;
+ coeffdef["FG_ALPHADOT"] = 32 ;
+ coeffdef["FG_BETA"] = 64 ;
+ coeffdef["FG_BETADOT"] = 128 ;
+ coeffdef["FG_PITCHRATE"] = 256 ;
+ coeffdef["FG_ROLLRATE"] = 512 ;
+ coeffdef["FG_YAWRATE"] = 1024 ;
+ coeffdef["FG_MACH"] = 2048 ;
+ coeffdef["FG_ALTITUDE"] = 4096 ;
+ coeffdef["FG_BI2VEL"] = 8192 ;
+ coeffdef["FG_CI2VEL"] = 16384 ;
+ coeffdef["FG_ELEVATOR_POS"] = 32768L ;
+ coeffdef["FG_AILERON_POS"] = 65536L ;
+ coeffdef["FG_RUDDER_POS"] = 131072L ;
+ coeffdef["FG_SPDBRAKE_POS"] = 262144L ;
+ coeffdef["FG_SPOILERS_POS"] = 524288L ;
+ coeffdef["FG_FLAPS_POS"] = 1048576L ;
+ coeffdef["FG_ELEVATOR_CMD"] = 2097152L ;
+ coeffdef["FG_AILERON_CMD"] = 4194304L ;
+ coeffdef["FG_RUDDER_CMD"] = 8388608L ;
+ coeffdef["FG_SPDBRAKE_CMD"] = 16777216L ;
+ coeffdef["FG_SPOILERS_CMD"] = 33554432L ;
+ coeffdef["FG_FLAPS_CMD"] = 67108864L ;
+ coeffdef["FG_SPARE3"] = 134217728L ;
+ coeffdef["FG_SPARE4"] = 268435456L ;
+ coeffdef["FG_SPARE5"] = 536870912L ;
+ coeffdef["FG_SPARE6"] = 1073741824L ;
}
+/******************************************************************************/
FGState::~FGState(void)
{
}
-
//***************************************************************************
//
// Reset: Assume all angles READ FROM FILE IN DEGREES !!
//
-bool FGState::Reset(string path, string fname)
+bool FGState::Reset(string path, string acname, string fname)
{
string resetDef;
float U, V, W;
float phi, tht, psi;
- resetDef = path + "/" + FDMExec->GetAircraft()->GetAircraftName() + "/" + fname;
+ resetDef = path + "/" + acname + "/" + fname;
ifstream resetfile(resetDef.c_str());
float phi, float tht, float psi,
float Latitude, float Longitude, float H)
{
+ FGColumnVector vUVW(3);
+ FGColumnVector vEuler(3);
float alpha, beta, gamma;
- float Q0, Q1, Q2, Q3;
- float T[4][4];
latitude = Latitude;
longitude = Longitude;
h = H;
+ FDMExec->GetAtmosphere()->Run();
gamma = 0.0;
if (W != 0.0)
else
beta = 0.0;
- FDMExec->GetTranslation()->SetUVW(U, V, W);
- FDMExec->GetRotation()->SetEuler(phi, tht, psi);
+ vUVW << U << V << W;
+ FDMExec->GetTranslation()->SetUVW(vUVW);
+ vEuler << phi << tht << psi;
+ FDMExec->GetRotation()->SetEuler(vEuler);
FDMExec->GetTranslation()->SetABG(alpha, beta, gamma);
Vt = sqrt(U*U + V*V + W*W);
- qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->CalcRho(h);
-
- Q0 = sin(psi*0.5)*sin(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*cos(phi*0.5);
- Q1 = -sin(psi*0.5)*sin(tht*0.5)*cos(phi*0.5) + cos(psi*0.5)*cos(tht*0.5)*sin(phi*0.5);
- Q2 = sin(psi*0.5)*cos(tht*0.5)*sin(phi*0.5) + cos(psi*0.5)*sin(tht*0.5)*cos(phi*0.5);
- Q3 = sin(psi*0.5)*cos(tht*0.5)*cos(phi*0.5) - cos(psi*0.5)*sin(tht*0.5)*sin(phi*0.5);
-
- FDMExec->GetRotation()->SetQ0123(Q0, Q1, Q2, Q3);
-
- T[1][1] = Q0*Q0 + Q1*Q1 - Q2*Q2 - Q3*Q3;
- T[1][2] = 2*(Q1*Q2 + Q0*Q3);
- T[1][3] = 2*(Q1*Q3 - Q0*Q2);
- T[2][1] = 2*(Q1*Q2 - Q0*Q3);
- T[2][2] = Q0*Q0 - Q1*Q1 + Q2*Q2 - Q3*Q3;
- T[2][3] = 2*(Q2*Q3 + Q0*Q1);
- T[3][1] = 2*(Q1*Q3 + Q0*Q2);
- T[3][2] = 2*(Q2*Q3 - Q0*Q1);
- T[3][3] = Q0*Q0 - Q1*Q1 - Q2*Q2 + Q3*Q3;
-
- FDMExec->GetPosition()->SetT(T[1][1], T[1][2], T[1][3],
- T[2][1], T[2][2], T[2][3],
- T[3][1], T[3][2], T[3][3]);
- DisplayData();
+ qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->GetDensity();
+
+ InitMatrices(phi, tht, psi);
}
+/******************************************************************************/
void FGState::Initialize(FGInitialCondition *FGIC)
{
phi = FGIC->GetPhiRadIC();
psi = FGIC->GetPsiRadIC();
- Initialize(U, V, W, phi, tht, psi,latitude, longitude, h);
+ Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
}
+/******************************************************************************/
bool FGState::StoreData(string fname)
{
ofstream datafile(fname.c_str());
if (datafile) {
- datafile << FDMExec->GetTranslation()->GetU();
- datafile << FDMExec->GetTranslation()->GetV();
- datafile << FDMExec->GetTranslation()->GetW();
+ datafile << (FDMExec->GetTranslation()->GetUVW())(1);
+ datafile << (FDMExec->GetTranslation()->GetUVW())(2);
+ datafile << (FDMExec->GetTranslation()->GetUVW())(3);
datafile << latitude;
datafile << longitude;
- datafile << FDMExec->GetRotation()->Getphi();
- datafile << FDMExec->GetRotation()->Gettht();
- datafile << FDMExec->GetRotation()->Getpsi();
+ datafile << (FDMExec->GetRotation()->GetEuler())(1);
+ datafile << (FDMExec->GetRotation()->GetEuler())(2);
+ datafile << (FDMExec->GetRotation()->GetEuler())(3);
datafile << h;
datafile.close();
return true;
}
}
+/******************************************************************************/
-bool FGState::DumpData(string fname)
+float FGState::GetParameter(string val_string)
{
- ofstream datafile(fname.c_str());
+ return GetParameter(coeffdef[val_string]);
+}
- if (datafile) {
- datafile << "U: " << FDMExec->GetTranslation()->GetU() << endl;
- datafile << "V: " << FDMExec->GetTranslation()->GetV() << endl;
- datafile << "W: " << FDMExec->GetTranslation()->GetW() << endl;
- datafile << "P: " << FDMExec->GetRotation()->GetP() << endl;
- datafile << "Q: " << FDMExec->GetRotation()->GetQ() << endl;
- datafile << "R: " << FDMExec->GetRotation()->GetR() << endl;
- datafile << "L: " << FDMExec->GetAircraft()->GetL() << endl;
- datafile << "M: " << FDMExec->GetAircraft()->GetM() << endl;
- datafile << "N: " << FDMExec->GetAircraft()->GetN() << endl;
- datafile << "latitude: " << latitude << endl;
- datafile << "longitude: " << longitude << endl;
- datafile << "alpha: " << FDMExec->GetTranslation()->Getalpha() << endl;
- datafile << "beta: " << FDMExec->GetTranslation()->Getbeta() << endl;
- datafile << "gamma: " << FDMExec->GetTranslation()->Getgamma() << endl;
- datafile << "phi: " << FDMExec->GetRotation()->Getphi() << endl;
- datafile << "tht: " << FDMExec->GetRotation()->Gettht() << endl;
- datafile << "psi: " << FDMExec->GetRotation()->Getpsi() << endl;
- datafile << "Pdot: " << FDMExec->GetRotation()->GetPdot() << endl;
- datafile << "Qdot: " << FDMExec->GetRotation()->GetQdot() << endl;
- datafile << "Rdot: " << FDMExec->GetRotation()->GetRdot() << endl;
- datafile << "h: " << h << endl;
- datafile << "a: " << a << endl;
- datafile << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
- datafile << "qbar: " << qbar << endl;
- datafile << "sim_time: " << sim_time << endl;
- datafile << "dt: " << dt << endl;
- datafile << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
- datafile.close();
- return true;
- } else {
- return false;
+/******************************************************************************/
+
+int FGState::GetParameterIndex(string val_string)
+{
+ return coeffdef[val_string];
+}
+
+/******************************************************************************/
+//
+// NEED WORK BELOW TO ADD NEW PARAMETERS !!!
+//
+float FGState::GetParameter(int val_idx)
+{
+ switch(val_idx) {
+ case FG_QBAR:
+ return Getqbar();
+ case FG_WINGAREA:
+ return FDMExec->GetAircraft()->GetWingArea();
+ case FG_WINGSPAN:
+ return FDMExec->GetAircraft()->GetWingSpan();
+ case FG_CBAR:
+ return FDMExec->GetAircraft()->Getcbar();
+ case FG_ALPHA:
+ return FDMExec->GetTranslation()->Getalpha();
+ case FG_ALPHADOT:
+ return Getadot();
+ case FG_BETA:
+ return FDMExec->GetTranslation()->Getbeta();
+ case FG_BETADOT:
+ return Getbdot();
+ case FG_PITCHRATE:
+ return (FDMExec->GetRotation()->GetPQR())(2);
+ case FG_ROLLRATE:
+ return (FDMExec->GetRotation()->GetPQR())(1);
+ case FG_YAWRATE:
+ return (FDMExec->GetRotation()->GetPQR())(3);
+ case FG_ELEVATOR_POS:
+ return FDMExec->GetFCS()->GetDePos();
+ case FG_AILERON_POS:
+ return FDMExec->GetFCS()->GetDaPos();
+ case FG_RUDDER_POS:
+ return FDMExec->GetFCS()->GetDrPos();
+ case FG_SPDBRAKE_POS:
+ return FDMExec->GetFCS()->GetDsbPos();
+ case FG_SPOILERS_POS:
+ return FDMExec->GetFCS()->GetDspPos();
+ case FG_FLAPS_POS:
+ return FDMExec->GetFCS()->GetDfPos();
+ case FG_ELEVATOR_CMD:
+ return FDMExec->GetFCS()->GetDeCmd();
+ case FG_AILERON_CMD:
+ return FDMExec->GetFCS()->GetDaCmd();
+ case FG_RUDDER_CMD:
+ return FDMExec->GetFCS()->GetDrCmd();
+ case FG_SPDBRAKE_CMD:
+ return FDMExec->GetFCS()->GetDsbCmd();
+ case FG_SPOILERS_CMD:
+ return FDMExec->GetFCS()->GetDspCmd();
+ case FG_FLAPS_CMD:
+ return FDMExec->GetFCS()->GetDfCmd();
+ case FG_MACH:
+ return GetMach();
+ case FG_ALTITUDE:
+ return Geth();
+ case FG_BI2VEL:
+ return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * GetVt());
+ case FG_CI2VEL:
+ return FDMExec->GetAircraft()->Getcbar()/(2.0 * GetVt());
+ }
+ return 0;
+}
+
+/******************************************************************************/
+
+void FGState::SetParameter(int val_idx, float val)
+{
+ switch(val_idx) {
+ case FG_ELEVATOR_POS:
+ FDMExec->GetFCS()->SetDePos(val);
+ break;
+ case FG_AILERON_POS:
+ FDMExec->GetFCS()->SetDaPos(val);
+ break;
+ case FG_RUDDER_POS:
+ FDMExec->GetFCS()->SetDrPos(val);
+ break;
+ case FG_SPDBRAKE_POS:
+ FDMExec->GetFCS()->SetDrPos(val);
+ break;
+ case FG_SPOILERS_POS:
+ FDMExec->GetFCS()->SetDrPos(val);
+ break;
+ case FG_FLAPS_POS:
+ FDMExec->GetFCS()->SetDrPos(val);
+ break;
}
}
+/******************************************************************************/
-bool FGState::DisplayData(void)
+void FGState::InitMatrices(float phi, float tht, float psi)
{
- cout << "U: " << FDMExec->GetTranslation()->GetU() << endl;
- cout << "V: " << FDMExec->GetTranslation()->GetV() << endl;
- cout << "W: " << FDMExec->GetTranslation()->GetW() << endl;
- cout << "P: " << FDMExec->GetRotation()->GetP()*RADTODEG << endl;
- cout << "Q: " << FDMExec->GetRotation()->GetQ()*RADTODEG << endl;
- cout << "R: " << FDMExec->GetRotation()->GetR()*RADTODEG << endl;
- cout << "L: " << FDMExec->GetAircraft()->GetL() << endl;
- cout << "M: " << FDMExec->GetAircraft()->GetM() << endl;
- cout << "N: " << FDMExec->GetAircraft()->GetN() << endl;
- cout << "Vt: " << Vt << endl;
- cout << "latitude: " << latitude << endl;
- cout << "longitude: " << longitude << endl;
- cout << "alpha: " << FDMExec->GetTranslation()->Getalpha()*RADTODEG << endl;
- cout << "beta: " << FDMExec->GetTranslation()->Getbeta()*RADTODEG << endl;
- cout << "gamma: " << FDMExec->GetTranslation()->Getgamma()*RADTODEG << endl;
- cout << "phi: " << FDMExec->GetRotation()->Getphi()*RADTODEG << endl;
- cout << "tht: " << FDMExec->GetRotation()->Gettht()*RADTODEG << endl;
- cout << "psi: " << FDMExec->GetRotation()->Getpsi()*RADTODEG << endl;
- cout << "Pdot: " << FDMExec->GetRotation()->GetPdot()*RADTODEG << endl;
- cout << "Qdot: " << FDMExec->GetRotation()->GetQdot()*RADTODEG << endl;
- cout << "Rdot: " << FDMExec->GetRotation()->GetRdot()*RADTODEG << endl;
- cout << "h: " << h << endl;
- cout << "a: " << a << endl;
- cout << "rho: " << FDMExec->GetAtmosphere()->Getrho() << endl;
- cout << "qbar: " << qbar << endl;
- cout << "sim_time: " << sim_time << endl;
- cout << "dt: " << dt << endl;
- cout << "m: " << FDMExec->GetAircraft()->GetMass() << endl;
-
- return true;
+ float thtd2, psid2, phid2;
+ float Sthtd2, Spsid2, Sphid2;
+ float Cthtd2, Cpsid2, Cphid2;
+ float Cphid2Cthtd2;
+ float Cphid2Sthtd2;
+ float Sphid2Sthtd2;
+ float Sphid2Cthtd2;
+
+ thtd2 = tht/2.0;
+ psid2 = psi/2.0;
+ phid2 = phi/2.0;
+
+ Sthtd2 = sin(thtd2);
+ Spsid2 = sin(psid2);
+ Sphid2 = sin(phid2);
+
+ Cthtd2 = cos(thtd2);
+ Cpsid2 = cos(psid2);
+ Cphid2 = cos(phid2);
+
+ Cphid2Cthtd2 = Cphid2*Cthtd2;
+ Cphid2Sthtd2 = Cphid2*Sthtd2;
+ Sphid2Sthtd2 = Sphid2*Sthtd2;
+ Sphid2Cthtd2 = Sphid2*Cthtd2;
+
+ vQtrn(1) = Cphid2Cthtd2*Cpsid2 + Sphid2Sthtd2*Spsid2;
+ vQtrn(2) = Sphid2Cthtd2*Cpsid2 - Cphid2Sthtd2*Spsid2;
+ vQtrn(3) = Cphid2Sthtd2*Cpsid2 + Sphid2Cthtd2*Spsid2;
+ vQtrn(4) = Cphid2Cthtd2*Spsid2 - Sphid2Sthtd2*Cpsid2;
+
+ CalcMatrices();
}
+
+/******************************************************************************/
+
+void FGState::CalcMatrices(void)
+{
+ float Q0Q0, Q1Q1, Q2Q2, Q3Q3;
+ float Q0Q1, Q0Q2, Q0Q3, Q1Q2;
+ float Q1Q3, Q2Q3;
+
+ Q0Q0 = vQtrn(1)*vQtrn(1);
+ Q1Q1 = vQtrn(2)*vQtrn(2);
+ Q2Q2 = vQtrn(3)*vQtrn(3);
+ Q3Q3 = vQtrn(4)*vQtrn(4);
+ Q0Q1 = vQtrn(1)*vQtrn(2);
+ Q0Q2 = vQtrn(1)*vQtrn(3);
+ Q0Q3 = vQtrn(1)*vQtrn(4);
+ Q1Q2 = vQtrn(2)*vQtrn(3);
+ 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 = mTb2l;
+ mTl2b.T();
+}
+
+/******************************************************************************/
+
+void FGState::IntegrateQuat(FGColumnVector vPQR, int rate)
+{
+ static FGColumnVector vlastQdot(4);
+ static FGColumnVector vQdot(4);
+
+ vQdot(1) = -0.5*(vQtrn(2)*vPQR(eP) + vQtrn(3)*vPQR(eQ) + vQtrn(4)*vPQR(eR));
+ 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.Normalize();
+
+ vlastQdot = vQdot;
+}
+
+/******************************************************************************/
+
+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));
+
+ vEuler(eTht) = asin(-mTb2l(1,3));
+
+ if (mTb2l(1,1) == 0.0) vEuler(ePsi) = 0.0;
+ else vEuler(ePsi) = atan2(mTb2l(1,2), mTb2l(1,1));
+
+ if (vEuler(ePsi) < 0.0) vEuler(ePsi) += 2*M_PI;
+
+ return vEuler;
+}
+
+/******************************************************************************/
+
+FGMatrix FGState::GetTs2b(float alpha, float beta)
+{
+ float ca, cb, sa, sb;
+
+ ca = cos(alpha);
+ sa = sin(alpha);
+ cb = cos(beta);
+ sb = sin(beta);
+
+ mTs2b(1,1) = -ca*cb;
+ mTs2b(1,2) = -ca*sb;
+ mTs2b(1,3) = sa;
+ mTs2b(2,1) = sb;
+ mTs2b(2,2) = cb;
+ mTs2b(2,3) = 0.0;
+ mTs2b(3,1) = -sa*cb;
+ mTs2b(3,2) = -sa*sb;
+ mTs2b(3,3) = -ca;
+
+ return mTs2b;
+}
+
+/******************************************************************************/
+
# include <fstream>
#endif
+#include <map>
#include "FGDefs.h"
#include "FGInitialCondition.h"
+#include "FGMatrix.h"
/*******************************************************************************
DEFINES
FGState(FGFDMExec*);
~FGState(void);
- bool Reset(string, string);
+ bool Reset(string, string, string);
void Initialize(float, float, float, float, float, float, float, float, float);
void Initialize(FGInitialCondition *FGIC);
bool StoreData(string);
- bool DumpData(string);
- bool DisplayData(void);
inline float GetVt(void) {return Vt;}
inline float Getdt(void) {return dt;}
inline float Getqbar(void) {return qbar;}
+ float GetParameter(int val_idx);
+ float GetParameter(string val_string);
+ int GetParameterIndex(string val_string);
inline void SetVt(float tt) {Vt = tt;}
inline float Setsim_time(float tt) {sim_time = tt; return sim_time;}
inline void Setdt(float tt) {dt = tt;}
- inline float IncrTime(void) {sim_time+=dt;return sim_time;}
+ void SetParameter(int, float);
+ inline float IncrTime(void) {sim_time+=dt;return sim_time;}
+ void InitMatrices(float phi, float tht, float psi);
+ void CalcMatrices(void);
+ void IntegrateQuat(FGColumnVector vPQR, int rate);
+ FGColumnVector CalcEuler(void);
+ FGMatrix GetTs2b(float alpha, float beta);
+ FGMatrix GetTl2b(void) {return mTl2b;}
+ FGMatrix GetTb2l(void) {return mTb2l;}
private:
FGFDMExec* FDMExec;
float LocalAltitudeOverRunway;
+ FGMatrix mTb2l;
+ FGMatrix mTl2b;
+ FGMatrix mTs2b;
+ FGColumnVector vQtrn;
-protected:
+ typedef map<string, long> CoeffMap;
+ CoeffMap coeffdef;
+protected:
+ enum {ePhi=1, eTht, ePsi};
+ enum {eP=1, eQ, eR};
};
/******************************************************************************/
*******************************************************************************/
-FGTank::FGTank(ifstream& acfile)
+FGTank::FGTank(FGConfigFile* AC_cfg)
{
string type;
- acfile >> type; // Type = 0: fuel, 1: oxidizer
+ *AC_cfg >> type; // Type = 0: fuel, 1: oxidizer
+
if (type == "FUEL") Type = ttFUEL;
else if (type == "OXIDIZER") Type = ttOXIDIZER;
else Type = ttUNKNOWN;
- acfile >> X; // inches
- acfile >> Y; // "
- acfile >> Z; // "
- acfile >> Radius; // "
- acfile >> Capacity; // pounds (amount it can hold)
- acfile >> Contents; // pounds (amount it is holding)
+ *AC_cfg >> X; // inches
+ *AC_cfg >> Y; // "
+ *AC_cfg >> Z; // "
+ *AC_cfg >> Radius; // "
+ *AC_cfg >> Capacity; // pounds (amount it can hold)
+ *AC_cfg >> Contents; // pounds (amount it is holding)
Selected = true;
- PctFull = 100.0*Contents/Capacity; // percent full; 0 to 100.0
+ PctFull = 100.0*Contents/Capacity; // percent full; 0 to 100.0
}
/*******************************************************************************
INCLUDES
*******************************************************************************/
+
#ifdef FGFS
# include <simgear/compiler.h>
# include STL_STRING
-# ifdef FG_HAVE_STD_INCLUDES
-# include <fstream>
-# else
-# include <fstream.h>
-# endif
FG_USING_STD(string);
#else
# include <string>
-# include <fstream>
#endif
+#include "FGConfigFile.h"
+
/*******************************************************************************
DEFINES
*******************************************************************************/
-
+
using namespace std;
/*******************************************************************************
class FGTank
{
public:
- FGTank(ifstream&);
+ FGTank(FGConfigFile*);
~FGTank(void);
float Reduce(float);
float inline GetZ(void) {return Z;}
enum TankType {ttUNKNOWN, ttFUEL, ttOXIDIZER};
-
+
private:
TankType Type;
float X, Y, Z;
--------------------------------------------------------------------------------
12/02/98 JSB Created
7/23/99 TP Added data member and modified Run and PutState to calcuate
- Mach number
+ Mach number
********************************************************************************
COMMENTS, REFERENCES, and NOTES
*******************************************************************************/
-FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex)
+FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex),
+ vUVW(3),
+ vPQR(3),
+ vForces(3),
+ vEuler(3)
{
Name = "FGTranslation";
- Udot = Vdot = Wdot = 0.0;
}
+/******************************************************************************/
FGTranslation::~FGTranslation(void)
{
}
+/******************************************************************************/
bool FGTranslation::Run(void)
{
+ static FGColumnVector vlastUVWdot(3);
+ static FGColumnVector vUVWdot(3);
+ static FGMatrix mVel(3,3);
+
if (!FGModel::Run()) {
GetState();
- lastUdot = Udot;
- lastVdot = Vdot;
- lastWdot = Wdot;
+ mVel(1,1) = 0.0;
+ mVel(1,2) = -vUVW(eW);
+ mVel(1,3) = vUVW(eV);
+ mVel(2,1) = vUVW(eW);
+ mVel(2,2) = 0.0;
+ mVel(2,3) = -vUVW(eU);
+ mVel(3,1) = -vUVW(eV);
+ mVel(3,2) = vUVW(eU);
+ mVel(3,3) = 0.0;
- Udot = V*R - W*Q + Fx/Mass;
- Vdot = W*P - U*R + Fy/Mass;
- Wdot = U*Q - V*P + Fz/Mass;
+ vUVWdot = mVel*vPQR + vForces/Mass;
- U += 0.5*dt*rate*(lastUdot + Udot);
- V += 0.5*dt*rate*(lastVdot + Vdot);
- W += 0.5*dt*rate*(lastWdot + Wdot);
+ vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot);
- Vt = U*U+V*V+W*W > 0.0 ? sqrt(U*U + V*V + W*W) : 0.0;
+ Vt = vUVW.Magnitude();
- if (W != 0.0)
- alpha = U*U > 0.0 ? atan2(W, U) : 0.0;
- if (V != 0.0)
- beta = U*U+W*W > 0.0 ? atan2(V, (fabs(U)/U)*sqrt(U*U + W*W)) : 0.0;
+ if (vUVW(eW) != 0.0)
+ alpha = vUVW(eU)*vUVW(eU) > 0.0 ? atan2(vUVW(eW), vUVW(eU)) : 0.0;
+ if (vUVW(eV) != 0.0)
+ beta = vUVW(eU)*vUVW(eU)+vUVW(eW)*vUVW(eW) > 0.0 ? atan2(vUVW(eV), (fabs(vUVW(eU))/vUVW(eU))*sqrt(vUVW(eU)*vUVW(eU) + vUVW(eW)*vUVW(eW))) : 0.0;
qbar = 0.5*rho*Vt*Vt;
mach = Vt / State->Geta();
+ vlastUVWdot = vUVWdot;
+
PutState();
} else {
}
return false;
}
+/******************************************************************************/
void FGTranslation::GetState(void)
{
dt = State->Getdt();
- P = Rotation->GetP();
- Q = Rotation->GetQ();
- R = Rotation->GetR();
-
- Fx = Aircraft->GetFx();
- Fy = Aircraft->GetFy();
- Fz = Aircraft->GetFz();
+ vPQR = Rotation->GetPQR();
+ vForces = Aircraft->GetForces();
Mass = Aircraft->GetMass();
rho = Atmosphere->GetDensity();
- phi = Rotation->Getphi();
- tht = Rotation->Gettht();
- psi = Rotation->Getpsi();
+ vEuler = Rotation->GetEuler();
}
+/******************************************************************************/
void FGTranslation::PutState(void)
{
#endif
#include "FGModel.h"
+#include "FGMatrix.h"
using namespace std;
FGTranslation(FGFDMExec*);
~FGTranslation(void);
- inline float GetU(void) {return U;}
- inline float GetV(void) {return V;}
- inline float GetW(void) {return W;}
-
- inline float GetUdot(void) {return Udot;}
- inline float GetVdot(void) {return Vdot;}
- inline float GetWdot(void) {return Wdot;}
+ inline FGColumnVector GetUVW(void) {return vUVW;}
inline float Getalpha(void) {return alpha;}
inline float Getbeta (void) {return beta; }
inline float Getgamma(void) {return gamma;}
- inline void SetU(float tt) {U = tt;}
- inline void SetV(float tt) {V = tt;}
- inline void SetW(float tt) {W = tt;}
-
- inline void SetUVW(float t1, float t2, float t3) {U=t1; V=t2; W=t3;}
+ inline void SetUVW(FGColumnVector tt) {vUVW = tt;}
inline void Setalpha(float tt) {alpha = tt;}
inline void Setbeta (float tt) {beta = tt;}
protected:
private:
- float U, V, W; // Body frame velocities owned by FGTranslation
- float P, Q, R;
+ FGColumnVector vUVW;
+ FGColumnVector vPQR;
+ FGColumnVector vForces;
+ FGColumnVector vEuler;
float Vt, qbar, mach;
- float Udot, Vdot, Wdot;
- float lastUdot, lastVdot, lastWdot;
- float phi, tht, psi;
- float Fx, Fy, Fz;
float Mass, dt;
float alpha, beta, gamma;
float rho;
#ifndef M_PI_2
/* get a definition for pi */
-#include <simgear/constants.h>
+#include <Include/fg_constants.h>
#define M_PI_2 FG_PI_2
#endif
float FGUtility::ToGeodetic()
{
- return 0.0;
+ return 0.0;
}
float FGUtility:: FromGeodetic()
{
- return 0.0;
+ return 0.0;
}
{
size = 0;
- #if defined( __BORLANDC__ ) || defined( _MSC_VER )
+ #if defined(__BORLANDC__) || defined(_MSC_VER)
WSADATA wsaData;
int PASCAL FAR wsaReturnCode;
wsaReturnCode = WSAStartup(MAKEWORD(1,1), &wsaData);
}
}
-#ifdef MACOS
-/* commented out destructor method. shutdown method needs to link when
- we don't have networking enabled */
-#else
FGfdmSocket::~FGfdmSocket(void)
{
if (sckt) shutdown(sckt,2);
WSACleanup();
#endif
}
-#endif
void FGfdmSocket::Clear(void)
{
INCLUDES
*******************************************************************************/
-#if __BCPLUSPLUS__ >= 0x0540 // If compiling under Borland C++Builder
-//---------------------------------------------------------------------------
+#if __BCPLUSPLUS__ >= 0x0540 // If compiling under Borland C++Builder
#pragma hdrstop
#include <condefs.h>
-USEUNIT("FGAircraft.cpp");
+USEUNIT("FGUtility.cpp");
USEUNIT("FGAtmosphere.cpp");
USEUNIT("FGAuxiliary.cpp");
USEUNIT("FGCoefficient.cpp");
+USEUNIT("FGConfigFile.cpp");
+USEUNIT("FGControls.cpp");
USEUNIT("FGEngine.cpp");
USEUNIT("FGFCS.cpp");
USEUNIT("FGFDMExec.cpp");
+USEUNIT("FGfdmSocket.cpp");
USEUNIT("FGInitialCondition.cpp");
+USEUNIT("FGLGear.cpp");
+USEUNIT("FGMatrix.cpp");
USEUNIT("FGModel.cpp");
USEUNIT("FGOutput.cpp");
USEUNIT("FGPosition.cpp");
USEUNIT("FGState.cpp");
USEUNIT("FGTank.cpp");
USEUNIT("FGTranslation.cpp");
-USEUNIT("FGUtility.cpp");
+USEUNIT("FGAircraft.cpp");
USERES("JSBSim.res");
-USEUNIT("FGLGear.cpp");
-USEUNIT("FGfdmSocket.cpp");
+USEUNIT("filtersjb\FGfcsComponent.cpp");
+USEUNIT("filtersjb\FGSwitch.cpp");
+USEUNIT("filtersjb\FGFilter.cpp");
+USEUNIT("filtersjb\FGGain.cpp");
+USEUNIT("filtersjb\FGGradient.cpp");
+USEUNIT("filtersjb\FGSummer.cpp");
+USEUNIT("filtersjb\FGDeadBand.cpp");
//---------------------------------------------------------------------------
#pragma argsused
#endif
int main(int argc, char** argv)
{
- FGFDMExec* FDMExec;
+ FGFDMExec* FDMExec;
if (argc != 3) {
cout << endl
FDMExec = new FGFDMExec();
FDMExec->GetAircraft()->LoadAircraft("aircraft", "engine", string(argv[1]));
- FDMExec->GetState()->Reset("aircraft", string(argv[2]));
+ if ( ! FDMExec->GetState()->Reset("aircraft", string(argv[1]), string(argv[2])))
+ FDMExec->GetState()->Initialize(2000,0,0,0,0,0,0.5,0.5,40000);
- while (FDMExec->GetState()->Getsim_time() <= 25.0)
+ float cmd = 0.0;
+
+ while (FDMExec->GetState()->Getsim_time() <= 5.0)
{
- //
- // Fake an elevator kick here after 5 seconds
- //
+ // Fake an elevator ramp here after 1 second, hold for one second, ramp down
- if (FDMExec->GetState()->Getsim_time() > 5.0 &&
- FDMExec->GetState()->Getsim_time() < 6.0)
+ if (FDMExec->GetState()->Getsim_time() >= 1.00 &&
+ FDMExec->GetState()->Getsim_time() < 2.0)
+ {
+ cmd = FDMExec->GetState()->Getsim_time() - 1.00;
+ } else if (FDMExec->GetState()->Getsim_time() >= 2.00 &&
+ FDMExec->GetState()->Getsim_time() < 3.0)
{
- FDMExec->GetFCS()->SetDe(0.05);
- } else {
- FDMExec->GetFCS()->SetDe(0.00);
+ cmd = 1.00;
+ } else if (FDMExec->GetState()->Getsim_time() >= 3.00 &&
+ FDMExec->GetState()->Getsim_time() < 4.0)
+ {
+ cmd = 4.0 - FDMExec->GetState()->Getsim_time();
+ } else {
+ cmd = 0.00;
}
+ FDMExec->GetFCS()->SetDeCmd(cmd); // input between -1 and 1
FDMExec->Run();
}
delete FDMExec;
-
- return 0;
-}
-#ifndef FGFS
-int WinMain()
-{
return 0;
}
-#endif
+
+SUBDIRS = filtersjb
+
EXTRA_DIST = JSBSim.cpp Makefile.solo
noinst_LIBRARIES = libJSBsim.a
FGAtmosphere.cpp FGAtmosphere.h \
FGAuxiliary.cpp FGAuxiliary.h \
FGCoefficient.cpp FGCoefficient.h \
+ FGConfigFile.cpp FGConfigFile.h \
+ FGControls.cpp FGControls.h \
FGDefs.h \
FGFCS.cpp FGFCS.h \
FGFDMExec.cpp FGFDMExec.h \
FGInitialCondition.cpp FGInitialCondition.h \
FGLGear.cpp FGLGear.h \
+ FGMatrix.cpp FGMatrix.h \
FGModel.cpp FGModel.h \
FGOutput.cpp FGOutput.h \
FGPosition.cpp FGPosition.h \
testJSBsim_SOURCES = JSBSim.cpp
-testJSBsim_LDADD = libJSBsim.a
+testJSBsim_LDADD = libJSBsim.a filtersjb/libfiltersjb.a
-INCLUDES += -I$(top_builddir)
+INCLUDES += -I$(top_builddir)/src
DEFS += -DFGFS
-JSBSim : FGAircraft.o FGAtmosphere.o FGCoefficient.o FGFCS.o FGFDMExec.o\
- FGModel.o FGOutput.o FGPosition.o FGRotation.o FGState.o FGTranslation.o\
- FGUtility.o FGEngine.o FGTank.o FGAuxiliary.o JSBSim.o
- g++ $(CCOPTS) -lm *.o -oJSBSim
+INCLUDES = -I.
+LINKDIR= -Lfiltersjb
+JSBSim_objects = FGAircraft.o FGAtmosphere.o FGCoefficient.o FGFCS.o FGFDMExec.o\
+FGModel.o FGOutput.o FGPosition.o FGRotation.o FGState.o FGTranslation.o\
+FGUtility.o FGEngine.o FGTank.o FGAuxiliary.o FGfdmSocket.o\
+FGConfigFile.o FGInitialCondition.o FGLGear.o FGMatrix.o
+
+JSBSim : $(JSBSim_objects) JSBSim.o
+ echo "Making JSBSim"
+ cd filtersjb && make -fMakefile.solo && cd ..
+ g++ $(INCLUDES) $(CCOPTS) $(LINKDIR) $(JSBSim_objects) JSBSim.o -oJSBSim -lm -lFCSComponents
+
FGAircraft.o : FGAircraft.cpp
- g++ $(CCOPTS) -c FGAircraft.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGAircraft.cpp
+
FGAtmosphere.o : FGAtmosphere.cpp
- g++ $(CCOPTS) -c FGAtmosphere.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGAtmosphere.cpp
+
FGAuxiliary.o : FGAuxiliary.cpp
- g++ $(CCOPTS) -c FGAuxiliary.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGAuxiliary.cpp
+
FGCoefficient.o : FGCoefficient.cpp
- g++ $(CCOPTS) -c FGCoefficient.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGCoefficient.cpp
+
FGFCS.o : FGFCS.cpp
- g++ $(CCOPTS) -c FGFCS.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGFCS.cpp
+
FGFDMExec.o : FGFDMExec.cpp
- g++ $(CCOPTS) -c FGFDMExec.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGFDMExec.cpp
+
FGModel.o : FGModel.cpp
- g++ $(CCOPTS) -c FGModel.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGModel.cpp
+
FGOutput.o : FGOutput.cpp
- g++ $(CCOPTS) -c FGOutput.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGOutput.cpp
+
FGPosition.o : FGPosition.cpp
- g++ $(CCOPTS) -c FGPosition.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGPosition.cpp
+
FGRotation.o : FGRotation.cpp
- g++ $(CCOPTS) -c FGRotation.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGRotation.cpp
+
FGState.o : FGState.cpp
- g++ $(CCOPTS) -c FGState.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGState.cpp
+
FGTranslation.o : FGTranslation.cpp
- g++ $(CCOPTS) -c FGTranslation.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGTranslation.cpp
+
FGUtility.o : FGUtility.cpp
- g++ $(CCOPTS) -c FGUtility.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGUtility.cpp
+
FGEngine.o : FGEngine.cpp
- g++ $(CCOPTS) -c FGEngine.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGEngine.cpp
+
FGTank.o : FGTank.cpp
- g++ $(CCOPTS) -c FGTank.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGTank.cpp
+
FGInitialCondition.o : FGInitialCondition.cpp
- g++ $(CCOPTS) -c FGInitialCondition.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGInitialCondition.cpp
+
+FGfdmSocket.o : FGfdmSocket.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGfdmSocket.cpp
+
+FGConfigFile.o : FGConfigFile.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGConfigFile.cpp
+
+FGLGear.o : FGLGear.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGLGear.cpp
+
+FGMatrix.o : FGMatrix.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c FGMatrix.cpp
+
JSBSim.o : JSBSim.cpp
- g++ $(CCOPTS) -c JSBSim.cpp
+ g++ $(INCLUDES) $(CCOPTS) -c JSBSim.cpp
+
+gpswitch.o: gptest.cpp
+ g++ -DSWITCH $(INCLUDES) $(CCOPTS) -c gptest.cpp -o gpswitch.o
+
+gpaop.o: gptest.cpp
+ g++ -DAOP $(INCLUDES) $(CCOPTS) -c gptest.cpp -o gpaop.o
+
+gpmap.o: gptest.cpp
+ g++ -DMAP $(INCLUDES) $(CCOPTS) -c gptest.cpp -o gpmap.o
+
+gpswitch: $(JSBSim_objects) gpswitch.o
+ g++ -DSWITCH $(LINKDIR) -o gpswitch gpswitch.o $(JSBSim_objects) -lm -lFCSComponents
+
+gpaop: $(JSBSim_objects) gpaop.o
+ g++ -Daop $(LINKDIR) -o gpaop gpaop.o $(JSBSim_objects) -lm -lFCSComponents
+
+gpmap: $(JSBSim_objects) gpmap.o
+ g++ -Dmap $(LINKDIR) -o gpmap gpmap.o $(JSBSim_objects) -lm -lFCSComponents
clean:
mv *.*~ backup
debug:
env CCOPTS=-g -WALL
make all
-
-
-
-
-
-
-
-
projects / flightgear.git / commitdiff