From: curt Date: Mon, 24 Apr 2000 23:49:06 +0000 (+0000) Subject: Updated JSBsim code. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=ad4416e143b5bd321737194dc38a4e83a4a14a47;p=flightgear.git Updated JSBsim code. --- diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index 9fb14d933..06462438e 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -119,13 +119,13 @@ int FGJSBsim::update( int multiloop ) { } // 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 ) ); @@ -180,9 +180,9 @@ int FGJSBsim::copy_to_JSBsim() { 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, @@ -203,15 +203,16 @@ int FGJSBsim::copy_from_JSBsim() { //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()); @@ -235,7 +236,7 @@ int FGJSBsim::copy_from_JSBsim() { 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() ); diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index 59e02b0bf..65ea475ec 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -39,7 +39,7 @@ HISTORY 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 @@ -101,8 +101,8 @@ Control INCLUDES *******************************************************************************/ -#include #include +#include #ifdef FGFS # ifndef __BORLANDC__ @@ -132,228 +132,106 @@ INCLUDES ************************************ 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 // @@ -411,28 +289,25 @@ void FGAircraft::MassChange() // Calculate new CG here. - Xt = Yt = Zt = Tw = 0; - Xw = Yw = Zw = 0; + Tw = 0; for (t=0; tGetX()*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; tGetX()-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; @@ -442,35 +317,23 @@ void FGAircraft::MassChange() } +/******************************************************************************/ 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 @@ -478,63 +341,233 @@ void FGAircraft::FMAero(void) // 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;iCalcThrust(); + 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; } +/******************************************************************************/ + + diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h index e2abf06e8..b3dcaa1f8 100644 --- a/src/FDM/JSBSim/FGAircraft.h +++ b/src/FDM/JSBSim/FGAircraft.h @@ -99,15 +99,15 @@ INCLUDES #ifdef FGFS # include # ifdef FG_HAVE_STD_INCLUDES -# include # include +# include # else -# include # include +# include # endif #else -# include # include +# include #endif #include "FGModel.h" @@ -115,6 +115,8 @@ INCLUDES #include "FGEngine.h" #include "FGTank.h" #include "FGLGear.h" +#include "FGConfigFile.h" +#include "FGMatrix.h" /******************************************************************************* DEFINITIONS @@ -128,6 +130,10 @@ CLASS DECLARATION 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); @@ -144,43 +150,38 @@ public: 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; @@ -189,8 +190,15 @@ private: FGTank* Tank[MAX_TANKS]; FGEngine *Engine[MAX_ENGINES]; - FGCoefficient *Coeff[6][10]; - int coeff_ctr[6]; + typedef map AxisIndex; + AxisIndex AxisIdx; + + typedef vector CoeffArray; + typedef vector CoeffVector; + + CoeffVector Coeff; + + void DisplayCoeffFactors(int multipliers); bool GearUp; @@ -204,6 +212,14 @@ private: string Axis[6]; vector lGear; + string AircraftPath; + string EnginePath; + void ReadMetrics(FGConfigFile*); + void ReadPropulsion(FGConfigFile*); + void ReadFlightControls(FGConfigFile*); + void ReadAerodynamics(FGConfigFile*); + void ReadUndercarriage(FGConfigFile*); + void ReadPrologue(FGConfigFile*); protected: diff --git a/src/FDM/JSBSim/FGAtmosphere.cpp b/src/FDM/JSBSim/FGAtmosphere.cpp index d494c1d31..c73da9765 100644 --- a/src/FDM/JSBSim/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/FGAtmosphere.cpp @@ -68,10 +68,11 @@ FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex) 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; } @@ -83,139 +84,104 @@ FGAtmosphere::~FGAtmosphere() 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; } diff --git a/src/FDM/JSBSim/FGAtmosphere.h b/src/FDM/JSBSim/FGAtmosphere.h index ac6f0d237..9f6407dc9 100644 --- a/src/FDM/JSBSim/FGAtmosphere.h +++ b/src/FDM/JSBSim/FGAtmosphere.h @@ -66,18 +66,27 @@ public: ~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: @@ -85,10 +94,10 @@ private: 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); diff --git a/src/FDM/JSBSim/FGAuxiliary.cpp b/src/FDM/JSBSim/FGAuxiliary.cpp index 5791b7706..b02e51c8a 100644 --- a/src/FDM/JSBSim/FGAuxiliary.cpp +++ b/src/FDM/JSBSim/FGAuxiliary.cpp @@ -27,12 +27,11 @@ 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 @@ -59,8 +58,8 @@ INCLUDES 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; } @@ -72,37 +71,44 @@ FGAuxiliary::~FGAuxiliary() 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){} diff --git a/src/FDM/JSBSim/FGCoefficient.cpp b/src/FDM/JSBSim/FGCoefficient.cpp index 2ed2d69ef..0251aa44c 100644 --- a/src/FDM/JSBSim/FGCoefficient.cpp +++ b/src/FDM/JSBSim/FGCoefficient.cpp @@ -45,274 +45,151 @@ INCLUDES *******************************************************************************/ #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> 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) { @@ -323,6 +200,7 @@ bool FGCoefficient::Allocate(int r, int c) return true; } +/******************************************************************************/ bool FGCoefficient::Allocate(int n) { @@ -332,6 +210,7 @@ bool FGCoefficient::Allocate(int n) return true; } +/******************************************************************************/ float FGCoefficient::Value(float rVal, float cVal) { @@ -355,12 +234,13 @@ float FGCoefficient::Value(float rVal, float cVal) SD = Value = col1temp + cFactor*(col2temp - col1temp); for (midx=0;midxGetParameter(mult_idx[midx]); } return Value; } +/******************************************************************************/ float FGCoefficient::Value(float Val) { @@ -382,27 +262,30 @@ float FGCoefficient::Value(float Val) SD = Value = Factor*(Table3D[r][1] - Table3D[r-1][1]) + Table3D[r-1][1]; for (midx=0;midxGetParameter(mult_idx[midx]); } return Value; } +/******************************************************************************/ float FGCoefficient::Value(void) { float Value; int midx; - + SD = Value = StaticValue; for (midx=0;midxGetParameter(mult_idx[midx]); } return Value; } +/******************************************************************************/ + float FGCoefficient::TotalValue() { switch(type) { @@ -411,60 +294,21 @@ float FGCoefficient::TotalValue() 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; } + +/******************************************************************************/ + diff --git a/src/FDM/JSBSim/FGCoefficient.h b/src/FDM/JSBSim/FGCoefficient.h index a3c494667..ea20976ef 100644 --- a/src/FDM/JSBSim/FGCoefficient.h +++ b/src/FDM/JSBSim/FGCoefficient.h @@ -40,18 +40,14 @@ INCLUDES #ifdef FGFS # include # include STL_STRING -# ifdef FG_HAVE_STD_INCLUDES -# include -# else -# include -# endif FG_USING_STD(string); #else # include -# include #endif #include +#include "FGConfigFile.h" +#include "FGDefs.h" /******************************************************************************* DEFINES @@ -59,25 +55,6 @@ 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 *******************************************************************************/ @@ -99,24 +76,39 @@ COMMENTS, REFERENCES, and NOTES 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 @@ -125,7 +117,7 @@ CLASS DECLARATION class FGCoefficient { public: - FGCoefficient(FGFDMExec*, ifstream&); + FGCoefficient(FGFDMExec*, FGConfigFile*); ~FGCoefficient(void); bool Allocate(int); bool Allocate(int, int); @@ -135,14 +127,14 @@ public: 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 CoeffMap; - CoeffMap coeffdef; + int numInstances; string filename; string description; string name; @@ -151,15 +143,13 @@ private: 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; diff --git a/src/FDM/JSBSim/FGConfigFile.cpp b/src/FDM/JSBSim/FGConfigFile.cpp new file mode 100644 index 000000000..8016df771 --- /dev/null +++ b/src/FDM/JSBSim/FGConfigFile.cpp @@ -0,0 +1,237 @@ +/******************************************************************************* + + 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 +#include + +/******************************************************************************* +************************************ 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("") != CurrentLine.npos) CommentsOn = true; + if (CurrentLine.find("") != 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; +} + diff --git a/src/FDM/JSBSim/FGConfigFile.h b/src/FDM/JSBSim/FGConfigFile.h new file mode 100644 index 000000000..8b40f53f7 --- /dev/null +++ b/src/FDM/JSBSim/FGConfigFile.h @@ -0,0 +1,94 @@ +/******************************************************************************* + + 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 +# include STL_STRING +# ifdef FG_HAVE_STD_INCLUDES +# include +# else +# include +# endif + FG_USING_STD(string); +#else +# include +# include +#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 diff --git a/src/FDM/JSBSim/FGControls.cpp b/src/FDM/JSBSim/FGControls.cpp index c8693a4f7..ed6ba2a73 100644 --- a/src/FDM/JSBSim/FGControls.cpp +++ b/src/FDM/JSBSim/FGControls.cpp @@ -51,6 +51,12 @@ FGControls::~FGControls() { // $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 diff --git a/src/FDM/JSBSim/FGControls.h b/src/FDM/JSBSim/FGControls.h index aaaa7dc2e..30f2e7879 100644 --- a/src/FDM/JSBSim/FGControls.h +++ b/src/FDM/JSBSim/FGControls.h @@ -177,6 +177,12 @@ extern FGControls controls; // $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. // diff --git a/src/FDM/JSBSim/FGDefs.h b/src/FDM/JSBSim/FGDefs.h index 152bbbb8d..3c00b117a 100644 --- a/src/FDM/JSBSim/FGDefs.h +++ b/src/FDM/JSBSim/FGDefs.h @@ -46,17 +46,49 @@ SENTRY #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 + diff --git a/src/FDM/JSBSim/FGEngine.cpp b/src/FDM/JSBSim/FGEngine.cpp index 7701445bf..7e4de5684 100644 --- a/src/FDM/JSBSim/FGEngine.cpp +++ b/src/FDM/JSBSim/FGEngine.cpp @@ -85,11 +85,7 @@ FGEngine::FGEngine(FGFDMExec* fdex, string enginePath, string engineName, int nu Output = FDMExec->GetOutput(); Name = engineName; -#ifdef MACOS - fullpath = enginePath + ":" + engineName + ".dat"; -#else fullpath = enginePath + "/" + engineName + ".dat"; -#endif ifstream enginefile(fullpath.c_str()); if (enginefile) { @@ -152,7 +148,7 @@ float FGEngine::CalcRocketThrust(void) { float lastThrust; - Throttle = FCS->GetThrottle(EngineNumber); + Throttle = FCS->GetThrottlePos(EngineNumber); lastThrust = Thrust; // last actual thrust if (Throttle < MinThrottle || Starved) { @@ -160,7 +156,7 @@ float FGEngine::CalcRocketThrust(void) 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; } @@ -175,7 +171,7 @@ float FGEngine::CalcPistonThrust(void) { float v,h,pa; - Throttle = FCS->GetThrottle(EngineNumber); + Throttle = FCS->GetThrottlePos(EngineNumber); Throttle /= 100; v=State->GetVt(); diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index eede2ea59..515eed5e3 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -48,6 +48,13 @@ INCLUDES #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 ************************************** *******************************************************************************/ @@ -58,28 +65,99 @@ FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) Name = "FGFCS"; } +/******************************************************************************/ FGFCS::~FGFCS(void) { } +/******************************************************************************/ bool FGFCS::Run(void) { if (!FGModel::Run()) { - + + for (unsigned int i=0; iRun(); + } else { } return false; } +/******************************************************************************/ + +void FGFCS::SetThrottleCmd(int engineNum, float setting) +{ + if (engineNum < 0) { + for (int ctr=0;ctrGetNumEngines();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;ctrGetNumEngines();ctr++) Throttle[ctr] = setting; + for (int ctr=0;ctrGetNumEngines();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(); } + diff --git a/src/FDM/JSBSim/FGFCS.h b/src/FDM/JSBSim/FGFCS.h index 1980cd0af..24f7d0eb7 100644 --- a/src/FDM/JSBSim/FGFCS.h +++ b/src/FDM/JSBSim/FGFCS.h @@ -38,7 +38,24 @@ SENTRY INCLUDES *******************************************************************************/ +#ifdef FGFS +# include +# include STL_STRING + FG_USING_STD(string); +# ifdef FG_HAVE_STD_INCLUDES +# include +# else +# include +# endif +#else +# include +# include +#endif + +#include "filtersjb/FGFCSComponent.h" #include "FGModel.h" +#include "FGConfigFile.h" + /******************************************************************************* CLASS DECLARATION @@ -48,32 +65,60 @@ using namespace std; 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 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 diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index df4733a05..9e349fa9e 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -119,7 +119,7 @@ FGFDMExec::FGFDMExec(void) Schedule(Translation, 1); Schedule(Position, 1); Schedule(Auxiliary, 1); - Schedule(Output, 60); + Schedule(Output, 1); terminate = false; frozen = false; diff --git a/src/FDM/JSBSim/FGFDMExec.h b/src/FDM/JSBSim/FGFDMExec.h index bfed56142..680f4d6b5 100644 --- a/src/FDM/JSBSim/FGFDMExec.h +++ b/src/FDM/JSBSim/FGFDMExec.h @@ -28,7 +28,7 @@ HISTORY 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 @@ -64,8 +64,8 @@ class FGInitialCondition; class FGFDMExec { public: - FGFDMExec(void); - ~FGFDMExec(void); + FGFDMExec::FGFDMExec(void); + FGFDMExec::~FGFDMExec(void); FGModel* FirstModel; diff --git a/src/FDM/JSBSim/FGInitialCondition.cpp b/src/FDM/JSBSim/FGInitialCondition.cpp index 7445440ed..64ad0a4e8 100644 --- a/src/FDM/JSBSim/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/FGInitialCondition.cpp @@ -78,16 +78,15 @@ FGInitialCondition::FGInitialCondition(FGFDMExec *fdmex) 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) @@ -98,12 +97,13 @@ 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) diff --git a/src/FDM/JSBSim/FGInitialCondition.h b/src/FDM/JSBSim/FGInitialCondition.h index a53a0c5bc..db02ead79 100644 --- a/src/FDM/JSBSim/FGInitialCondition.h +++ b/src/FDM/JSBSim/FGInitialCondition.h @@ -69,9 +69,11 @@ class FGInitialCondition 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 diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index 05417b0d2..21b7f28b6 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -44,9 +44,10 @@ INCLUDES *******************************************************************************/ -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; } diff --git a/src/FDM/JSBSim/FGLGear.h b/src/FDM/JSBSim/FGLGear.h index 4378fe93f..ce5195c94 100644 --- a/src/FDM/JSBSim/FGLGear.h +++ b/src/FDM/JSBSim/FGLGear.h @@ -39,16 +39,16 @@ COMMENTS, REFERENCES, and NOTES ******************************************************************************** [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 @@ -57,17 +57,13 @@ INCLUDES #ifdef FGFS # include # include STL_STRING -# ifdef FG_HAVE_STD_INCLUDES -# include -# else -# include -# endif FG_USING_STD(string); #else -# include # include #endif +#include "FGConfigFile.h" + /******************************************************************************* DEFINITIONS *******************************************************************************/ @@ -81,7 +77,7 @@ CLASS DECLARATION class FGLGear { public: - FGLGear(ifstream&); + FGLGear(FGConfigFile*); ~FGLGear(void); float Force(void); diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp index 1c1fcf937..87c71a1b5 100644 --- a/src/FDM/JSBSim/FGMatrix.cpp +++ b/src/FDM/JSBSim/FGMatrix.cpp @@ -1,397 +1,536 @@ /******************************************************************************* 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 #include "FGMatrix.h" -#include -#include -#include - -/******************************************************************************* -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(); } diff --git a/src/FDM/JSBSim/FGMatrix.h b/src/FDM/JSBSim/FGMatrix.h index b3591b946..c7a45d03b 100644 --- a/src/FDM/JSBSim/FGMatrix.h +++ b/src/FDM/JSBSim/FGMatrix.h @@ -1,14 +1,15 @@ /******************************************************************************* 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 *******************************************************************************/ @@ -20,78 +21,113 @@ INCLUDES *******************************************************************************/ #include -#include -#include +#ifdef FGFS +# include +# include STL_STRING +# ifdef FG_HAVE_STD_INCLUDES +# include +# else +# include +# endif + FG_USING_STD(string); +#else +# include +# include +#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; }; /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGModel.h b/src/FDM/JSBSim/FGModel.h index 621719a57..709758bee 100644 --- a/src/FDM/JSBSim/FGModel.h +++ b/src/FDM/JSBSim/FGModel.h @@ -88,6 +88,13 @@ public: 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; diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index c3b145b44..c2f0af287 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -64,12 +64,14 @@ FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) #endif } +/******************************************************************************/ FGOutput::~FGOutput(void) { if (socket) delete socket; } +/******************************************************************************/ bool FGOutput::Run(void) { @@ -82,208 +84,135 @@ 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) { @@ -365,9 +294,10 @@ void FGOutput::SocketOutput(void) socket->Append(FCS->GetDa()); socket->Append(FCS->GetDe()); socket->Append(FCS->GetDr()); - socket->Send(); + socket->Send(); */ } +/******************************************************************************/ void FGOutput::SocketStatusOutput(string out_str) { @@ -381,3 +311,5 @@ void FGOutput::SocketStatusOutput(string out_str) socket->Send(); } +/******************************************************************************/ + diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 436538c89..79e5d60b9 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -80,96 +80,61 @@ INCLUDES *******************************************************************************/ -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(); @@ -179,6 +144,7 @@ void FGPosition::GetState(void) Radius = State->Geth() + EARTHRAD; } +/******************************************************************************/ void FGPosition::PutState(void) { @@ -187,3 +153,5 @@ void FGPosition::PutState(void) State->Seth(Radius - EARTHRAD); } +/******************************************************************************/ + diff --git a/src/FDM/JSBSim/FGPosition.h b/src/FDM/JSBSim/FGPosition.h index 8789f8a6b..0bff7bbfb 100644 --- a/src/FDM/JSBSim/FGPosition.h +++ b/src/FDM/JSBSim/FGPosition.h @@ -41,7 +41,9 @@ SENTRY /******************************************************************************* INCLUDES *******************************************************************************/ + #include "FGModel.h" +#include "FGMatrix.h" using namespace std; @@ -51,40 +53,11 @@ CLASS DECLARATION 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; @@ -92,6 +65,18 @@ private: 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); }; /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp index 4385e13aa..9607fc1f1 100644 --- a/src/FDM/JSBSim/FGRotation.cpp +++ b/src/FDM/JSBSim/FGRotation.cpp @@ -68,7 +68,7 @@ INCLUDES #ifndef M_PI /* get a definition for pi */ -#include +#include #define M_PI FG_PI #endif @@ -77,106 +77,64 @@ INCLUDES *******************************************************************************/ -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) -{ -} +/******************************************************************************/ diff --git a/src/FDM/JSBSim/FGRotation.h b/src/FDM/JSBSim/FGRotation.h index 09452f5d0..edf1ec64d 100644 --- a/src/FDM/JSBSim/FGRotation.h +++ b/src/FDM/JSBSim/FGRotation.h @@ -68,6 +68,7 @@ INCLUDES #endif #include "FGModel.h" +#include "FGMatrix.h" using namespace std; @@ -77,67 +78,28 @@ CLASS DECLARATION 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);} }; /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 069228189..2170072a5 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -63,7 +63,10 @@ INCLUDES *******************************************************************************/ -FGState::FGState(FGFDMExec* fdex) +FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), + mTl2b(3,3), + mTs2b(3,3), + vQtrn(4) { FDMExec = fdex; @@ -75,26 +78,58 @@ FGState::FGState(FGFDMExec* 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()); @@ -129,13 +164,14 @@ void FGState::Initialize(float U, float V, float W, 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) @@ -147,36 +183,19 @@ void FGState::Initialize(float U, float V, float W, 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) { @@ -194,23 +213,24 @@ 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; @@ -220,77 +240,242 @@ bool FGState::StoreData(string fname) } } +/******************************************************************************/ -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; +} + +/******************************************************************************/ + diff --git a/src/FDM/JSBSim/FGState.h b/src/FDM/JSBSim/FGState.h index 9311cba4b..6391165d4 100644 --- a/src/FDM/JSBSim/FGState.h +++ b/src/FDM/JSBSim/FGState.h @@ -58,8 +58,10 @@ INCLUDES # include #endif +#include #include "FGDefs.h" #include "FGInitialCondition.h" +#include "FGMatrix.h" /******************************************************************************* DEFINES @@ -79,12 +81,10 @@ public: 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;} @@ -104,6 +104,9 @@ public: 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;} @@ -124,8 +127,16 @@ public: 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: @@ -140,9 +151,17 @@ private: FGFDMExec* FDMExec; float LocalAltitudeOverRunway; + FGMatrix mTb2l; + FGMatrix mTl2b; + FGMatrix mTs2b; + FGColumnVector vQtrn; -protected: + typedef map CoeffMap; + CoeffMap coeffdef; +protected: + enum {ePhi=1, eTht, ePsi}; + enum {eP=1, eQ, eR}; }; /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGTank.cpp b/src/FDM/JSBSim/FGTank.cpp index ea1180695..c08ff94e1 100644 --- a/src/FDM/JSBSim/FGTank.cpp +++ b/src/FDM/JSBSim/FGTank.cpp @@ -42,22 +42,23 @@ INCLUDES *******************************************************************************/ -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 } diff --git a/src/FDM/JSBSim/FGTank.h b/src/FDM/JSBSim/FGTank.h index 8ffaf3bac..9d43f17b9 100644 --- a/src/FDM/JSBSim/FGTank.h +++ b/src/FDM/JSBSim/FGTank.h @@ -43,24 +43,21 @@ SENTRY /******************************************************************************* INCLUDES *******************************************************************************/ + #ifdef FGFS # include # include STL_STRING -# ifdef FG_HAVE_STD_INCLUDES -# include -# else -# include -# endif FG_USING_STD(string); #else # include -# include #endif +#include "FGConfigFile.h" + /******************************************************************************* DEFINES *******************************************************************************/ - + using namespace std; /******************************************************************************* @@ -70,7 +67,7 @@ CLASS DECLARATION class FGTank { public: - FGTank(ifstream&); + FGTank(FGConfigFile*); ~FGTank(void); float Reduce(float); @@ -83,7 +80,7 @@ public: float inline GetZ(void) {return Z;} enum TankType {ttUNKNOWN, ttFUEL, ttOXIDIZER}; - + private: TankType Type; float X, Y, Z; diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp index 2191efc2e..717458989 100644 --- a/src/FDM/JSBSim/FGTranslation.cpp +++ b/src/FDM/JSBSim/FGTranslation.cpp @@ -33,7 +33,7 @@ HISTORY -------------------------------------------------------------------------------- 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 @@ -73,74 +73,82 @@ INCLUDES *******************************************************************************/ -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) { diff --git a/src/FDM/JSBSim/FGTranslation.h b/src/FDM/JSBSim/FGTranslation.h index 3e2857b9d..62f298bd7 100644 --- a/src/FDM/JSBSim/FGTranslation.h +++ b/src/FDM/JSBSim/FGTranslation.h @@ -68,6 +68,7 @@ INCLUDES #endif #include "FGModel.h" +#include "FGMatrix.h" using namespace std; @@ -81,23 +82,13 @@ public: 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;} @@ -110,13 +101,11 @@ public: 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; diff --git a/src/FDM/JSBSim/FGUtility.cpp b/src/FDM/JSBSim/FGUtility.cpp index 9ed89e70f..5bc366d73 100644 --- a/src/FDM/JSBSim/FGUtility.cpp +++ b/src/FDM/JSBSim/FGUtility.cpp @@ -59,7 +59,7 @@ INCLUDES #ifndef M_PI_2 /* get a definition for pi */ -#include +#include #define M_PI_2 FG_PI_2 #endif @@ -82,12 +82,12 @@ FGUtility::~FGUtility() float FGUtility::ToGeodetic() { - return 0.0; + return 0.0; } float FGUtility:: FromGeodetic() { - return 0.0; + return 0.0; } diff --git a/src/FDM/JSBSim/FGfdmSocket.cpp b/src/FDM/JSBSim/FGfdmSocket.cpp index f6381b23c..ef1d8885e 100644 --- a/src/FDM/JSBSim/FGfdmSocket.cpp +++ b/src/FDM/JSBSim/FGfdmSocket.cpp @@ -47,7 +47,7 @@ FGfdmSocket::FGfdmSocket(string address, int port) { 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); @@ -86,10 +86,6 @@ FGfdmSocket::FGfdmSocket(string address, int port) } } -#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); @@ -97,7 +93,6 @@ FGfdmSocket::~FGfdmSocket(void) WSACleanup(); #endif } -#endif void FGfdmSocket::Clear(void) { diff --git a/src/FDM/JSBSim/JSBSim.cpp b/src/FDM/JSBSim/JSBSim.cpp index c9ed9ee8d..1b8f3c831 100644 --- a/src/FDM/JSBSim/JSBSim.cpp +++ b/src/FDM/JSBSim/JSBSim.cpp @@ -39,18 +39,22 @@ HISTORY INCLUDES *******************************************************************************/ -#if __BCPLUSPLUS__ >= 0x0540 // If compiling under Borland C++Builder -//--------------------------------------------------------------------------- +#if __BCPLUSPLUS__ >= 0x0540 // If compiling under Borland C++Builder #pragma hdrstop #include -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"); @@ -58,10 +62,15 @@ USEUNIT("FGRotation.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 @@ -92,7 +101,7 @@ USEUNIT("FGfdmSocket.cpp"); int main(int argc, char** argv) { - FGFDMExec* FDMExec; + FGFDMExec* FDMExec; if (argc != 3) { cout << endl @@ -104,33 +113,37 @@ int main(int argc, char** argv) 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 + diff --git a/src/FDM/JSBSim/Makefile.am b/src/FDM/JSBSim/Makefile.am index bbf80f4b2..b27027c7f 100644 --- a/src/FDM/JSBSim/Makefile.am +++ b/src/FDM/JSBSim/Makefile.am @@ -1,3 +1,5 @@ +SUBDIRS = filtersjb + EXTRA_DIST = JSBSim.cpp Makefile.solo noinst_LIBRARIES = libJSBsim.a @@ -6,11 +8,14 @@ libJSBsim_a_SOURCES = FGAircraft.cpp FGAircraft.h \ 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 \ @@ -26,8 +31,8 @@ noinst_PROGRAMS = testJSBsim 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 diff --git a/src/FDM/JSBSim/Makefile.solo b/src/FDM/JSBSim/Makefile.solo index 661c1dfb0..3b31517af 100644 --- a/src/FDM/JSBSim/Makefile.solo +++ b/src/FDM/JSBSim/Makefile.solo @@ -1,41 +1,95 @@ -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 @@ -48,11 +102,3 @@ all: debug: env CCOPTS=-g -WALL make all - - - - - - - -