From ea80ed959e180356dc03b62484477bdfcf20952b Mon Sep 17 00:00:00 2001 From: curt Date: Tue, 2 May 2000 18:25:30 +0000 Subject: [PATCH] Partial JSBsim update. --- src/FDM/JSBSim.cxx | 15 +- src/FDM/JSBSim/FGAircraft.cpp | 119 +++++++- src/FDM/JSBSim/FGAircraft.h | 28 +- src/FDM/JSBSim/FGCoefficient.h | 6 +- src/FDM/JSBSim/FGFCS.cpp | 5 +- src/FDM/JSBSim/FGFDMExec.cpp | 7 +- src/FDM/JSBSim/FGInitialCondition.cpp | 225 +++++++++++---- src/FDM/JSBSim/FGInitialCondition.h | 11 +- src/FDM/JSBSim/FGModel.h | 2 +- src/FDM/JSBSim/FGOutput.cpp | 377 +++++++++++++++++++------- src/FDM/JSBSim/FGOutput.h | 6 + src/FDM/JSBSim/FGPosition.cpp | 2 +- src/FDM/JSBSim/FGRotation.cpp | 56 ++-- src/FDM/JSBSim/FGRotation.h | 2 + src/FDM/JSBSim/FGState.cpp | 6 +- src/FDM/JSBSim/FGTranslation.cpp | 77 +++--- src/FDM/JSBSim/FGTranslation.h | 6 +- src/FDM/JSBSim/JSBSim.h | 215 +++++++++++++++ 18 files changed, 912 insertions(+), 253 deletions(-) create mode 100644 src/FDM/JSBSim/JSBSim.h diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index dc40c5a32..e2d550b00 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -64,10 +64,12 @@ int FGJSBsim::init( double dt ) { FGPath engine_path( current_options.get_fg_root() ); engine_path.append( "Engine" ); + FDMExec.GetState()->Setdt( dt ); + FDMExec.GetAircraft()->LoadAircraft( aircraft_path.str(), - engine_path.str(), + engine_path.str(), current_options.get_aircraft() ); - FG_LOG( FG_FLIGHT, FG_INFO, " loaded aircraft" << + FG_LOG( FG_FLIGHT, FG_INFO, " loaded aircraft" << current_options.get_aircraft() ); FG_LOG( FG_FLIGHT, FG_INFO, "Initializing JSBsim with:" ); @@ -85,9 +87,9 @@ int FGJSBsim::init( double dt ) { current_options.get_uBody(), current_options.get_vBody(), current_options.get_wBody(), - get_Phi() * DEGTORAD, - get_Theta() * DEGTORAD, - get_Psi() * DEGTORAD, + get_Phi(), + get_Theta(), + get_Psi(), get_Latitude(), get_Longitude(), get_Altitude() @@ -95,7 +97,6 @@ int FGJSBsim::init( double dt ) { FG_LOG( FG_FLIGHT, FG_INFO, " loaded initial conditions" ); - FDMExec.GetState()->Setdt( dt ); FG_LOG( FG_FLIGHT, FG_INFO, " set dt" ); FG_LOG( FG_FLIGHT, FG_INFO, "Finished initializing JSBsim" ); @@ -129,6 +130,8 @@ int FGJSBsim::update( int multiloop ) { FDMExec.GetFCS()->SetDspCmd( 0.0 ); FDMExec.GetFCS()->SetThrottleCmd( FGControls::ALL_ENGINES, controls.get_throttle( 0 ) * 100.0 ); + FDMExec.GetFCS()->SetThrottlePos( FGControls::ALL_ENGINES, + controls.get_throttle( 0 ) * 100.0 ); // FCS->SetBrake( controls.get_brake( 0 ) ); // Inform JSBsim of the local terrain altitude diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index ce8f1990b..763a0787f 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -139,13 +139,14 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex), vbaseXYZcg(3), vXYZcg(3), vXYZep(3), - vEuler(3) + vEuler(3), + vFs(3) { Name = "FGAircraft"; - AxisIdx["LIFT"] = 0; + AxisIdx["DRAG"] = 0; AxisIdx["SIDE"] = 1; - AxisIdx["DRAG"] = 2; + AxisIdx["LIFT"] = 2; AxisIdx["ROLL"] = 3; AxisIdx["PITCH"] = 4; AxisIdx["YAW"] = 5; @@ -197,6 +198,8 @@ bool FGAircraft::LoadAircraft(string aircraft_path, string engine_path, string f } else if (token == "FLIGHT_CONTROL") { cout << " Reading Flight Control" << endl; ReadFlightControls(&AC_cfg); + } else if (token == "OUTPUT") { + ReadOutput(&AC_cfg); } } @@ -322,7 +325,6 @@ void FGAircraft::MassChange() void FGAircraft::FMAero(void) { - static FGColumnVector vFs(3); static FGColumnVector vDXYZcg(3); unsigned int axis_ctr,ctr; @@ -348,7 +350,7 @@ void FGAircraft::FMAero(void) 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 + vMoments(eN) += vForces(eY)*vDXYZcg(eX) - vForces(eX)*vDXYZcg(eY); // yawing moment for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) { for (ctr = 0; ctr < Coeff[axis_ctr+3].size(); ctr++) { @@ -511,6 +513,71 @@ void FGAircraft::ReadUndercarriage(FGConfigFile* AC_cfg) /******************************************************************************/ +void FGAircraft::ReadOutput(FGConfigFile* AC_cfg) +{ + string token, parameter; + int OutRate = 0; + int subsystems = 0; + + token = AC_cfg->GetValue("NAME"); + Output->SetFilename(token); + token = AC_cfg->GetValue("TYPE"); + Output->SetType(token); + AC_cfg->GetNextConfigLine(); + + while ((token = AC_cfg->GetValue()) != "/OUTPUT") { + *AC_cfg >> parameter; + if (parameter == "RATE_IN_HZ") *AC_cfg >> OutRate; + if (parameter == "SIMULATION") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssSimulation; + } + if (parameter == "AEROSURFACES") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssAerosurfaces; + } + if (parameter == "RATES") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssRates; + } + if (parameter == "VELOCITIES") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssVelocities; + } + if (parameter == "FORCES") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssForces; + } + if (parameter == "MOMENTS") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssMoments; + } + if (parameter == "ATMOSPHERE") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssAtmosphere; + } + if (parameter == "MASSPROPS") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssMassProps; + } + if (parameter == "POSITION") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssPosition; + } + if (parameter == "COEFFICIENTS") { + *AC_cfg >> parameter; + if (parameter == "ON") subsystems += ssCoefficients; + } + } + + Output->SetSubsystems(subsystems); + + OutRate = OutRate>120?120:(OutRate<0?0:OutRate); + Output->SetRate( (int)(0.5 + 1.0/(State->Getdt()*OutRate)) ); +} + +/******************************************************************************/ + void FGAircraft::ReadPrologue(FGConfigFile* AC_cfg) { string token = AC_cfg->GetValue(); @@ -571,4 +638,46 @@ void FGAircraft::DisplayCoeffFactors(int multipliers) /******************************************************************************/ +string FGAircraft::GetCoefficientStrings(void) +{ + string CoeffStrings = ""; + bool firstime = true; + + for (unsigned int axis = 0; axis < 6; axis++) { + for (unsigned int sd = 0; sd < Coeff[axis].size(); sd++) { + if (firstime) { + firstime = false; + } else { + CoeffStrings += ", "; + } + CoeffStrings += Coeff[axis][sd].Getname(); + } + } + + return CoeffStrings; +} + +/******************************************************************************/ + +string FGAircraft::GetCoefficientValues(void) +{ + string SDValues = ""; + char buffer[10]; + bool firstime = true; + + for (unsigned int axis = 0; axis < 6; axis++) { + for (unsigned int sd = 0; sd < Coeff[axis].size(); sd++) { + if (firstime) { + firstime = false; + } else { + SDValues += ", "; + } + sprintf(buffer, "%9.6f", Coeff[axis][sd].GetSD()); + SDValues += string(buffer); + } + } + + return SDValues;; +} + diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h index b3dcaa1f8..f62df7bee 100644 --- a/src/FDM/JSBSim/FGAircraft.h +++ b/src/FDM/JSBSim/FGAircraft.h @@ -134,6 +134,7 @@ class FGAircraft : public FGModel enum {eX=1, eY, eZ}; enum {eP=1, eQ, eR}; enum {ePhi=1, eTht, ePsi}; + public: FGAircraft(FGFDMExec*); ~FGAircraft(void); @@ -152,13 +153,27 @@ public: inline float GetMass(void) {return Mass;} inline FGColumnVector GetMoments(void) {return vMoments;} inline FGColumnVector GetForces(void) {return vForces;} + inline FGColumnVector GetvFs(void) {return vFs;} 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 int GetNumEngines(void) {return numEngines;} inline FGColumnVector GetXYZcg(void) {return vXYZcg;} + string GetCoefficientStrings(void); + string GetCoefficientValues(void); + enum { ssSimulation = 1, + ssAerosurfaces = 2, + ssRates = 4, + ssVelocities = 8, + ssForces = 16, + ssMoments = 32, + ssAtmosphere = 64, + ssMassProps = 128, + ssCoefficients = 256, + ssPosition = 512 } subsystems; + private: void GetState(void); void FMAero(void); @@ -168,6 +183,7 @@ private: void MassChange(void); FGColumnVector vMoments; FGColumnVector vForces; + FGColumnVector vFs; FGColumnVector vXYZrp; FGColumnVector vbaseXYZcg; FGColumnVector vXYZcg; @@ -202,14 +218,6 @@ private: bool GearUp; - enum Param {LiftCoeff, - DragCoeff, - SideCoeff, - RollCoeff, - PitchCoeff, - YawCoeff, - numCoeffs}; - string Axis[6]; vector lGear; string AircraftPath; @@ -220,9 +228,7 @@ private: void ReadAerodynamics(FGConfigFile*); void ReadUndercarriage(FGConfigFile*); void ReadPrologue(FGConfigFile*); - -protected: - + void ReadOutput(FGConfigFile*); }; /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGCoefficient.h b/src/FDM/JSBSim/FGCoefficient.h index ea20976ef..ed83d053e 100644 --- a/src/FDM/JSBSim/FGCoefficient.h +++ b/src/FDM/JSBSim/FGCoefficient.h @@ -125,8 +125,10 @@ public: float Value(float); float Value(void); float TotalValue(void); - inline float GetSDValue(void) {return SD;} - inline void SetSDValue(float tt) {SD = tt;} + inline string Getname(void) {return name;} + inline float GetSD(void) {return SD;} +// 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}; diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index 3d35a59f3..5231ceb8b 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -97,7 +97,7 @@ void FGFCS::SetThrottleCmd(int engineNum, float setting) if (engineNum < 0) { for (int ctr=0;ctrGetNumEngines();ctr++) ThrottleCmd[ctr] = setting; } else { - ThrottlePos[engineNum] = setting; + ThrottleCmd[engineNum] = setting; } } @@ -106,8 +106,7 @@ void FGFCS::SetThrottleCmd(int engineNum, float setting) void FGFCS::SetThrottlePos(int engineNum, float setting) { if (engineNum < 0) { - for (int ctr=0;ctrGetNumEngines();ctr++) - ThrottlePos[ctr] = ThrottleCmd[ctr]; + for (int ctr=0;ctrGetNumEngines();ctr++) ThrottlePos[ctr] = ThrottleCmd[ctr]; } else { ThrottlePos[engineNum] = setting; } diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index 9e349fa9e..d5324fdd0 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -112,7 +112,7 @@ FGFDMExec::FGFDMExec(void) // instance, the atmosphere model gets executed every fifth pass it is called // by the executive. Everything else here gets executed each pass. - Schedule(Atmosphere, 5); + Schedule(Atmosphere, 1); Schedule(FCS, 1); Schedule(Aircraft, 1); Schedule(Rotation, 1); @@ -176,13 +176,16 @@ bool FGFDMExec::Run(void) return true; } + bool FGFDMExec::RunIC(FGInitialCondition *fgic) { - float save_dt=State->Getdt(); + float save_dt = State->Getdt(); + State->Setdt(0.0); State->Initialize(fgic); Run(); State->Setdt(save_dt); + return true; } diff --git a/src/FDM/JSBSim/FGInitialCondition.cpp b/src/FDM/JSBSim/FGInitialCondition.cpp index 64ad0a4e8..274defcce 100644 --- a/src/FDM/JSBSim/FGInitialCondition.cpp +++ b/src/FDM/JSBSim/FGInitialCondition.cpp @@ -62,159 +62,290 @@ INCLUDES #include "FGDefs.h" -FGInitialCondition::FGInitialCondition(FGFDMExec *fdmex) + + + +FGInitialCondition::FGInitialCondition(FGFDMExec *FDMExec) { - vt=vc=0; - mach=0; - alpha=beta=gamma=0; - theta=phi=psi=0; - altitude=hdot=0; - latitude=longitude=0; - - atm=fdmex->GetAtmosphere(); + vt=vc=0; + mach=0; + alpha=beta=gamma=0; + theta=phi=psi=0; + altitude=hdot=0; + latitude=longitude=0; + fdmex=FDMExec; + fdmex->GetPosition()->Seth(altitude); + fdmex->GetAtmosphere()->Run(); + } FGInitialCondition::~FGInitialCondition(void) {}; -/* void FGInitialCondition::SetVcalibratedKtsIC(float tt) +void FGInitialCondition::SetVcalibratedKtsIC(float tt) { - vc=tt*KTSTOFPS; - - vt=sqrt(atm->GetDensity(0)/atm->GetDensity(altitude)*vc*vc); - - //mach=vt*sqrt(SHRATIO*Reng*atm->GetTemperature(altitude)); + vc=tt*KTSTOFPS; + if(getMachFromVcas(&mach,vc)) { + vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed(); + } } - */ + void FGInitialCondition::SetVtrueKtsIC(float tt) { - vt=tt*KTSTOFPS; - //vc=sqrt(atm->GetDensity(altitude)/atm->GetDensity(0)*vt*vt); - //mach=vt*sqrt(SHRATIO*Reng*atm->GetTemperature(altitude)); + vt=tt*KTSTOFPS; + mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); + vc=calcVcas(mach); } -/* 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); -} */ + mach=tt; + vt=mach*fdmex->GetAtmosphere()->GetSoundSpeed(); + vc=calcVcas(mach); + //cout << "Vt: " << vt*FPSTOKTS << " Vc: " << vc*FPSTOKTS << endl; +} void FGInitialCondition::SetAltitudeFtIC(float tt) { - altitude=tt; - //mach=vt/sqrt(SHRATIO*Reng*atm->GetTemperature(altitude)); - //vc=sqrt(atm->GetDensity(altitude)/atm->GetDensity(0)*vt*vt); + altitude=tt; + fdmex->GetPosition()->Seth(altitude); + fdmex->GetAtmosphere()->Run(); + mach=vt/fdmex->GetAtmosphere()->GetSoundSpeed(); + vc=calcVcas(mach); } void FGInitialCondition::SetFlightPathAngleDegIC(float tt) { - gamma=tt*DEGTORAD; - theta=alpha+gamma; + gamma=tt*DEGTORAD; + theta=alpha+gamma; } void FGInitialCondition::SetAlphaDegIC(float tt) { - alpha=tt*DEGTORAD; - theta=alpha+gamma; + alpha=tt*DEGTORAD; + theta=alpha+gamma; } void FGInitialCondition::SetBetaDegIC(float tt) { - beta=tt*DEGTORAD; + beta=tt*DEGTORAD; } void FGInitialCondition::SetRollAngleDegIC(float tt) { - phi=tt*DEGTORAD; + phi=tt*DEGTORAD; } void FGInitialCondition::SetPitchAngleDegIC(float tt) { - theta=tt*DEGTORAD; - alpha=theta-gamma; + theta=tt*DEGTORAD; + alpha=theta-gamma; } void FGInitialCondition::SetHeadingDegIC(float tt) { - psi=tt*DEGTORAD; + psi=tt*DEGTORAD; } void FGInitialCondition::SetLatitudeDegIC(float tt) { - latitude=tt*DEGTORAD; + latitude=tt*DEGTORAD; } void FGInitialCondition::SetLongitudeDegIC(float tt) { - longitude=tt*DEGTORAD; + longitude=tt*DEGTORAD; } float FGInitialCondition::GetUBodyFpsIC(void) { - return vt*cos(alpha)*cos(beta); + return vt*cos(alpha)*cos(beta); } float FGInitialCondition::GetVBodyFpsIC(void) { - return vt*sin(beta); + return vt*sin(beta); } float FGInitialCondition::GetWBodyFpsIC(void) { - return vt*sin(alpha)*cos(beta); + return vt*sin(alpha)*cos(beta); } float FGInitialCondition::GetThetaRadIC(void) { - return theta; + return theta; } float FGInitialCondition::GetPhiRadIC(void) { - return phi; + return phi; } float FGInitialCondition::GetPsiRadIC(void) { - return psi; + return psi; } float FGInitialCondition::GetLatitudeRadIC(void) { - return latitude; + return latitude; } float FGInitialCondition::GetLongitudeRadIC(void) { - return longitude; + return longitude; } float FGInitialCondition::GetAltitudeFtIC(void) { - return altitude; + return altitude; } +float FGInitialCondition::calcVcas(float Mach) { + + float p=fdmex->GetAtmosphere()->GetPressure(); + float psl=fdmex->GetAtmosphere()->GetPressureSL(); + float rhosl=fdmex->GetAtmosphere()->GetDensitySL(); + float pt,A,B,D,vcas; + + 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 + + + //the normal shock assumption should not be a bad one -- most supersonic + //aircraft place the pitot probe out front so that it is the forward + //most point on the aircraft. The real shock would, of course, take + //on something like the shape of a rounded-off cone but, here again, + //the assumption should be good since the opening of the pitot probe + //is very small and, therefore, the effects of the shock curvature + //should be small as well. AFAIK, this approach is fairly well accepted + //within the aerospace community + + 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)); + //cout << "calcVcas: vcas= " << vcas*FPSTOKTS << " mach= " << Mach << " pressure: " << p << endl; + return vcas; +} + +bool FGInitialCondition::findMachInterval(float *mlo, float *mhi, float vcas) { + //void find_interval(inter_params &ip,eqfunc f,float y,float constant, int &flag){ + + int i=0; + bool found=false; + float flo,fhi,fguess; + float lo,hi,guess,step; + step=0.1; + guess=1.5; + fguess=calcVcas(guess)-vcas; + lo=hi=guess; + do{ + step=2*step; + lo-=step; + if(lo < 0) + lo=0; + hi+=step; + i++; + flo=calcVcas(lo)-vcas; + fhi=calcVcas(hi)-vcas; + if(flo*fhi <=0){ //found interval with root + found=true; + if(flo*fguess <= 0){ //narrow interval down a bit + hi=lo+step; //to pass solver interval that is as + //small as possible + } + else if(fhi*fguess <= 0){ + lo=hi-step; + } + } + //cout << "findMachInterval: i=" << i << " Lo= " << lo << " Hi= " << hi << endl; + } + while((found == 0) && (i <= 100)); + *mlo=lo; + *mhi=hi; + return found; +} + + + +bool FGInitialCondition::getMachFromVcas(float *Mach,float vcas) { + + + float x1,x2,x3,f1,f2,f3,d,d0; + float eps=1E-3; + float const relax =0.9; + int i; + bool success=false; + //initializations + if(findMachInterval(&x1,&x3,vcas)) { + + f1=calcVcas(x1)-vcas; + f3=calcVcas(x3)-vcas; + d0=fabs(x3-x1); + + //iterations + i=0; + while ((fabs(d) > eps) && (i < 100)){ + + d=(x3-x1)/d0; + x2=x1-d*d0*f1/(f3-f1); + f2=calcVcas(x2)-vcas; + if(f1*f2 <= 0.0){ + x3=x2; + f3=f2; + f1=relax*f1; + } + else if(f2*f3 <= 0){ + x1=x2; + f1=f2; + f3=relax*f3; + } + //cout << i << endl; + i++; + }//end while + if(i < 100) { + success=true; + *Mach=x2; + } + + } + //cout << "Success= " << success << " Vcas: " << vcas*FPSTOKTS << " Mach: " << *Mach << endl; + return success; +} diff --git a/src/FDM/JSBSim/FGInitialCondition.h b/src/FDM/JSBSim/FGInitialCondition.h index db02ead79..384a80420 100644 --- a/src/FDM/JSBSim/FGInitialCondition.h +++ b/src/FDM/JSBSim/FGInitialCondition.h @@ -69,10 +69,9 @@ 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 @@ -106,7 +105,11 @@ class FGInitialCondition float u,v,w; float latitude,longitude; - FGAtmosphere *atm; + FGFDMExec *fdmex; + + float calcVcas(float Mach); + bool findMachInterval(float *mlo, float *mhi,float vcas); + bool getMachFromVcas(float *Mach,float vcas); }; #endif diff --git a/src/FDM/JSBSim/FGModel.h b/src/FDM/JSBSim/FGModel.h index 709758bee..4b4c24f4a 100644 --- a/src/FDM/JSBSim/FGModel.h +++ b/src/FDM/JSBSim/FGModel.h @@ -85,7 +85,7 @@ public: string Name; virtual bool Run(void); virtual bool InitModel(void); - void SetRate(int tt) {rate = tt;}; + virtual void SetRate(int tt) {rate = tt;}; protected: enum {eU=1, eV, eW}; diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index c7fb4f159..48663603c 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -28,8 +28,7 @@ FUNCTIONAL DESCRIPTION -------------------------------------------------------------------------------- This is the place where you create output routines to dump data for perusal -later. Some machines may not support the ncurses console output. Borland is one -of those environments which does not, so the ncurses stuff is commented out. +later. HISTORY -------------------------------------------------------------------------------- @@ -59,6 +58,10 @@ FGOutput::FGOutput(FGFDMExec* fdmex) : FGModel(fdmex) Name = "FGOutput"; sFirstPass = dFirstPass = true; socket = 0; + Type = otNone; + Filename = "JSBSim.out"; + SubSystems = 0; + #ifdef FG_WITH_JSBSIM_SOCKET socket = new FGfdmSocket("localhost",1138); #endif @@ -76,71 +79,174 @@ FGOutput::~FGOutput(void) bool FGOutput::Run(void) { if (!FGModel::Run()) { -// SocketOutput(); -// DelimitedOutput("JSBSimData.csv"); -// DelimitedOutput(); + + if (Type == otSocket) { + SocketOutput(); + } else if (Type == otCSV) { + if (Filename != "COUT" && Filename != "cout" && Filename.size() > 0) { + DelimitedOutput(Filename); + } else { + DelimitedOutput(); + } + } else if (Type == otTerminal) { + // Not done yet + } else if (Type == otNone) { + // Do nothing + } else { + // Not a valid type of output + } + } else { } + return false; } /******************************************************************************/ +void FGOutput::SetType(string type) +{ + if (type == "CSV") { + Type = otCSV; + } else if (type == "TABULAR") { + Type = otTab; + } else if (type == "SOCKET") { + Type = otSocket; + } else if (type == "TERMINAL") { + Type = otTerminal; + } else if (type != "NONE"){ + Type = otUnknown; + cerr << "Unknown type of output specified in config file" << endl; + } +} + +/******************************************************************************/ + void FGOutput::DelimitedOutput(void) { if (dFirstPass) { - cout << "Time,"; - cout << "QBar,"; - cout << "Vtotal,"; - cout << "Throttle,"; - cout << "Aileron,"; - cout << "Elevator,"; - cout << "Rudder,"; - cout << "Rho,"; - cout << "Ixx,"; - cout << "Iyy,"; - cout << "Izz,"; - cout << "Ixz,"; - cout << "Mass,"; - 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 << "Time"; + if (SubSystems & FGAircraft::ssSimulation) { + // Nothing here, yet + } + if (SubSystems & FGAircraft::ssAerosurfaces) { + cout << ", "; + cout << "Throttle, "; + cout << "Aileron Cmd, "; + cout << "Elevator Cmd, "; + cout << "Rudder Cmd, "; + cout << "Aileron Pos, "; + cout << "Elevator Pos, "; + cout << "Rudder Pos"; + } + if (SubSystems & FGAircraft::ssRates) { + cout << ", "; + cout << "P, Q, R"; + } + if (SubSystems & FGAircraft::ssVelocities) { + cout << ", "; + cout << "QBar, "; + cout << "Vtotal, "; + cout << "U, V, W, "; + cout << "Vn, Ve, Vd"; + } + if (SubSystems & FGAircraft::ssForces) { + cout << ", "; + cout << "XsForce, YsForce, ZsForce, "; + cout << "Xforce, Yforce, Zforce"; + } + if (SubSystems & FGAircraft::ssMoments) { + cout << ", "; + cout << "L, M, N"; + } + if (SubSystems & FGAircraft::ssAtmosphere) { + cout << ", "; + cout << "Rho"; + } + if (SubSystems & FGAircraft::ssMassProps) { + cout << ", "; + cout << "Ixx, "; + cout << "Iyy, "; + cout << "Izz, "; + cout << "Ixz, "; + cout << "Mass, "; + cout << "Xcg, Ycg, Zcg"; + } + if (SubSystems & FGAircraft::ssPosition) { + cout << ", "; + cout << "Altitude, "; + cout << "Phi, Tht, Psi, "; + cout << "Alpha, "; + cout << "Latitude, "; + cout << "Longitude"; + } + if (SubSystems & FGAircraft::ssCoefficients) { + cout << ", "; + cout << Aircraft->GetCoefficientStrings(); + } + cout << endl; dFirstPass = false; } - cout << State->Getsim_time() << ","; - cout << Translation->Getqbar() << ","; - cout << Translation->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->GetXYZcg() << ","; - cout << Aircraft->GetForces() << ","; - cout << Aircraft->GetMoments() << ","; - cout << Position->Geth() << ","; - cout << Rotation->GetEuler() << ","; - cout << Rotation->GetPQR() << ","; - cout << Translation->GetUVW() << ","; - cout << Translation->Getalpha() << ","; - cout << Position->GetVel() << ","; - cout << Position->GetLatitude() << ","; - cout << Position->GetLongitude(); + cout << State->Getsim_time(); + if (SubSystems & FGAircraft::ssSimulation) { + } + if (SubSystems & FGAircraft::ssAerosurfaces) { + cout << ", "; + cout << FCS->GetThrottlePos(0) << ", "; + cout << FCS->GetDaCmd() << ", "; + cout << FCS->GetDeCmd() << ", "; + cout << FCS->GetDrCmd() << ", "; + cout << FCS->GetDaPos() << ", "; + cout << FCS->GetDePos() << ", "; + cout << FCS->GetDrPos(); + } + if (SubSystems & FGAircraft::ssRates) { + cout << ", "; + cout << Rotation->GetPQR(); + } + if (SubSystems & FGAircraft::ssVelocities) { + cout << ", "; + cout << Translation->Getqbar() << ", "; + cout << Translation->GetVt() << ", "; + cout << Translation->GetUVW() << ", "; + cout << Position->GetVel(); + } + if (SubSystems & FGAircraft::ssForces) { + cout << ", "; + cout << Aircraft->GetvFs() << ", "; + cout << Aircraft->GetForces(); + } + if (SubSystems & FGAircraft::ssMoments) { + cout << ", "; + cout << Aircraft->GetMoments(); + } + if (SubSystems & FGAircraft::ssAtmosphere) { + cout << ", "; + cout << Atmosphere->GetDensity(); + } + if (SubSystems & FGAircraft::ssMassProps) { + cout << ", "; + cout << Aircraft->GetIxx() << ", "; + cout << Aircraft->GetIyy() << ", "; + cout << Aircraft->GetIzz() << ", "; + cout << Aircraft->GetIxz() << ", "; + cout << Aircraft->GetMass() << ", "; + cout << Aircraft->GetXYZcg(); + } + if (SubSystems & FGAircraft::ssPosition) { + cout << ", "; + cout << Position->Geth() << ", "; + cout << Rotation->GetEuler() << ", "; + cout << Translation->Getalpha() << ", "; + cout << Position->GetLatitude() << ", "; + cout << Position->GetLongitude(); + } + if (SubSystems & FGAircraft::ssCoefficients) { + cout << ", "; + cout << Aircraft->GetCoefficientValues(); + } cout << endl; } @@ -150,58 +256,127 @@ void FGOutput::DelimitedOutput(string fname) { if (sFirstPass) { datafile.open(fname.c_str()); - datafile << "Time,"; - datafile << "QBar,"; - datafile << "Vtotal,"; - datafile << "Throttle,"; - datafile << "Aileron,"; - datafile << "Elevator,"; - datafile << "Rudder,"; - datafile << "Rho,"; - datafile << "Ixx,"; - datafile << "Iyy,"; - datafile << "Izz,"; - datafile << "Ixz,"; - datafile << "Mass,"; - 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 << "Time"; + if (SubSystems & FGAircraft::ssSimulation) { + // Nothing here, yet + } + if (SubSystems & FGAircraft::ssAerosurfaces) { + datafile << ", "; + datafile << "Throttle, "; + datafile << "Aileron Cmd, "; + datafile << "Elevator Cmd, "; + datafile << "Rudder Cmd, "; + datafile << "Aileron Pos, "; + datafile << "Elevator Pos, "; + datafile << "Rudder Pos"; + } + if (SubSystems & FGAircraft::ssRates) { + datafile << ", "; + datafile << "P, Q, R"; + } + if (SubSystems & FGAircraft::ssVelocities) { + datafile << ", "; + datafile << "QBar, "; + datafile << "Vtotal, "; + datafile << "U, V, W, "; + datafile << "Vn, Ve, Vd"; + } + if (SubSystems & FGAircraft::ssForces) { + datafile << ", "; + datafile << "XsForce, YsForce, ZsForce, "; + datafile << "Xforce, Yforce, Zforce"; + } + if (SubSystems & FGAircraft::ssMoments) { + datafile << ", "; + datafile << "L, M, N"; + } + if (SubSystems & FGAircraft::ssAtmosphere) { + datafile << ", "; + datafile << "Rho"; + } + if (SubSystems & FGAircraft::ssMassProps) { + datafile << ", "; + datafile << "Ixx, "; + datafile << "Iyy, "; + datafile << "Izz, "; + datafile << "Ixz, "; + datafile << "Mass, "; + datafile << "Xcg, Ycg, Zcg"; + } + if (SubSystems & FGAircraft::ssPosition) { + datafile << ", "; + datafile << "Altitude, "; + datafile << "Phi, Tht, Psi, "; + datafile << "Alpha, "; + datafile << "Latitude, "; + datafile << "Longitude"; + } + if (SubSystems & FGAircraft::ssCoefficients) { + datafile << ", "; + datafile << Aircraft->GetCoefficientStrings(); + } datafile << endl; sFirstPass = false; } - datafile << State->Getsim_time() << ","; - datafile << Translation->Getqbar() << ","; - datafile << Translation->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->GetXYZcg() << ","; - datafile << Aircraft->GetForces() << ","; - datafile << Aircraft->GetMoments() << ","; - datafile << Position->Geth() << ","; - datafile << Rotation->GetEuler() << ","; - datafile << Rotation->GetPQR() << ","; - datafile << Translation->GetUVW() << ","; - datafile << Translation->Getalpha() << ","; - datafile << Position->GetVel() << ","; - datafile << Position->GetLatitude() << ","; - datafile << Position->GetLongitude(); + datafile << State->Getsim_time(); + if (SubSystems & FGAircraft::ssSimulation) { + } + if (SubSystems & FGAircraft::ssAerosurfaces) { + datafile << ", "; + datafile << FCS->GetThrottlePos(0) << ", "; + datafile << FCS->GetDaCmd() << ", "; + datafile << FCS->GetDeCmd() << ", "; + datafile << FCS->GetDrCmd() << ", "; + datafile << FCS->GetDaPos() << ", "; + datafile << FCS->GetDePos() << ", "; + datafile << FCS->GetDrPos(); + } + if (SubSystems & FGAircraft::ssRates) { + datafile << ", "; + datafile << Rotation->GetPQR(); + } + if (SubSystems & FGAircraft::ssVelocities) { + datafile << ", "; + datafile << Translation->Getqbar() << ", "; + datafile << Translation->GetVt() << ", "; + datafile << Translation->GetUVW() << ", "; + datafile << Position->GetVel(); + } + if (SubSystems & FGAircraft::ssForces) { + datafile << ", "; + datafile << Aircraft->GetvFs() << ", "; + datafile << Aircraft->GetForces(); + } + if (SubSystems & FGAircraft::ssMoments) { + datafile << ", "; + datafile << Aircraft->GetMoments(); + } + if (SubSystems & FGAircraft::ssAtmosphere) { + datafile << ", "; + datafile << Atmosphere->GetDensity(); + } + if (SubSystems & FGAircraft::ssMassProps) { + datafile << ", "; + datafile << Aircraft->GetIxx() << ", "; + datafile << Aircraft->GetIyy() << ", "; + datafile << Aircraft->GetIzz() << ", "; + datafile << Aircraft->GetIxz() << ", "; + datafile << Aircraft->GetMass() << ", "; + datafile << Aircraft->GetXYZcg(); + } + if (SubSystems & FGAircraft::ssPosition) { + datafile << ", "; + datafile << Position->Geth() << ", "; + datafile << Rotation->GetEuler() << ", "; + datafile << Translation->Getalpha() << ", "; + datafile << Position->GetLatitude() << ", "; + datafile << Position->GetLongitude(); + } + if (SubSystems & FGAircraft::ssCoefficients) { + datafile << ", "; + datafile << Aircraft->GetCoefficientValues(); + } datafile << endl; datafile.flush(); } diff --git a/src/FDM/JSBSim/FGOutput.h b/src/FDM/JSBSim/FGOutput.h index 470c2b7ae..0abf0ca5b 100644 --- a/src/FDM/JSBSim/FGOutput.h +++ b/src/FDM/JSBSim/FGOutput.h @@ -72,11 +72,17 @@ public: void DelimitedOutput(string); void SocketOutput(void); void SocketStatusOutput(string); + void SetFilename(string fn) {Filename = fn;} + void SetType(string); + void SetSubsystems(int tt) {SubSystems = tt;} protected: private: bool sFirstPass, dFirstPass; + int SubSystems; + string Filename; + enum {otNone, otCSV, otTab, otSocket, otTerminal, otUnknown} Type; ofstream datafile; FGfdmSocket* socket; }; diff --git a/src/FDM/JSBSim/FGPosition.cpp b/src/FDM/JSBSim/FGPosition.cpp index 5849c9344..b71beef78 100644 --- a/src/FDM/JSBSim/FGPosition.cpp +++ b/src/FDM/JSBSim/FGPosition.cpp @@ -106,7 +106,7 @@ bool FGPosition:: Run(void) if (!FGModel::Run()) { GetState(); - vVel = State->GetTb2l()*vUVW; + vVel = State->GetTl2b()*vUVW; cosLat = cos(Latitude); if (cosLat != 0) LongitudeDot = vVel(eEast) / (Radius * cosLat); diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp index 9607fc1f1..8a80034a2 100644 --- a/src/FDM/JSBSim/FGRotation.cpp +++ b/src/FDM/JSBSim/FGRotation.cpp @@ -78,11 +78,12 @@ INCLUDES FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex), - vPQR(3), - vEuler(3), - vMoments(3) + vPQR(3), + vPQRdot(3), + vEuler(3), + vMoments(3) { - Name = "FGRotation"; + Name = "FGRotation"; } /******************************************************************************/ @@ -95,45 +96,44 @@ FGRotation::~FGRotation(void) bool FGRotation::Run(void) { - float L2, N1; - static FGColumnVector vlastPQRdot(3); - static FGColumnVector vPQRdot(3); + float L2, N1; + static FGColumnVector vlastPQRdot(3); - if (!FGModel::Run()) { - GetState(); + if (!FGModel::Run()) { + GetState(); - 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 = 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); - 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); + 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); - vPQR += dt*rate*(vlastPQRdot + vPQRdot)/2.0; + vPQR += dt*rate*(vlastPQRdot + vPQRdot)/2.0; - State->IntegrateQuat(vPQR, rate); - State->CalcMatrices(); - vEuler = State->CalcEuler(); + State->IntegrateQuat(vPQR, rate); + State->CalcMatrices(); + vEuler = State->CalcEuler(); - vlastPQRdot = vPQRdot; + vlastPQRdot = vPQRdot; - } else { - } - return false; + } else { + } + return false; } /******************************************************************************/ void FGRotation::GetState(void) { - dt = State->Getdt(); + dt = State->Getdt(); - vMoments = Aircraft->GetMoments(); + vMoments = Aircraft->GetMoments(); - Ixx = Aircraft->GetIxx(); - Iyy = Aircraft->GetIyy(); - Izz = Aircraft->GetIzz(); - Ixz = Aircraft->GetIxz(); + Ixx = Aircraft->GetIxx(); + Iyy = Aircraft->GetIyy(); + Izz = Aircraft->GetIzz(); + Ixz = Aircraft->GetIxz(); } /******************************************************************************/ diff --git a/src/FDM/JSBSim/FGRotation.h b/src/FDM/JSBSim/FGRotation.h index 51e5d412a..01fa8aed0 100644 --- a/src/FDM/JSBSim/FGRotation.h +++ b/src/FDM/JSBSim/FGRotation.h @@ -81,6 +81,7 @@ CLASS DECLARATION class FGRotation : public FGModel { FGColumnVector vPQR; + FGColumnVector vPQRdot; FGColumnVector vMoments; FGColumnVector vEuler; @@ -96,6 +97,7 @@ public: bool Run(void); inline FGColumnVector GetPQR(void) {return vPQR;} + inline FGColumnVector GetPQRdot(void) {return vPQRdot;} inline FGColumnVector GetEuler(void) {return vEuler;} inline void SetPQR(FGColumnVector tt) {vPQR = tt;} inline void SetEuler(FGColumnVector tt) {vEuler = tt;} diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index d0cbaa5f3..ffe25c163 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -171,8 +171,8 @@ void FGState::Initialize(float U, float V, float W, float alpha, beta, gamma; float qbar, Vt; - FDMExec->GetPosition()->SetLatitude(Latitude*DEGTORAD); - FDMExec->GetPosition()->SetLongitude(Longitude*DEGTORAD); + FDMExec->GetPosition()->SetLatitude(Latitude); + FDMExec->GetPosition()->SetLongitude(Longitude); FDMExec->GetPosition()->Seth(H); FDMExec->GetAtmosphere()->Run(); @@ -480,7 +480,7 @@ FGMatrix FGState::GetTs2b(float alpha, float beta) mTs2b(1,1) = -ca*cb; mTs2b(1,2) = -ca*sb; mTs2b(1,3) = sa; - mTs2b(2,1) = sb; + mTs2b(2,1) = -sb; mTs2b(2,2) = cb; mTs2b(2,3) = 0.0; mTs2b(3,1) = -sa*cb; diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp index d2b60d934..ac1d9bcf3 100644 --- a/src/FDM/JSBSim/FGTranslation.cpp +++ b/src/FDM/JSBSim/FGTranslation.cpp @@ -74,17 +74,19 @@ INCLUDES FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex), - vUVW(3), - vPQR(3), - vForces(3), - vEuler(3) + vUVW(3), + vWindUVW(3), + vUVWdot(3), + vPQR(3), + vForces(3), + vEuler(3) { - Name = "FGTranslation"; - qbar = 0; - Vt = 0.0; - Mach = 0.0; - alpha = beta = gamma = 0.0; - rho = 0.002378; + Name = "FGTranslation"; + qbar = 0; + Vt = 0.0; + Mach = 0.0; + alpha = beta = gamma = 0.0; + rho = 0.002378; } /******************************************************************************/ @@ -97,44 +99,43 @@ FGTranslation::~FGTranslation(void) bool FGTranslation::Run(void) { - static FGColumnVector vlastUVWdot(3); - static FGColumnVector vUVWdot(3); - static FGMatrix mVel(3,3); + static FGColumnVector vlastUVWdot(3); + static FGMatrix mVel(3,3); - if (!FGModel::Run()) { + if (!FGModel::Run()) { - GetState(); + GetState(); - 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; + 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; - vUVWdot = mVel*vPQR + vForces/Mass; + vUVWdot = mVel*vPQR + vForces/Mass; - vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot); + vUVW += 0.5*dt*rate*(vlastUVWdot + vUVWdot) + vWindUVW; - Vt = vUVW.Magnitude(); + Vt = vUVW.Magnitude(); - 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; + 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; + qbar = 0.5*rho*Vt*Vt; - Mach = Vt / State->Geta(); + Mach = Vt / State->Geta(); - vlastUVWdot = vUVWdot; + vlastUVWdot = vUVWdot; - } else { - } - return false; + } else { + } + return false; } /******************************************************************************/ @@ -150,5 +151,7 @@ void FGTranslation::GetState(void) rho = Atmosphere->GetDensity(); vEuler = Rotation->GetEuler(); + +// vWindUVW = Atmosphere->GetWindUVW(); } diff --git a/src/FDM/JSBSim/FGTranslation.h b/src/FDM/JSBSim/FGTranslation.h index b742a07f7..298d16689 100644 --- a/src/FDM/JSBSim/FGTranslation.h +++ b/src/FDM/JSBSim/FGTranslation.h @@ -85,6 +85,7 @@ public: ~FGTranslation(void); inline FGColumnVector GetUVW(void) {return vUVW;} + inline FGColumnVector GetUVWdot(void) { return vUVWdot; } inline float Getalpha(void) {return alpha;} inline float Getbeta (void) {return beta; } @@ -102,13 +103,14 @@ public: inline void SetVt (float tt) {Vt = tt;} inline void SetABG(float t1, float t2, float t3) {alpha=t1; beta=t2; gamma=t3;} - + bool Run(void); protected: private: - FGColumnVector vUVW; + FGColumnVector vUVW,vWindUVW; + FGColumnVector vUVWdot; FGColumnVector vPQR; FGColumnVector vForces; FGColumnVector vEuler; diff --git a/src/FDM/JSBSim/JSBSim.h b/src/FDM/JSBSim/JSBSim.h new file mode 100644 index 000000000..c0d74cf9f --- /dev/null +++ b/src/FDM/JSBSim/JSBSim.h @@ -0,0 +1,215 @@ +/******************************************************************************* + + Header: FGAircraft.h + Author: Jon S. Berndt + Date started: 12/12/98 + + ------------- 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 +-------------------------------------------------------------------------------- +12/12/98 JSB Created + +******************************************************************************** +SENTRY +*******************************************************************************/ + +#ifndef FGAIRCRAFT_H +#define FGAIRCRAFT_H + +/******************************************************************************* +COMMENTS, REFERENCES, and NOTES +*******************************************************************************/ +/* +The aerodynamic coefficients used in this model typically are: + +Longitudinal + CL0 - Reference lift at zero alpha + CD0 - Reference drag at zero alpha + CDM - Drag due to Mach + CLa - Lift curve slope (w.r.t. alpha) + CDa - Drag curve slope (w.r.t. alpha) + CLq - Lift due to pitch rate + CLM - Lift due to Mach + CLadt - Lift due to alpha rate + + Cmadt - Pitching Moment due to alpha rate + Cm0 - Reference Pitching moment at zero alpha + Cma - Pitching moment slope (w.r.t. alpha) + Cmq - Pitch damping (pitch moment due to pitch rate) + CmM - Pitch Moment due to Mach + +Lateral + Cyb - Side force due to sideslip + Cyr - Side force due to yaw rate + + Clb - Dihedral effect (roll moment due to sideslip) + Clp - Roll damping (roll moment due to roll rate) + Clr - Roll moment due to yaw rate + Cnb - Weathercocking stability (yaw moment due to sideslip) + Cnp - Rudder adverse yaw (yaw moment due to roll rate) + Cnr - Yaw damping (yaw moment due to yaw rate) + +Control + CLDe - Lift due to elevator + CDDe - Drag due to elevator + CyDr - Side force due to rudder + CyDa - Side force due to aileron + + CmDe - Pitch moment due to elevator + ClDa - Roll moment due to aileron + ClDr - Roll moment due to rudder + CnDr - Yaw moment due to rudder + CnDa - Yaw moment due to aileron + +[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 +[2] D. M. Henderson, "Euler Angles, Quaternions, and Transformation Matrices", + JSC 12960, July 1977 +[3] Richard E. McFarland, "A Standard Kinematic Model for Flight Simulation at + NASA-Ames", NASA CR-2497, January 1975 +[4] Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", + 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 +*/ + +/******************************************************************************* +INCLUDES +*******************************************************************************/ + +#ifdef FGFS +# include +# ifdef FG_HAVE_STD_INCLUDES +# include +# include +# else +# include +# include +# endif +#else +# include +# include +#endif + +#include "FGModel.h" +#include "FGCoefficient.h" +#include "FGEngine.h" +#include "FGTank.h" +#include "FGLGear.h" +#include "FGConfigFile.h" + +/******************************************************************************* +DEFINITIONS +*******************************************************************************/ + +using namespace std; + +/******************************************************************************* +CLASS DECLARATION +*******************************************************************************/ + +class FGAircraft : public FGModel +{ +public: + FGAircraft(FGFDMExec*); + ~FGAircraft(void); + + bool Run(void); + bool LoadAircraft(string, string, string); + bool LoadAircraftEx(string, string, string); + inline string GetAircraftName(void) {return AircraftName;} + inline void SetGearUp(bool tt) {GearUp = tt;} + inline bool GetGearUp(void) {return GearUp;} + inline float GetWingArea(void) {return WingArea;} + inline float GetWingSpan(void) {return WingSpan;} + inline float Getcbar(void) {return cbar;} + inline FGEngine* GetEngine(int tt) {return Engine[tt];} + 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 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;} + +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; + 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; + + int numTanks; + int numEngines; + int numSelectedOxiTanks; + int numSelectedFuelTanks; + FGTank* Tank[MAX_TANKS]; + FGEngine *Engine[MAX_ENGINES]; + + FGCoefficient *Coeff[6][10]; + int coeff_ctr[6]; + + bool GearUp; + + enum Param {LiftCoeff, + DragCoeff, + SideCoeff, + RollCoeff, + PitchCoeff, + YawCoeff, + numCoeffs}; + + string Axis[6]; + vector lGear; + +protected: + +}; + +/******************************************************************************/ +#endif