From a1b2152d7f83ed2f7399f949ffa8964d4ebb38c7 Mon Sep 17 00:00:00 2001 From: curt Date: Sat, 31 Jul 1999 04:55:23 +0000 Subject: [PATCH] Updates from Jon. --- src/FDM/JSBSim/FGAircraft.cpp | 2 + src/FDM/JSBSim/FGControls.cpp | 4 +- src/FDM/JSBSim/FGControls.h | 4 +- src/FDM/JSBSim/FGFDMExec.cpp | 6 +- src/FDM/JSBSim/FGOutput.cpp | 57 +++++++++--------- src/FDM/JSBSim/FGState.cpp | 107 ++++++++++++++++++++-------------- src/FDM/JSBSim/FGState.h | 1 + 7 files changed, 101 insertions(+), 80 deletions(-) diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index 82d9e4599..4fcfc64b4 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -399,7 +399,9 @@ void FGAircraft::FAero(void) void FGAircraft::FGear(void) { if (GearUp) { + // crash routine } else { + } } diff --git a/src/FDM/JSBSim/FGControls.cpp b/src/FDM/JSBSim/FGControls.cpp index c4de8a2ef..58cd62b03 100644 --- a/src/FDM/JSBSim/FGControls.cpp +++ b/src/FDM/JSBSim/FGControls.cpp @@ -51,8 +51,8 @@ FGControls::~FGControls() { // $Log$ -// Revision 1.3 1999/06/28 15:39:23 curt -// Updates contributed by Jon Berndt. +// Revision 1.4 1999/07/31 02:55:24 curt +// Updates from Jon. // // Revision 1.1 1999/02/13 01:12:03 curt // Initial Revision. diff --git a/src/FDM/JSBSim/FGControls.h b/src/FDM/JSBSim/FGControls.h index 6ed9c16bd..9419abd35 100644 --- a/src/FDM/JSBSim/FGControls.h +++ b/src/FDM/JSBSim/FGControls.h @@ -177,8 +177,8 @@ extern FGControls controls; // $Log$ -// Revision 1.3 1999/06/28 15:39:24 curt -// Updates contributed by Jon Berndt. +// Revision 1.4 1999/07/31 02:55:24 curt +// Updates from Jon. // // Revision 1.1 1999/02/13 01:12:03 curt // Initial Revision. diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index e89ee8844..2780a14fe 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -93,7 +93,7 @@ FGFDMExec::FGFDMExec(void) Rotation = new FGRotation(this); Position = new FGPosition(this); Auxiliary = new FGAuxiliary(this); - Output = new FGOutput(this); +// Output = new FGOutput(this); State = new FGState(this); @@ -106,7 +106,7 @@ FGFDMExec::FGFDMExec(void) if (!Rotation->InitModel()) {cerr << "Rotation model init failed"; Error+=16;} if (!Position->InitModel()) {cerr << "Position model init failed"; Error+=32;} if (!Auxiliary->InitModel()) {cerr << "Auxiliary model init failed"; Error+=64;} - if (!Output->InitModel()) {cerr << "Output model init failed"; Error+=128;} +// if (!Output->InitModel()) {cerr << "Output model init failed"; Error+=128;} Schedule(Atmosphere, 5); Schedule(FCS, 1); @@ -115,7 +115,7 @@ FGFDMExec::FGFDMExec(void) Schedule(Translation, 1); Schedule(Position, 1); Schedule(Auxiliary, 1); - Schedule(Output, 1); +// Schedule(Output, 1); terminate = false; frozen = false; diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index abf5bdcac..9094f8997 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -196,33 +196,34 @@ void FGOutput::DelimitedOutput(void) cout << "N"; cout << endl; FirstPass = false; - } else { - cout << State->Getsim_time() << ","; - cout << State->Geth() << ","; - cout << Rotation->Getphi() << ","; - cout << Rotation->Gettht() << ","; - cout << Rotation->Getpsi() << ","; - cout << Atmosphere->Getrho() << ","; - 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 << 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 << endl; } + + cout << State->Getsim_time() << ","; + cout << State->Geth() << ","; + cout << Rotation->Getphi() << ","; + cout << Rotation->Gettht() << ","; + cout << Rotation->Getpsi() << ","; + cout << Atmosphere->Getrho() << ","; + 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 << 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 << endl; + } diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index a55abf3b5..f7df28c17 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -107,51 +107,7 @@ bool FGState::Reset(string path, string fname) resetfile >> h; resetfile.close(); -// Change all angular measurements from degrees (as in config file) to radians - - gamma = 0.0; - if (W != 0.0) - alpha = U*U > 0.0 ? atan2(W, U) : 0.0; - else - alpha = 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; - else - beta = 0.0; - - latitude *= M_PI / 180.0; - longitude *= M_PI / 180.0; - phi *= M_PI / 180.0; - tht *= M_PI / 180.0; - psi *= M_PI / 180.0; - - FDMExec->GetTranslation()->SetUVW(U, V, W); - FDMExec->GetRotation()->SetEuler(phi, tht, psi); - 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]); + Initialize(U, V, W, phi, tht, psi, latitude, longitude, h); return true; } else { @@ -161,6 +117,67 @@ bool FGState::Reset(string path, string fname) } +void FGState::Initialize(float U, float V, float W, + float phi, float tht, float psi, + float Latitude, float Longitude, float H) +{ + float alpha, beta, gamma; + float Q0, Q1, Q2, Q3; + float T[4][4]; + + latitude = Latitude; + longitude = Longitude; + h = H; + +// Change all angular measurements from degrees (as in config file) to radians + + gamma = 0.0; + if (W != 0.0) + alpha = U*U > 0.0 ? atan2(W, U) : 0.0; + else + alpha = 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; + else + beta = 0.0; + + latitude *= M_PI / 180.0; + longitude *= M_PI / 180.0; + phi *= M_PI / 180.0; + tht *= M_PI / 180.0; + psi *= M_PI / 180.0; + + FDMExec->GetTranslation()->SetUVW(U, V, W); + FDMExec->GetRotation()->SetEuler(phi, tht, psi); + 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(); +} + + bool FGState::StoreData(string fname) { ofstream datafile(fname.c_str()); diff --git a/src/FDM/JSBSim/FGState.h b/src/FDM/JSBSim/FGState.h index 7ac238e74..d1150ee05 100644 --- a/src/FDM/JSBSim/FGState.h +++ b/src/FDM/JSBSim/FGState.h @@ -78,6 +78,7 @@ public: ~FGState(void); bool Reset(string, string); + void Initialize(float, float, float, float, float, float, float, float, float); bool StoreData(string); bool DumpData(string); bool DisplayData(void); -- 2.39.5