From f3ca9b2e050af754168c0dc8d67e95e2fbd7ce20 Mon Sep 17 00:00:00 2001 From: curt Date: Tue, 17 Apr 2001 21:19:54 +0000 Subject: [PATCH] Synced with latest JSBSim. X15 works for me, but C172 segfaults. --- src/FDM/JSBSim.cxx | 540 ++++++++++++++++--------------- src/FDM/JSBSim.hxx | 10 + src/FDM/JSBSim/FGAircraft.cpp | 25 +- src/FDM/JSBSim/FGAircraft.h | 16 +- src/FDM/JSBSim/FGAtmosphere.cpp | 5 +- src/FDM/JSBSim/FGAuxiliary.h | 4 +- src/FDM/JSBSim/FGCoefficient.cpp | 10 +- src/FDM/JSBSim/FGConfigFile.cpp | 2 +- src/FDM/JSBSim/FGDefs.h | 4 + src/FDM/JSBSim/FGFCS.cpp | 41 ++- src/FDM/JSBSim/FGFCS.h | 4 +- src/FDM/JSBSim/FGFDMExec.cpp | 4 +- src/FDM/JSBSim/FGForce.cpp | 6 +- src/FDM/JSBSim/FGLGear.cpp | 28 +- src/FDM/JSBSim/FGLGear.h | 11 +- src/FDM/JSBSim/FGMatrix.cpp | 8 +- src/FDM/JSBSim/FGMatrix.h | 4 +- src/FDM/JSBSim/FGNozzle.cpp | 1 + src/FDM/JSBSim/FGOutput.cpp | 8 +- src/FDM/JSBSim/FGPiston.cpp | 1 + src/FDM/JSBSim/FGPropeller.cpp | 5 +- src/FDM/JSBSim/FGRotation.cpp | 4 +- src/FDM/JSBSim/FGRotation.h | 5 + src/FDM/JSBSim/FGState.cpp | 213 ++++++------ src/FDM/JSBSim/FGState.h | 53 ++- src/FDM/JSBSim/FGThruster.h | 1 + src/FDM/JSBSim/FGTranslation.cpp | 6 +- src/FDM/JSBSim/FGTranslation.h | 11 +- src/FDM/JSBSim/FGTrim.cpp | 6 +- src/FDM/JSBSim/FGTrimAxis.cpp | 40 +-- 30 files changed, 620 insertions(+), 456 deletions(-) diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index 4ed9a6d27..901df669d 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -64,6 +64,17 @@ FGJSBsim::FGJSBsim( double dt ) bool result; fdmex=new FGFDMExec; + + State = fdmex->GetState(); + Atmosphere = fdmex->GetAtmosphere(); + FCS = fdmex->GetFCS(); + Propulsion = fdmex->GetPropulsion(); + Aircraft = fdmex->GetAircraft(); + Translation = fdmex->GetTranslation(); + Rotation = fdmex->GetRotation(); + Position = fdmex->GetPosition(); + Auxiliary = fdmex->GetAuxiliary(); + fgic=new FGInitialCondition(fdmex); needTrim=true; @@ -73,22 +84,23 @@ FGJSBsim::FGJSBsim( double dt ) SGPath engine_path( globals->get_fg_root() ); engine_path.append( "Engine" ); set_delta_t( dt ); - fdmex->GetState()->Setdt( dt ); + State->Setdt( dt ); result = fdmex->LoadModel( aircraft_path.str(), - engine_path.str(), - fgGetString("/sim/aircraft") ); - int Neng=fdmex->GetPropulsion()->GetNumEngines(); -// int Neng=fdmex->GetAircraft()->GetNumEngines(); + engine_path.str(), + fgGetString("/sim/aircraft") ); + + int Neng = Propulsion->GetNumEngines(); SG_LOG(SG_FLIGHT,SG_INFO, "Neng: " << Neng ); + for(int i=0;iGetFCS()->GetPitchTrimCmd()); - fgSetDouble("/fdm/trim/throttle", fdmex->GetFCS()->GetThrottleCmd(0)); - fgSetDouble("/fdm/trim/aileron", fdmex->GetFCS()->GetDaCmd()); - fgSetDouble("/fdm/trim/rudder", fdmex->GetFCS()->GetDrCmd()); + fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd()); + fgSetDouble("/fdm/trim/throttle", FCS->GetThrottleCmd(0)); + fgSetDouble("/fdm/trim/aileron", FCS->GetDaCmd()); + fgSetDouble("/fdm/trim/rudder", FCS->GetDrCmd()); trimmed = fgGetValue("/fdm/trim/trimmed",true); trimmed->setBoolValue(false); @@ -98,8 +110,8 @@ FGJSBsim::FGJSBsim( double dt ) /******************************************************************************/ FGJSBsim::~FGJSBsim(void) { if(fdmex != NULL) { - delete fdmex; - delete fgic; + delete fdmex; + delete fgic; } } @@ -109,54 +121,53 @@ FGJSBsim::~FGJSBsim(void) { // each subsequent iteration through the EOM void FGJSBsim::init() { - // Explicitly call the superclass's - // init method first. + // Explicitly call the superclass's + // init method first. FGInterface::init(); bool result; SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" ); - fdmex->GetAtmosphere()->UseInternal(); - + Atmosphere->UseInternal(); + SG_LOG( SG_FLIGHT, SG_INFO, " Initializing JSBSim with:" ); + switch(fgic->GetSpeedSet()) { case setned: - SG_LOG(SG_FLIGHT,SG_INFO, " Vn,Ve,Vd= " - << fdmex->GetPosition()->GetVn() - << ", " << fdmex->GetPosition()->GetVe() - << ", " << fdmex->GetPosition()->GetVd() - << " ft/s"); - break; + SG_LOG(SG_FLIGHT,SG_INFO, " Vn,Ve,Vd= " + << Position->GetVn() << ", " + << Position->GetVe() << ", " + << Position->GetVd() << " ft/s"); + break; case setuvw: - SG_LOG(SG_FLIGHT,SG_INFO, " U,V,W= " - << fdmex->GetTranslation()->GetUVW()(1) - << ", " << fdmex->GetTranslation()->GetUVW()(2) - << ", " << fdmex->GetTranslation()->GetUVW()(3) - << " ft/s"); - break; + SG_LOG(SG_FLIGHT,SG_INFO, " U,V,W= " + << Translation->GetUVW(1) << ", " + << Translation->GetUVW(2) << ", " + << Translation->GetUVW(3) << " ft/s"); + break; case setmach: - SG_LOG(SG_FLIGHT,SG_INFO, " Mach: " - << fdmex->GetTranslation()->GetMach() ); - break; + SG_LOG(SG_FLIGHT,SG_INFO, " Mach: " + << Translation->GetMach() ); + break; case setvc: default: - SG_LOG(SG_FLIGHT,SG_INFO, " Indicated Airspeed: " - << fdmex->GetAuxiliary()->GetVcalibratedKTS() << " knots" ); - + SG_LOG(SG_FLIGHT,SG_INFO, " Indicated Airspeed: " + << Auxiliary->GetVcalibratedKTS() << " knots" ); + break; } - SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " - << fdmex->GetRotation()->Getphi()*RADTODEG << " deg"); - SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " - << fdmex->GetRotation()->Gettht()*RADTODEG << " deg" ); - SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: " - << fdmex->GetRotation()->Getpsi()*RADTODEG << " deg" ); - SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: " - << fdmex->GetPosition()->GetLatitude() << " deg" ); - SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: " - << fdmex->GetPosition()->GetLongitude() << " deg" ); - + SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " + << Rotation->Getphi()*RADTODEG << " deg"); + SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " + << Rotation->Gettht()*RADTODEG << " deg" ); + SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: " + << Rotation->Getpsi()*RADTODEG << " deg" ); + SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: " + << Position->GetLatitude() << " deg" ); + SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: " + << Position->GetLongitude() << " deg" ); + SG_LOG( SG_FLIGHT, SG_INFO, " loaded initial conditions" ); SG_LOG( SG_FLIGHT, SG_INFO, " set dt" ); @@ -169,61 +180,61 @@ void FGJSBsim::init() { // Run an iteration of the EOM (equations of motion) bool FGJSBsim::update( int multiloop ) { - + int i; - + double save_alt = 0.0; - copy_to_JSBsim(); trimmed->setBoolValue(false); - if(needTrim && fgGetBool("/sim/startup/trim")) { - //fgic->SetSeaLevelRadiusFtIC( get_Sea_level_radius() ); - //fgic->SetTerrainAltitudeFtIC( scenery.cur_elev * SG_METER_TO_FEET ); - FGTrim *fgtrim; - if(fgic->GetVcalibratedKtsIC() < 10 ) { - fgic->SetVcalibratedKtsIC(0.0); - fgtrim=new FGTrim(fdmex,fgic,tGround); - } else { - fgtrim=new FGTrim(fdmex,fgic,tLongitudinal); - } - if(!fgtrim->DoTrim()) { - fgtrim->Report(); - fgtrim->TrimStats(); - } else { - trimmed->setBoolValue(true); - } - fgtrim->ReportState(); - delete fgtrim; - - needTrim=false; - - fgSetDouble("/fdm/trim/pitch-trim", fdmex->GetFCS()->GetPitchTrimCmd()); - fgSetDouble("/fdm/trim/throttle", fdmex->GetFCS()->GetThrottleCmd(0)); - fgSetDouble("/fdm/trim/aileron", fdmex->GetFCS()->GetDaCmd()); - fgSetDouble("/fdm/trim/rudder", fdmex->GetFCS()->GetDrCmd()); - - controls.set_elevator_trim(fdmex->GetFCS()->GetPitchTrimCmd()); - controls.set_elevator(fdmex->GetFCS()->GetDeCmd()); - controls.set_throttle(FGControls::ALL_ENGINES, - fdmex->GetFCS()->GetThrottleCmd(0)); + if (needTrim && fgGetBool("/sim/startup/trim")) { + + //fgic->SetSeaLevelRadiusFtIC( get_Sea_level_radius() ); + //fgic->SetTerrainAltitudeFtIC( scenery.cur_elev * SG_METER_TO_FEET ); + + FGTrim *fgtrim; + if(fgic->GetVcalibratedKtsIC() < 10 ) { + fgic->SetVcalibratedKtsIC(0.0); + fgtrim=new FGTrim(fdmex,fgic,tGround); + } else { + fgtrim=new FGTrim(fdmex,fgic,tLongitudinal); + } + if(!fgtrim->DoTrim()) { + fgtrim->Report(); + fgtrim->TrimStats(); + } else { + trimmed->setBoolValue(true); + } + fgtrim->ReportState(); + delete fgtrim; + + needTrim=false; - controls.set_aileron(fdmex->GetFCS()->GetDaCmd()); - controls.set_rudder(fdmex->GetFCS()->GetDrCmd()); + fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd()); + fgSetDouble("/fdm/trim/throttle", FCS->GetThrottleCmd(0)); + fgSetDouble("/fdm/trim/aileron", FCS->GetDaCmd()); + fgSetDouble("/fdm/trim/rudder", FCS->GetDrCmd()); + + controls.set_elevator_trim(FCS->GetPitchTrimCmd()); + controls.set_elevator(FCS->GetDeCmd()); + controls.set_throttle(FGControls::ALL_ENGINES, + FCS->GetThrottleCmd(0)); + + controls.set_aileron(FCS->GetDaCmd()); + controls.set_rudder( FCS->GetDrCmd()); - SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" ); + SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" ); } for( i=0; iset_RPM( controls.get_throttle(i)*2700 ); - get_engine(i)->set_Throttle( controls.get_throttle(i) ); + get_engine(i)->set_RPM( Propulsion->GetThruster(i)->GetRPM() ); + get_engine(i)->set_Throttle( controls.get_throttle(i) ); } - - - for ( i = 0; i < multiloop; i++ ) { - fdmex->Run(); + + for ( i=0; i < multiloop; i++ ) { + fdmex->Run(); } // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048); @@ -233,14 +244,6 @@ bool FGJSBsim::update( int multiloop ) { // autopilot (and the rest of the sim can use the updated values copy_from_JSBsim(); - - - - // but lets restore our original bogus altitude when we are done - - - - //climb rate now set from FDM in copy_from_x() return true; } @@ -251,29 +254,33 @@ bool FGJSBsim::update( int multiloop ) { bool FGJSBsim::copy_to_JSBsim() { // copy control positions into the JSBsim structure - fdmex->GetFCS()->SetDaCmd( controls.get_aileron()); - fdmex->GetFCS()->SetDeCmd( controls.get_elevator()); - fdmex->GetFCS()->SetPitchTrimCmd(controls.get_elevator_trim()); - fdmex->GetFCS()->SetDrCmd( -1*controls.get_rudder()); - fdmex->GetFCS()->SetDfCmd( controls.get_flaps() ); - fdmex->GetFCS()->SetDsbCmd( 0.0 ); //speedbrakes - fdmex->GetFCS()->SetDspCmd( 0.0 ); //spoilers - fdmex->GetFCS()->SetThrottleCmd( FGControls::ALL_ENGINES, - controls.get_throttle( 0 )); - fdmex->GetFCS()->SetLBrake( controls.get_brake( 0 ) ); - fdmex->GetFCS()->SetRBrake( controls.get_brake( 1 ) ); - fdmex->GetFCS()->SetCBrake( controls.get_brake( 2 ) ); - - fdmex->GetPosition()->SetSeaLevelRadius( get_Sea_level_radius() ); - fdmex->GetPosition()->SetRunwayRadius( scenery.cur_elev*SG_METER_TO_FEET - + get_Sea_level_radius() ); - - fdmex->GetAtmosphere()->SetExTemperature(get_Static_temperature()); - fdmex->GetAtmosphere()->SetExPressure(get_Static_pressure()); - fdmex->GetAtmosphere()->SetExDensity(get_Density()); - fdmex->GetAtmosphere()->SetWindNED(get_V_north_airmass(), - get_V_east_airmass(), - get_V_down_airmass()); + FCS->SetDaCmd( controls.get_aileron()); + FCS->SetDeCmd( controls.get_elevator()); + FCS->SetPitchTrimCmd(controls.get_elevator_trim()); + FCS->SetDrCmd( -controls.get_rudder()); + FCS->SetDfCmd( controls.get_flaps() ); + FCS->SetDsbCmd( 0.0 ); //speedbrakes + FCS->SetDspCmd( 0.0 ); //spoilers + FCS->SetThrottleCmd( FGControls::ALL_ENGINES, + controls.get_throttle( 0 )); + FCS->SetLBrake( controls.get_brake( 0 ) ); + FCS->SetRBrake( controls.get_brake( 1 ) ); + FCS->SetCBrake( controls.get_brake( 2 ) ); + + Position->SetSeaLevelRadius( get_Sea_level_radius() ); + Position->SetRunwayRadius( scenery.cur_elev*SG_METER_TO_FEET + + get_Sea_level_radius() ); + + Atmosphere->SetExTemperature(get_Static_temperature()); + Atmosphere->SetExPressure(get_Static_pressure()); + Atmosphere->SetExDensity(get_Density()); + Atmosphere->SetWindNED(get_V_north_airmass(), + get_V_east_airmass(), + get_V_down_airmass()); +// SG_LOG(SG_FLIGHT,SG_INFO, "Wind NED: " +// << get_V_north_airmass() << ", " +// << get_V_east_airmass() << ", " +// << get_V_down_airmass() ); return true; } @@ -283,114 +290,114 @@ bool FGJSBsim::copy_to_JSBsim() { // Convert from the JSBsim generic_ struct to the FGInterface struct bool FGJSBsim::copy_from_JSBsim() { - unsigned i, j; - - _set_Inertias( fdmex->GetAircraft()->GetMass(), - fdmex->GetAircraft()->GetIxx(), - fdmex->GetAircraft()->GetIyy(), - fdmex->GetAircraft()->GetIzz(), - fdmex->GetAircraft()->GetIxz() ); - - _set_CG_Position( fdmex->GetAircraft()->GetXYZcg()(1), - fdmex->GetAircraft()->GetXYZcg()(2), - fdmex->GetAircraft()->GetXYZcg()(3) ); - - _set_Accels_Body( fdmex->GetTranslation()->GetUVWdot()(1), - fdmex->GetTranslation()->GetUVWdot()(2), - fdmex->GetTranslation()->GetUVWdot()(3) ); - - _set_Accels_CG_Body( fdmex->GetTranslation()->GetUVWdot()(1), - fdmex->GetTranslation()->GetUVWdot()(2), - fdmex->GetTranslation()->GetUVWdot()(3) ); - - //_set_Accels_CG_Body_N ( fdmex->GetTranslation()->GetNcg()(1), - // fdmex->GetTranslation()->GetNcg()(2), - // fdmex->GetTranslation()->GetNcg()(3) ); + unsigned int i, j; + + _set_Inertias( Aircraft->GetMass(), + Aircraft->GetIxx(), + Aircraft->GetIyy(), + Aircraft->GetIzz(), + Aircraft->GetIxz() ); + + _set_CG_Position( Aircraft->GetXYZcg(1), + Aircraft->GetXYZcg(2), + Aircraft->GetXYZcg(3) ); + + _set_Accels_Body( Translation->GetUVWdot(1), + Translation->GetUVWdot(2), + Translation->GetUVWdot(3) ); + + _set_Accels_CG_Body( Translation->GetUVWdot(1), + Translation->GetUVWdot(2), + Translation->GetUVWdot(3) ); + + //_set_Accels_CG_Body_N ( Translation->GetNcg(1), + // Translation->GetNcg(2), + // Translation->GetNcg(3) ); // - _set_Accels_Pilot_Body( fdmex->GetAuxiliary()->GetPilotAccel()(1), - fdmex->GetAuxiliary()->GetPilotAccel()(2), - fdmex->GetAuxiliary()->GetPilotAccel()(3) ); - - //_set_Accels_Pilot_Body_N( fdmex->GetAuxiliary()->GetNpilot()(1), - // fdmex->GetAuxiliary()->GetNpilot()(2), - // fdmex->GetAuxiliary()->GetNpilot()(3) ); - - _set_Nlf( fdmex->GetAircraft()->GetNlf() ); - + _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1), + Auxiliary->GetPilotAccel(2), + Auxiliary->GetPilotAccel(3) ); + + //_set_Accels_Pilot_Body_N( Auxiliary->GetNpilot(1), + // Auxiliary->GetNpilot(2), + // Auxiliary->GetNpilot(3) ); + + _set_Nlf( Aircraft->GetNlf() ); + // Velocities - _set_Velocities_Local( fdmex->GetPosition()->GetVn(), - fdmex->GetPosition()->GetVe(), - fdmex->GetPosition()->GetVd() ); + _set_Velocities_Local( Position->GetVn(), + Position->GetVe(), + Position->GetVd() ); - _set_Velocities_Wind_Body( fdmex->GetTranslation()->GetUVW()(1), - fdmex->GetTranslation()->GetUVW()(2), - fdmex->GetTranslation()->GetUVW()(3) ); - - _set_V_rel_wind( fdmex->GetTranslation()->GetVt() ); - - _set_V_equiv_kts( fdmex->GetAuxiliary()->GetVequivalentKTS() ); + _set_Velocities_Wind_Body( Translation->GetUVW(1), + Translation->GetUVW(2), + Translation->GetUVW(3) ); - // _set_V_calibrated( fdmex->GetAuxiliary()->GetVcalibratedFPS() ); + _set_V_rel_wind( Translation->GetVt() ); - _set_V_calibrated_kts( fdmex->GetAuxiliary()->GetVcalibratedKTS() ); - - _set_V_ground_speed( fdmex->GetPosition()->GetVground() ); + _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() ); + + // _set_V_calibrated( Auxiliary->GetVcalibratedFPS() ); - _set_Omega_Body( fdmex->GetRotation()->GetPQR()(1), - fdmex->GetRotation()->GetPQR()(2), - fdmex->GetRotation()->GetPQR()(3) ); + _set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() ); - _set_Euler_Rates( fdmex->GetRotation()->GetEulerRates()(1), - fdmex->GetRotation()->GetEulerRates()(2), - fdmex->GetRotation()->GetEulerRates()(3) ); + _set_V_ground_speed( Position->GetVground() ); - _set_Geocentric_Rates(fdmex->GetPosition()->GetLatitudeDot(), - fdmex->GetPosition()->GetLongitudeDot(), - fdmex->GetPosition()->Gethdot() ); + _set_Omega_Body( Rotation->GetPQR(1), + Rotation->GetPQR(2), + Rotation->GetPQR(3) ); - _set_Mach_number( fdmex->GetTranslation()->GetMach() ); + _set_Euler_Rates( Rotation->GetEulerRates(1), + Rotation->GetEulerRates(2), + Rotation->GetEulerRates(3) ); + + _set_Geocentric_Rates(Position->GetLatitudeDot(), + Position->GetLongitudeDot(), + Position->Gethdot() ); + + _set_Mach_number( Translation->GetMach() ); // Positions - _updatePosition( fdmex->GetPosition()->GetLatitude(), - fdmex->GetPosition()->GetLongitude(), - fdmex->GetPosition()->Geth() ); - - _set_Euler_Angles( fdmex->GetRotation()->Getphi(), - fdmex->GetRotation()->Gettht(), - fdmex->GetRotation()->Getpsi() ); + _updatePosition( Position->GetLatitude(), + Position->GetLongitude(), + Position->Geth() ); - _set_Alpha( fdmex->GetTranslation()->Getalpha() ); - _set_Beta( fdmex->GetTranslation()->Getbeta() ); + _set_Euler_Angles( Rotation->Getphi(), + Rotation->Gettht(), + Rotation->Getpsi() ); - - _set_Gamma_vert_rad( fdmex->GetPosition()->GetGamma() ); + _set_Alpha( Translation->Getalpha() ); + _set_Beta( Translation->Getbeta() ); + + + _set_Gamma_vert_rad( Position->GetGamma() ); // set_Gamma_horiz_rad( Gamma_horiz_rad ); - _set_Earth_position_angle( fdmex->GetAuxiliary()->GetEarthPositionAngle() ); + _set_Earth_position_angle( Auxiliary->GetEarthPositionAngle() ); + + _set_Climb_Rate( Position->Gethdot() ); - _set_Climb_Rate( fdmex->GetPosition()->Gethdot() ); - for ( i = 1; i <= 3; i++ ) { - for ( j = 1; j <= 3; j++ ) { - _set_T_Local_to_Body( i, j, fdmex->GetState()->GetTl2b()(i,j) ); - } + for ( j = 1; j <= 3; j++ ) { + _set_T_Local_to_Body( i, j, State->GetTl2b(i,j) ); + } } return true; } void FGJSBsim::snap_shot(void) { - fgic->SetLatitudeRadIC(get_Lat_geocentric() ); - fgic->SetLongitudeRadIC( get_Longitude() ); - fgic->SetAltitudeFtIC( get_Altitude() ); - fgic->SetTerrainAltitudeFtIC( get_Runway_altitude() ); - fgic->SetVtrueFpsIC( get_V_rel_wind() ); - fgic->SetPitchAngleRadIC( get_Theta() ); - fgic->SetRollAngleRadIC( get_Phi() ); - fgic->SetTrueHeadingRadIC( get_Psi() ); - fgic->SetClimbRateFpsIC( get_Climb_Rate() ); -} + fgic->SetLatitudeRadIC(get_Lat_geocentric() ); + fgic->SetLongitudeRadIC( get_Longitude() ); + fgic->SetAltitudeFtIC( get_Altitude() ); + fgic->SetTerrainAltitudeFtIC( get_Runway_altitude() ); + fgic->SetVtrueFpsIC( get_V_rel_wind() ); + fgic->SetPitchAngleRadIC( get_Theta() ); + fgic->SetRollAngleRadIC( get_Phi() ); + fgic->SetTrueHeadingRadIC( get_Psi() ); + fgic->SetClimbRateFpsIC( get_Climb_Rate() ); +} bool FGJSBsim::ToggleDataLogging(void) { @@ -412,9 +419,9 @@ bool FGJSBsim::ToggleDataLogging(bool state) { //Positions void FGJSBsim::set_Latitude(double lat) { double sea_level_radius_meters,lat_geoc; - - SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat ); - + + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat ); + snap_shot(); sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc); _set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); @@ -423,24 +430,24 @@ void FGJSBsim::set_Latitude(double lat) { fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} void FGJSBsim::set_Longitude(double lon) { - + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon ); - + snap_shot(); fgic->SetLongitudeRadIC(lon); fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} void FGJSBsim::set_Altitude(double alt) { double sea_level_radius_meters,lat_geoc; - - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt ); - + + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt ); + snap_shot(); sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc); _set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); @@ -451,31 +458,31 @@ void FGJSBsim::set_Altitude(double alt) { copy_from_JSBsim(); //update the bus needTrim=true; } - + void FGJSBsim::set_V_calibrated_kts(double vc) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_V_calibrated_kts: " << vc ); - + snap_shot(); fgic->SetVcalibratedKtsIC(vc); fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} void FGJSBsim::set_Mach_number(double mach) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); - + snap_shot(); fgic->SetMachIC(mach); fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} void FGJSBsim::set_Velocities_Local( double north, double east, double down ){ - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " - << north << ", " << east << ", " << down ); - + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " + << north << ", " << east << ", " << down ); + snap_shot(); fgic->SetVnorthFpsIC(north); fgic->SetVeastFpsIC(east); @@ -483,12 +490,12 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double down ){ fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){ - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: " - << u << ", " << v << ", " << w ); - + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: " + << u << ", " << v << ", " << w ); + snap_shot(); fgic->SetUBodyFpsIC(u); fgic->SetVBodyFpsIC(v); @@ -496,81 +503,81 @@ void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){ fdmex->RunIC(fgic); //loop JSBSim once copy_from_JSBsim(); //update the bus needTrim=true; -} +} -//Euler angles +//Euler angles void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: " - << phi << ", " << theta << ", " << psi ); - + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: " + << phi << ", " << theta << ", " << psi ); + snap_shot(); fgic->SetPitchAngleRadIC(theta); fgic->SetRollAngleRadIC(phi); fgic->SetTrueHeadingRadIC(psi); fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus - needTrim=true; -} + copy_from_JSBsim(); //update the bus + needTrim=true; +} //Flight Path void FGJSBsim::set_Climb_Rate( double roc) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc ); - + snap_shot(); fgic->SetClimbRateFpsIC(roc); fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus - needTrim=true; -} + copy_from_JSBsim(); //update the bus + needTrim=true; +} void FGJSBsim::set_Gamma_vert_rad( double gamma) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma ); - + snap_shot(); fgic->SetFlightPathAngleRadIC(gamma); fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus - needTrim=true; -} + copy_from_JSBsim(); //update the bus + needTrim=true; +} //Earth void FGJSBsim::set_Sea_level_radius(double slr) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Sea_level_radius: " << slr ); - + snap_shot(); fgic->SetSeaLevelRadiusFtIC(slr); fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus - needTrim=true; -} + copy_from_JSBsim(); //update the bus + needTrim=true; +} void FGJSBsim::set_Runway_altitude(double ralt) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt ); - + snap_shot(); _set_Runway_altitude( ralt ); fgic->SetTerrainAltitudeFtIC( ralt ); fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus - needTrim=true; -} + copy_from_JSBsim(); //update the bus + needTrim=true; +} -void FGJSBsim::set_Static_pressure(double p) { +void FGJSBsim::set_Static_pressure(double p) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_pressure: " << p ); - + snap_shot(); - fdmex->GetAtmosphere()->SetExPressure(p); - if(fdmex->GetAtmosphere()->External() == true) - needTrim=true; + Atmosphere->SetExPressure(p); + if(Atmosphere->External() == true) + needTrim=true; } - -void FGJSBsim::set_Static_temperature(double T) { + +void FGJSBsim::set_Static_temperature(double T) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_temperature: " << T ); snap_shot(); - fdmex->GetAtmosphere()->SetExTemperature(T); - if(fdmex->GetAtmosphere()->External() == true) - needTrim=true; + Atmosphere->SetExTemperature(T); + if(Atmosphere->External() == true) + needTrim=true; } @@ -578,21 +585,22 @@ void FGJSBsim::set_Density(double rho) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho ); snap_shot(); - fdmex->GetAtmosphere()->SetExDensity(rho); - if(fdmex->GetAtmosphere()->External() == true) - needTrim=true; + Atmosphere->SetExDensity(rho); + if(Atmosphere->External() == true) + needTrim=true; } void FGJSBsim::set_Velocities_Local_Airmass (double wnorth, - double weast, - double wdown ) { + double weast, + double wdown ) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local_Airmass: " - << wnorth << ", " << weast << ", " << wdown ); + << wnorth << ", " << weast << ", " << wdown ); + _set_Velocities_Local_Airmass( wnorth, weast, wdown ); snap_shot(); - fdmex->GetAtmosphere()->SetWindNED(wnorth, weast, wdown ); - if(fdmex->GetAtmosphere()->External() == true) + Atmosphere->SetWindNED(wnorth, weast, wdown ); + if(Atmosphere->External() == true) needTrim=true; } diff --git a/src/FDM/JSBSim.hxx b/src/FDM/JSBSim.hxx index 3c749c4c8..a25bb91b3 100644 --- a/src/FDM/JSBSim.hxx +++ b/src/FDM/JSBSim.hxx @@ -202,6 +202,16 @@ private: FGInitialCondition *fgic; bool needTrim; + FGState* State; + FGAtmosphere* Atmosphere; + FGFCS* FCS; + FGPropulsion* Propulsion; + FGAircraft* Aircraft; + FGTranslation* Translation; + FGRotation* Rotation; + FGPosition* Position; + FGAuxiliary* Auxiliary; + int runcount; float trim_elev; float trim_throttle; diff --git a/src/FDM/JSBSim/FGAircraft.cpp b/src/FDM/JSBSim/FGAircraft.cpp index 08d8b1be2..6414a79b7 100644 --- a/src/FDM/JSBSim/FGAircraft.cpp +++ b/src/FDM/JSBSim/FGAircraft.cpp @@ -162,12 +162,15 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex), vMoments(3), vForces(3), vFs(3), + vLastFs(3), vXYZrp(3), vbaseXYZcg(3), vXYZcg(3), vXYZep(3), - vEuler(3) - + vEuler(3), + vXYZtank(3), + vDXYZcg(3), + vAeroBodyForces(3) { Name = "FGAircraft"; @@ -262,8 +265,8 @@ bool FGAircraft::Run(void) { MassChange(); FMProp(); FMAero(); - FMGear(); FMMass(); + FMGear(); nlf = 0; if (fabs(Position->GetGamma()) < 1.57) { @@ -280,7 +283,6 @@ bool FGAircraft::Run(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGAircraft::MassChange() { - static FGColumnVector vXYZtank(3); float Tw; float IXXt, IYYt, IZZt, IXZt; // unsigned int t; @@ -368,16 +370,14 @@ void FGAircraft::MassChange() { Iyy = baseIyy + IYYt; Izz = baseIzz + IZZt; Ixz = baseIxz + IXZt; - } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGAircraft::FMAero(void) { - static FGColumnVector vDXYZcg(3); - static FGColumnVector vAeroBodyForces(3); unsigned int axis_ctr,ctr; + vLastFs = vFs; for (axis_ctr=1; axis_ctr<=3; axis_ctr++) vFs(axis_ctr) = 0.0; for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) { @@ -749,3 +749,14 @@ void FGAircraft::Debug(void) //TODO: Add your source code here } +//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +float FGAircraft::GetLoD(void) { + float LoD; + if (vFs(1) != 0.00) + LoD = vFs(3)/vFs(1); + else + LoD = 0.00; + + return LoD; +} diff --git a/src/FDM/JSBSim/FGAircraft.h b/src/FDM/JSBSim/FGAircraft.h index 524eb0f8b..9bc51cc1d 100644 --- a/src/FDM/JSBSim/FGAircraft.h +++ b/src/FDM/JSBSim/FGAircraft.h @@ -58,9 +58,9 @@ INCLUDES #include "FGModel.h" #include "FGCoefficient.h" #include "FGPropulsion.h" -#include "FGLGear.h" #include "FGConfigFile.h" #include "FGMatrix.h" +#include "FGLGear.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -161,7 +161,12 @@ public: inline float GetMass(void) { return Mass; } inline FGColumnVector GetMoments(void) { return vMoments; } inline FGColumnVector GetForces(void) { return vForces; } + inline FGColumnVector GetAeroBodyForces(void) { return vAeroBodyForces; } + inline float GetAeroBodyForces(int axis) { return vAeroBodyForces(axis); } inline FGColumnVector GetvFs(void) { return vFs; } + inline float GetvFs(int axis) { return vFs(axis); } + inline FGColumnVector GetvLastFs(void) { return vLastFs; } + inline float GetvLastFs(int axis) { return vLastFs(axis); } inline float GetIxx(void) { return Ixx; } inline float GetIyy(void) { return Iyy; } inline float GetIzz(void) { return Izz; } @@ -169,6 +174,9 @@ public: inline FGColumnVector GetXYZcg(void) { return vXYZcg; } inline FGColumnVector GetXYZrp(void) { return vXYZrp; } inline FGColumnVector GetXYZep(void) { return vXYZep; } + inline float GetXYZcg(int idx) { return vXYZcg(idx); } + inline float GetXYZrp(int idx) { return vXYZrp(idx); } + inline float GetXYZep(int idx) { return vXYZep(idx); } inline float GetNlf(void) { return nlf; } inline float GetAlphaCLMax(void) { return alphaclmax; } inline float GetAlphaCLMin(void) { return alphaclmin; } @@ -183,6 +191,8 @@ public: string GetGroundReactionStrings(void); string GetGroundReactionValues(void); + float GetLoD(void); + /// Subsystem types for specifying which will be output in the FDM data logging enum SubSystems { /** Subsystem: Simulation (= 1) */ ssSimulation = 1, @@ -210,11 +220,15 @@ private: FGColumnVector vMoments; FGColumnVector vForces; FGColumnVector vFs; + FGColumnVector vLastFs; FGColumnVector vXYZrp; FGColumnVector vbaseXYZcg; FGColumnVector vXYZcg; FGColumnVector vXYZep; FGColumnVector vEuler; + FGColumnVector vXYZtank; + FGColumnVector vDXYZcg; + FGColumnVector vAeroBodyForces; float baseIxx, baseIyy, baseIzz, baseIxz, EmptyMass, Mass; float Ixx, Iyy, Izz, Ixz; float alpha, beta; diff --git a/src/FDM/JSBSim/FGAtmosphere.cpp b/src/FDM/JSBSim/FGAtmosphere.cpp index 2862dfc6d..770e6a5c3 100644 --- a/src/FDM/JSBSim/FGAtmosphere.cpp +++ b/src/FDM/JSBSim/FGAtmosphere.cpp @@ -70,7 +70,7 @@ CLASS IMPLEMENTATION FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex), - vWindNED(3) + vWindNED(3) { Name = "FGAtmosphere"; h = 0; @@ -80,7 +80,6 @@ FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex), SLdensity = density; SLsoundspeed = sqrt(SHRATIO*Reng*temperature); useExternal=false; - vWindNED(1)=0;vWindNED(2)=0;vWindNED(3)=0; if (debug_lvl & 2) cout << "Instantiated: " << Name << endl; } @@ -110,7 +109,7 @@ bool FGAtmosphere::Run(void) temperature = exTemperature; } - psiw = atan2( vWindNED(2), vWindNED(1) ); + if (vWindNED(1) != 0.0) psiw = atan2( vWindNED(2), vWindNED(1) ); if (psiw < 0) psiw += 2*M_PI; diff --git a/src/FDM/JSBSim/FGAuxiliary.h b/src/FDM/JSBSim/FGAuxiliary.h index b386896dc..2210dd368 100644 --- a/src/FDM/JSBSim/FGAuxiliary.h +++ b/src/FDM/JSBSim/FGAuxiliary.h @@ -89,8 +89,10 @@ public: inline float GetVequivalentKTS(void) { return veas*FPSTOKTS; } inline FGColumnVector GetPilotAccel(void) { return vPilotAccel; } + inline float GetPilotAccel(int idx) { return vPilotAccel(idx); } inline FGColumnVector GetNpilot(void) { return vPilotAccel*INVGRAVITY; } - + inline float GetNpilot(int idx) { return (vPilotAccel*INVGRAVITY)(idx); } + inline float GetEarthPositionAngle(void) { return earthPosAngle; } float GetHeadWind(void); diff --git a/src/FDM/JSBSim/FGCoefficient.cpp b/src/FDM/JSBSim/FGCoefficient.cpp index 449a83593..61053881a 100644 --- a/src/FDM/JSBSim/FGCoefficient.cpp +++ b/src/FDM/JSBSim/FGCoefficient.cpp @@ -48,11 +48,11 @@ INCLUDES #include "FGState.h" #include "FGFDMExec.h" -#ifndef FGFS -# include -#else -# include STL_IOMANIP -#endif +#ifndef FGFS +# include +#else +# include STL_IOMANIP +#endif static const char *IdSrc = "$Id$"; static const char *IdHdr = "ID_COEFFICIENT"; diff --git a/src/FDM/JSBSim/FGConfigFile.cpp b/src/FDM/JSBSim/FGConfigFile.cpp index 4d756992f..51e22c4c7 100644 --- a/src/FDM/JSBSim/FGConfigFile.cpp +++ b/src/FDM/JSBSim/FGConfigFile.cpp @@ -32,7 +32,7 @@ CLASS IMPLEMENTATION FGConfigFile::FGConfigFile(string cfgFileName) { - cfgfile.open(cfgFileName.c_str()); + cfgfile.open(cfgFileName.c_str(), ios::in | ios::binary ); CommentsOn = false; CurrentIndex = 0; Opened = true; diff --git a/src/FDM/JSBSim/FGDefs.h b/src/FDM/JSBSim/FGDefs.h index 30c5d5bbb..f4d727a37 100644 --- a/src/FDM/JSBSim/FGDefs.h +++ b/src/FDM/JSBSim/FGDefs.h @@ -71,9 +71,13 @@ enum eParam { FG_ALPHADOT, FG_BETA, FG_BETADOT, + FG_PHI, + FG_THT, + FG_PSI, FG_PITCHRATE, FG_ROLLRATE, FG_YAWRATE, + FG_CL_SQRD, FG_MACH, FG_ALTITUDE, FG_BI2VEL, diff --git a/src/FDM/JSBSim/FGFCS.cpp b/src/FDM/JSBSim/FGFCS.cpp index 21bb6b232..f97ecadff 100644 --- a/src/FDM/JSBSim/FGFCS.cpp +++ b/src/FDM/JSBSim/FGFCS.cpp @@ -65,7 +65,8 @@ extern short debug_lvl; CLASS IMPLEMENTATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) { +FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) +{ Name = "FGFCS"; DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = PTrimCmd = 0.0; @@ -82,7 +83,9 @@ FGFCS::~FGFCS() ThrottleCmd.clear(); ThrottlePos.clear(); - for(unsigned int i=0;iRun(); + for (i=0; iRun(); } else { } @@ -101,9 +106,12 @@ bool FGFCS::Run(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -void FGFCS::SetThrottleCmd(int engineNum, float setting) { +void FGFCS::SetThrottleCmd(int engineNum, float setting) +{ + unsigned int ctr; + if (engineNum < 0) { - for (unsigned int ctr=0;ctrGetValue("NAME"); @@ -193,11 +205,14 @@ float FGFCS::GetBrake(FGLGear::BrakeGroup bg) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -string FGFCS::GetComponentStrings(void){ +string FGFCS::GetComponentStrings(void) +{ + unsigned int comp; + string CompStrings = ""; bool firstime = true; - for (unsigned int comp = 0; comp < Components.size(); comp++) { + for (comp = 0; comp < Components.size(); comp++) { if (firstime) firstime = false; else CompStrings += ", "; @@ -211,11 +226,13 @@ string FGFCS::GetComponentStrings(void){ string FGFCS::GetComponentValues(void) { + unsigned int comp; + string CompValues = ""; char buffer[10]; bool firstime = true; - for (unsigned int comp = 0; comp < Components.size(); comp++) { + for (comp = 0; comp < Components.size(); comp++) { if (firstime) firstime = false; else CompValues += ", "; diff --git a/src/FDM/JSBSim/FGFCS.h b/src/FDM/JSBSim/FGFCS.h index a721256b5..b21dc8add 100644 --- a/src/FDM/JSBSim/FGFCS.h +++ b/src/FDM/JSBSim/FGFCS.h @@ -289,7 +289,7 @@ public: /** Sets the throttle command for the specified engine @param engine engine ID number @param cmd throttle command in percent (0 - 100)*/ - inline void SetThrottleCmd(int engine, float cmd); + void SetThrottleCmd(int engine, float cmd); //@} /// @name Aerosurface position setting @@ -321,7 +321,7 @@ public: /** Sets the actual throttle setting for the specified engine @param engine engine ID number @param cmd throttle setting in percent (0 - 100)*/ - inline void SetThrottlePos(int engine, float cmd); + void SetThrottlePos(int engine, float cmd); //@} /// @name Landing Gear brakes diff --git a/src/FDM/JSBSim/FGFDMExec.cpp b/src/FDM/JSBSim/FGFDMExec.cpp index f3098ef8a..dd5d275c2 100644 --- a/src/FDM/JSBSim/FGFDMExec.cpp +++ b/src/FDM/JSBSim/FGFDMExec.cpp @@ -177,7 +177,9 @@ bool FGFDMExec::Allocate(void) { Auxiliary = new FGAuxiliary(this); Output = new FGOutput(this); - State = new FGState(this); + State = new FGState(this); // This must be done here, as the FGState + // class needs valid pointers to the above + // model classes // Initialize models so they can communicate with each other diff --git a/src/FDM/JSBSim/FGForce.cpp b/src/FDM/JSBSim/FGForce.cpp index 903bbeea6..572e7cb62 100644 --- a/src/FDM/JSBSim/FGForce.cpp +++ b/src/FDM/JSBSim/FGForce.cpp @@ -88,9 +88,9 @@ FGColumnVector FGForce::GetBodyForces(void) { //find the distance from this vector's location to the cg //needs to be done like this to convert from structural to body coords - vDXYZ(1) = -(vXYZn(1) - fdmex->GetAircraft()->GetXYZcg()(1))*INCHTOFT; - vDXYZ(2) = (vXYZn(2) - fdmex->GetAircraft()->GetXYZcg()(2))*INCHTOFT; //cg and rp values are in inches - vDXYZ(3) = -(vXYZn(3) - fdmex->GetAircraft()->GetXYZcg()(3))*INCHTOFT; + vDXYZ(1) = -(vXYZn(1) - fdmex->GetAircraft()->GetXYZcg(1))*INCHTOFT; + vDXYZ(2) = (vXYZn(2) - fdmex->GetAircraft()->GetXYZcg(2))*INCHTOFT; //cg and rp values are in inches + vDXYZ(3) = -(vXYZn(3) - fdmex->GetAircraft()->GetXYZcg(3))*INCHTOFT; vM=vMn +vDXYZ*vFb; diff --git a/src/FDM/JSBSim/FGLGear.cpp b/src/FDM/JSBSim/FGLGear.cpp index 2ff9adbb5..0f36ade4f 100644 --- a/src/FDM/JSBSim/FGLGear.cpp +++ b/src/FDM/JSBSim/FGLGear.cpp @@ -327,9 +327,14 @@ FGColumnVector FGLGear::Force(void) FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip; } -// Compute the vertical force on the wheel. +// Compute the vertical force on the wheel using square-law damping (per comment +// in paper AIAA-2000-4303 - see header prologue comments). We might consider +// allowing for both square and linear damping force calculation. Also need to +// possibly give a "rebound damping factor" that differs from the compression +// case. NOTE: SQUARE LAW DAMPING NO GOOD! - vLocalForce(eZ) = min(-compressLength * kSpring - compressSpeed * bDamp, (float)0.0); + vLocalForce(eZ) = min(-compressLength * kSpring + - compressSpeed * bDamp, (float)0.0); MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ))); MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); @@ -347,14 +352,17 @@ FGColumnVector FGLGear::Force(void) vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; vLocalForce(eY) = SideForce*CosWheel + RollingForce*SinWheel; -// Note to Jon: At this point the forces will be too big when the airplane is stopped or -// rolling to a stop. We need to make sure that the gear forces just balance out the non-gear forces -// when the airplane is stopped. That way the airplane won't start to accelerate until the non-gear -// forces are larger than the gear forces. I think that the proper fix should go into FGAircraft::FMGear. -// This routine would only compute the local strut forces and return them to FMGear. All of the gear -// forces would get adjusted in FMGear using the total non-gear forces. Then the gear moments would be -// calculated. If strange things start happening to the airplane during testing as it rolls to a stop, -// then we need to implement this change. I ran out of time to do it now but have the equations. +// Note to Jon: At this point the forces will be too big when the airplane is +// stopped or rolling to a stop. We need to make sure that the gear forces just +// balance out the non-gear forces when the airplane is stopped. That way the +// airplane won't start to accelerate until the non-gear/ forces are larger than +// the gear forces. I think that the proper fix should go into FGAircraft::FMGear. +// This routine would only compute the local strut forces and return them to +// FMGear. All of the gear forces would get adjusted in FMGear using the total +// non-gear forces. Then the gear moments would be calculated. If strange things +// start happening to the airplane during testing as it rolls to a stop, then we +// need to implement this change. I ran out of time to do it now but have the +// equations. // Transform the forces back to the body frame and compute the moment. diff --git a/src/FDM/JSBSim/FGLGear.h b/src/FDM/JSBSim/FGLGear.h index 8f1a18672..303e7239b 100644 --- a/src/FDM/JSBSim/FGLGear.h +++ b/src/FDM/JSBSim/FGLGear.h @@ -46,7 +46,6 @@ INCLUDES #include "FGConfigFile.h" #include "FGMatrix.h" #include "FGFDMExec.h" -#include "FGState.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% DEFINITIONS @@ -62,6 +61,7 @@ class FGAircraft; class FGPosition; class FGRotation; class FGFCS; +class FGState; /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] @@ -163,6 +163,8 @@ CLASS DOCUMENTATION NASA-Ames", NASA CR-2497, January 1975 @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", Wiley & Sons, 1979 ISBN 0-471-03032-5 + @see W. A. Ragsdale, "A Generic Landing Gear Dynamics Model for LASRS++", + AIAA-2000-4303 */ /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -194,8 +196,10 @@ public: /// Gets the location of the gear in Body axes FGColumnVector GetBodyLocation(void) { return vWhlBodyVec; } - + float GetBodyLocation(int idx) { return vWhlBodyVec(idx); } + FGColumnVector GetLocalGear(void) { return vLocalGear; } + float GetLocalGear(int idx) { return vLocalGear(idx); } /// Gets the name of the gear inline string GetName(void) {return name; } @@ -264,4 +268,7 @@ private: #include "FGFCS.h" //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +#include "FGState.h" + #endif diff --git a/src/FDM/JSBSim/FGMatrix.cpp b/src/FDM/JSBSim/FGMatrix.cpp index a7e068783..d59ff81fb 100644 --- a/src/FDM/JSBSim/FGMatrix.cpp +++ b/src/FDM/JSBSim/FGMatrix.cpp @@ -603,9 +603,9 @@ FGColumnVector FGColumnVector::Normalize(void) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector& FGColumnVector::operator*(const FGColumnVector& V) +FGColumnVector FGColumnVector::operator*(const FGColumnVector& V) { - static FGColumnVector Product(3); + FGColumnVector Product(3); if (Rows() != 3 || V.Rows() != 3) { MatrixException mE; @@ -622,9 +622,9 @@ FGColumnVector& FGColumnVector::operator*(const FGColumnVector& V) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -FGColumnVector& FGColumnVector::multElementWise(const FGColumnVector& V) +FGColumnVector FGColumnVector::multElementWise(const FGColumnVector& V) { - static FGColumnVector Product(3); + FGColumnVector Product(3); if (Rows() != 3 || V.Rows() != 3) { MatrixException mE; diff --git a/src/FDM/JSBSim/FGMatrix.h b/src/FDM/JSBSim/FGMatrix.h index c34dac64d..8c92e871c 100644 --- a/src/FDM/JSBSim/FGMatrix.h +++ b/src/FDM/JSBSim/FGMatrix.h @@ -142,7 +142,7 @@ public: ~FGColumnVector(void); FGColumnVector operator*(const double scalar); - FGColumnVector& operator*(const FGColumnVector& V); // Cross product operator + FGColumnVector operator*(const FGColumnVector& V); // Cross product operator FGColumnVector operator/(const double scalar); FGColumnVector operator+(const FGColumnVector& B); // must not return reference FGColumnVector operator-(const FGColumnVector& B); @@ -154,7 +154,7 @@ public: double& operator()(int m) const; - FGColumnVector& multElementWise(const FGColumnVector& V); + FGColumnVector multElementWise(const FGColumnVector& V); private: void Debug(void); diff --git a/src/FDM/JSBSim/FGNozzle.cpp b/src/FDM/JSBSim/FGNozzle.cpp index 7d4db6af3..cf1ed5b0f 100644 --- a/src/FDM/JSBSim/FGNozzle.cpp +++ b/src/FDM/JSBSim/FGNozzle.cpp @@ -74,6 +74,7 @@ FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMEx } Thrust = 0; + Type = ttNozzle; if (debug_lvl & 2) cout << "Instantiated: FGNozzle" << endl; } diff --git a/src/FDM/JSBSim/FGOutput.cpp b/src/FDM/JSBSim/FGOutput.cpp index cf1afb5b9..f7ed98b62 100644 --- a/src/FDM/JSBSim/FGOutput.cpp +++ b/src/FDM/JSBSim/FGOutput.cpp @@ -161,7 +161,8 @@ void FGOutput::DelimitedOutput(void) } if (SubSystems & FGAircraft::ssForces) { cout << ", "; - cout << "XsForce, YsForce, ZsForce, "; + cout << "Drag, Side, Lift, "; + cout << "L/D, "; cout << "Xforce, Yforce, Zforce"; } if (SubSystems & FGAircraft::ssMoments) { @@ -235,6 +236,7 @@ void FGOutput::DelimitedOutput(void) if (SubSystems & FGAircraft::ssForces) { cout << ", "; cout << Aircraft->GetvFs() << ", "; + cout << Aircraft->GetLoD() << ", "; cout << Aircraft->GetForces(); } if (SubSystems & FGAircraft::ssMoments) { @@ -313,7 +315,8 @@ void FGOutput::DelimitedOutput(string fname) } if (SubSystems & FGAircraft::ssForces) { datafile << ", "; - datafile << "XsForce, YsForce, ZsForce, "; + datafile << "Drag, Side, Lift, "; + datafile << "L/D, "; datafile << "Xforce, Yforce, Zforce"; } if (SubSystems & FGAircraft::ssMoments) { @@ -390,6 +393,7 @@ void FGOutput::DelimitedOutput(string fname) if (SubSystems & FGAircraft::ssForces) { datafile << ", "; datafile << Aircraft->GetvFs() << ", "; + datafile << Aircraft->GetLoD() << ", "; datafile << Aircraft->GetForces(); } if (SubSystems & FGAircraft::ssMoments) { diff --git a/src/FDM/JSBSim/FGPiston.cpp b/src/FDM/JSBSim/FGPiston.cpp index 914d7fa1b..5cd169293 100644 --- a/src/FDM/JSBSim/FGPiston.cpp +++ b/src/FDM/JSBSim/FGPiston.cpp @@ -84,6 +84,7 @@ FGPiston::FGPiston(FGFDMExec* exec, FGConfigFile* Eng_cfg) : FGEngine(exec) } } + Type = etPiston; EngineNumber = 0; if (debug_lvl & 2) cout << "Instantiated: FGPiston" << endl; diff --git a/src/FDM/JSBSim/FGPropeller.cpp b/src/FDM/JSBSim/FGPropeller.cpp index 27ea9266d..54321871f 100644 --- a/src/FDM/JSBSim/FGPropeller.cpp +++ b/src/FDM/JSBSim/FGPropeller.cpp @@ -96,6 +96,9 @@ FGPropeller::FGPropeller(FGFDMExec* exec, FGConfigFile* Prop_cfg) : FGThruster(e } } + Type = ttPropeller; + RPM = 0; + if (debug_lvl & 2) cout << "Instantiated: FGPropeller" << endl; } @@ -146,7 +149,7 @@ float FGPropeller::Calculate(float PowerAvailable) vFn(1) = Thrust; omega = RPS*2.0*M_PI; - if (omega <= 500) omega = 1.0; + if (omega <= 5) omega = 1.0; Torque = PowerAvailable / omega; RPM = (RPS + ((Torque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0; diff --git a/src/FDM/JSBSim/FGRotation.cpp b/src/FDM/JSBSim/FGRotation.cpp index e96acb1a0..d9fbb0301 100644 --- a/src/FDM/JSBSim/FGRotation.cpp +++ b/src/FDM/JSBSim/FGRotation.cpp @@ -81,7 +81,8 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex), vPQRdot(3), vMoments(3), vEuler(3), - vEulerRates(3) + vEulerRates(3), + vlastPQRdot(3) { Name = "FGRotation"; cTht=cPhi=cPsi=1.0; @@ -103,7 +104,6 @@ bool FGRotation::Run(void) { float L2, N1; float tTheta; - static FGColumnVector vlastPQRdot(3); if (!FGModel::Run()) { GetState(); diff --git a/src/FDM/JSBSim/FGRotation.h b/src/FDM/JSBSim/FGRotation.h index d072bda1e..2c943bce9 100644 --- a/src/FDM/JSBSim/FGRotation.h +++ b/src/FDM/JSBSim/FGRotation.h @@ -90,9 +90,13 @@ public: bool Run(void); inline FGColumnVector GetPQR(void) {return vPQR;} + inline float GetPQR(int axis) {return vPQR(axis);} inline FGColumnVector GetPQRdot(void) {return vPQRdot;} + inline float GetPQRdot(int idx) {return vPQRdot(idx);} inline FGColumnVector GetEuler(void) {return vEuler;} + inline float GetEuler(int axis) {return vEuler(axis);} inline FGColumnVector GetEulerRates(void) { return vEulerRates; } + inline float GetEulerRates(int axis) { return vEulerRates(axis); } inline void SetPQR(FGColumnVector tt) {vPQR = tt;} inline void SetEuler(FGColumnVector tt) {vEuler = tt;} @@ -114,6 +118,7 @@ private: FGColumnVector vMoments; FGColumnVector vEuler; FGColumnVector vEulerRates; + FGColumnVector vlastPQRdot; float cTht,sTht; float cPhi,sPhi; diff --git a/src/FDM/JSBSim/FGState.cpp b/src/FDM/JSBSim/FGState.cpp index 4d930ece6..8c7bcedd6 100644 --- a/src/FDM/JSBSim/FGState.cpp +++ b/src/FDM/JSBSim/FGState.cpp @@ -53,15 +53,6 @@ INCLUDES #endif #include "FGState.h" -#include "FGFDMExec.h" -#include "FGAtmosphere.h" -#include "FGFCS.h" -#include "FGAircraft.h" -#include "FGTranslation.h" -#include "FGRotation.h" -#include "FGPosition.h" -#include "FGAuxiliary.h" -#include "FGOutput.h" static const char *IdSrc = "$Id$"; static const char *IdHdr = ID_STATE; @@ -89,7 +80,10 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), mTl2b(3,3), mTs2b(3,3), vQtrn(4), - vlastQdot(4) + vlastQdot(4), + vQdot(4), + vTmp(4), + vEuler(3) { FDMExec = fdex; @@ -97,7 +91,15 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), sim_time = 0.0; dt = 1.0/120.0; ActiveEngine = -1; - + + Aircraft = FDMExec->GetAircraft(); + Translation = FDMExec->GetTranslation(); + Rotation = FDMExec->GetRotation(); + Position = FDMExec->GetPosition(); + FCS = FDMExec->GetFCS(); + Output = FDMExec->GetOutput(); + Atmosphere = FDMExec->GetAtmosphere(); + RegisterVariable(FG_TIME, " time " ); RegisterVariable(FG_QBAR, " qbar " ); RegisterVariable(FG_WINGAREA, " wing_area " ); @@ -107,9 +109,13 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3), RegisterVariable(FG_ALPHADOT, " alphadot " ); RegisterVariable(FG_BETA, " beta " ); RegisterVariable(FG_BETADOT, " betadot " ); + RegisterVariable(FG_PHI, " roll_angle " ); + RegisterVariable(FG_THT, " pitch_angle " ); + RegisterVariable(FG_PSI, " heading_angle " ); RegisterVariable(FG_PITCHRATE, " pitch_rate " ); RegisterVariable(FG_ROLLRATE, " roll_rate " ); RegisterVariable(FG_YAWRATE, " yaw_rate " ); + RegisterVariable(FG_CL_SQRD, " Clift_sqrd " ); RegisterVariable(FG_MACH, " mach " ); RegisterVariable(FG_ALTITUDE, " altitude " ); RegisterVariable(FG_BI2VEL, " BI2Vel " ); @@ -149,77 +155,93 @@ FGState::~FGState() //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% float FGState::GetParameter(eParam val_idx) { + float scratch; + switch(val_idx) { case FG_TIME: return sim_time; case FG_QBAR: - return FDMExec->GetTranslation()->Getqbar(); + return Translation->Getqbar(); case FG_WINGAREA: - return FDMExec->GetAircraft()->GetWingArea(); + return Aircraft->GetWingArea(); case FG_WINGSPAN: - return FDMExec->GetAircraft()->GetWingSpan(); + return Aircraft->GetWingSpan(); case FG_CBAR: - return FDMExec->GetAircraft()->Getcbar(); + return Aircraft->Getcbar(); case FG_ALPHA: - return FDMExec->GetTranslation()->Getalpha(); + return Translation->Getalpha(); case FG_ALPHADOT: - return FDMExec->GetTranslation()->Getadot(); + return Translation->Getadot(); case FG_BETA: - return FDMExec->GetTranslation()->Getbeta(); + return Translation->Getbeta(); case FG_BETADOT: - return FDMExec->GetTranslation()->Getbdot(); + return Translation->Getbdot(); + case FG_PHI: + return Rotation->Getphi(); + case FG_THT: + return Rotation->Gettht(); + case FG_PSI: + return Rotation->Getpsi(); case FG_PITCHRATE: - return (FDMExec->GetRotation()->GetPQR())(2); + return Rotation->GetPQR(eQ); case FG_ROLLRATE: - return (FDMExec->GetRotation()->GetPQR())(1); + return Rotation->GetPQR(eP); case FG_YAWRATE: - return (FDMExec->GetRotation()->GetPQR())(3); + return Rotation->GetPQR(eR); + case FG_CL_SQRD: + if (Translation->Getqbar() > 0.00) + scratch = Aircraft->GetvLastFs(eLift)/(Aircraft->GetWingArea()*Translation->Getqbar()); + else + scratch = 0.0; + return scratch*scratch; case FG_ELEVATOR_POS: - return FDMExec->GetFCS()->GetDePos(); + return FCS->GetDePos(); case FG_AILERON_POS: - return FDMExec->GetFCS()->GetDaPos(); + return FCS->GetDaPos(); case FG_RUDDER_POS: - return FDMExec->GetFCS()->GetDrPos(); + return FCS->GetDrPos(); case FG_SPDBRAKE_POS: - return FDMExec->GetFCS()->GetDsbPos(); + return FCS->GetDsbPos(); case FG_SPOILERS_POS: - return FDMExec->GetFCS()->GetDspPos(); + return FCS->GetDspPos(); case FG_FLAPS_POS: - return FDMExec->GetFCS()->GetDfPos(); + return FCS->GetDfPos(); case FG_ELEVATOR_CMD: - return FDMExec->GetFCS()->GetDeCmd(); + return FCS->GetDeCmd(); case FG_AILERON_CMD: - return FDMExec->GetFCS()->GetDaCmd(); + return FCS->GetDaCmd(); case FG_RUDDER_CMD: - return FDMExec->GetFCS()->GetDrCmd(); + return FCS->GetDrCmd(); case FG_SPDBRAKE_CMD: - return FDMExec->GetFCS()->GetDsbCmd(); + return FCS->GetDsbCmd(); case FG_SPOILERS_CMD: - return FDMExec->GetFCS()->GetDspCmd(); + return FCS->GetDspCmd(); case FG_FLAPS_CMD: - return FDMExec->GetFCS()->GetDfCmd(); + return FCS->GetDfCmd(); case FG_MACH: - return FDMExec->GetTranslation()->GetMach(); + return Translation->GetMach(); case FG_ALTITUDE: - return FDMExec->GetPosition()->Geth(); + return Position->Geth(); case FG_BI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(Translation->GetVt() > 0) + return Aircraft->GetWingSpan()/(2.0 * Translation->GetVt()); else return 0; case FG_CI2VEL: - if(FDMExec->GetTranslation()->GetVt() > 0) - return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); + if(Translation->GetVt() > 0) + return Aircraft->Getcbar()/(2.0 * Translation->GetVt()); else return 0; case FG_THROTTLE_CMD: - return FDMExec->GetFCS()->GetThrottleCmd(0); + if (ActiveEngine < 0) return FCS->GetThrottleCmd(0); + else return FCS->GetThrottleCmd(ActiveEngine); case FG_THROTTLE_POS: - return FDMExec->GetFCS()->GetThrottlePos(0); + if (ActiveEngine < 0) return FCS->GetThrottlePos(0); + else return FCS->GetThrottlePos(ActiveEngine); case FG_HOVERB: - return FDMExec->GetPosition()->GetHOverB(); + return Position->GetHOverB(); case FG_PITCH_TRIM_CMD: - return FDMExec->GetFCS()->GetPitchTrimCmd(); + return FCS->GetPitchTrimCmd(); default: cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl; return 0.0; @@ -244,47 +266,47 @@ eParam FGState::GetParameterIndex(string val_string) { void FGState::SetParameter(eParam val_idx, float val) { switch(val_idx) { case FG_ELEVATOR_POS: - FDMExec->GetFCS()->SetDePos(val); + FCS->SetDePos(val); break; case FG_AILERON_POS: - FDMExec->GetFCS()->SetDaPos(val); + FCS->SetDaPos(val); break; case FG_RUDDER_POS: - FDMExec->GetFCS()->SetDrPos(val); + FCS->SetDrPos(val); break; case FG_SPDBRAKE_POS: - FDMExec->GetFCS()->SetDsbPos(val); + FCS->SetDsbPos(val); break; case FG_SPOILERS_POS: - FDMExec->GetFCS()->SetDspPos(val); + FCS->SetDspPos(val); break; case FG_FLAPS_POS: - FDMExec->GetFCS()->SetDfPos(val); + FCS->SetDfPos(val); break; case FG_THROTTLE_POS: - FDMExec->GetFCS()->SetThrottlePos(ActiveEngine,val); + FCS->SetThrottlePos(ActiveEngine,val); break; case FG_ELEVATOR_CMD: - FDMExec->GetFCS()->SetDeCmd(val); + FCS->SetDeCmd(val); break; case FG_AILERON_CMD: - FDMExec->GetFCS()->SetDaCmd(val); + FCS->SetDaCmd(val); break; case FG_RUDDER_CMD: - FDMExec->GetFCS()->SetDrCmd(val); + FCS->SetDrCmd(val); break; case FG_SPDBRAKE_CMD: - FDMExec->GetFCS()->SetDsbCmd(val); + FCS->SetDsbCmd(val); break; case FG_SPOILERS_CMD: - FDMExec->GetFCS()->SetDspCmd(val); + FCS->SetDspCmd(val); break; case FG_FLAPS_CMD: - FDMExec->GetFCS()->SetDfCmd(val); + FCS->SetDfCmd(val); break; case FG_THROTTLE_CMD: - FDMExec->GetFCS()->SetThrottleCmd(ActiveEngine,val); + FCS->SetThrottleCmd(ActiveEngine,val); break; case FG_ACTIVE_ENGINE: @@ -292,19 +314,19 @@ void FGState::SetParameter(eParam val_idx, float val) { break; case FG_LEFT_BRAKE_CMD: - FDMExec->GetFCS()->SetLBrake(val); + FCS->SetLBrake(val); break; case FG_CENTER_BRAKE_CMD: - FDMExec->GetFCS()->SetCBrake(val); + FCS->SetCBrake(val); break; case FG_RIGHT_BRAKE_CMD: - FDMExec->GetFCS()->SetRBrake(val); + FCS->SetRBrake(val); break; case FG_SET_LOGGING: - if (val < -0.01) FDMExec->GetOutput()->Disable(); - else if (val > 0.01) FDMExec->GetOutput()->Enable(); - else FDMExec->GetOutput()->Toggle(); + if (val < -0.01) Output->Disable(); + else if (val > 0.01) Output->Enable(); + else Output->Toggle(); break; default: @@ -325,7 +347,7 @@ bool FGState::Reset(string path, string acname, string fname) { resetDef = path + "/" + acname + "/" + fname + ".xml"; - ifstream resetfile(resetDef.c_str()); + ifstream resetfile(resetDef.c_str(), ios::in | ios::binary ); if (resetfile) { resetfile >> U; @@ -339,9 +361,9 @@ bool FGState::Reset(string path, string acname, string fname) { resetfile >> h; resetfile.close(); - FDMExec->GetPosition()->SetLatitude(latitude*DEGTORAD); - FDMExec->GetPosition()->SetLongitude(longitude*DEGTORAD); - FDMExec->GetPosition()->Seth(h); + Position->SetLatitude(latitude*DEGTORAD); + Position->SetLongitude(longitude*DEGTORAD); + Position->Seth(h); Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD, latitude*DEGTORAD, longitude*DEGTORAD, h); @@ -363,15 +385,15 @@ void FGState::Initialize(float U, float V, float W, float Latitude, float Longitude, float H) { FGColumnVector vUVW(3); FGColumnVector vLocalVelNED(3); - FGColumnVector vEuler(3); + FGColumnVector vLocalEuler(3); float alpha, beta; float qbar, Vt; - FDMExec->GetPosition()->SetLatitude(Latitude); - FDMExec->GetPosition()->SetLongitude(Longitude); - FDMExec->GetPosition()->Seth(H); + Position->SetLatitude(Latitude); + Position->SetLongitude(Longitude); + Position->Seth(H); - FDMExec->GetAtmosphere()->Run(); + Atmosphere->Run(); if (W != 0.0) alpha = U*U > 0.0 ? atan2(W, U) : 0.0; @@ -383,25 +405,25 @@ void FGState::Initialize(float U, float V, float W, beta = 0.0; vUVW << U << V << W; - FDMExec->GetTranslation()->SetUVW(vUVW); + Translation->SetUVW(vUVW); - vEuler << phi << tht << psi; - FDMExec->GetRotation()->SetEuler(vEuler); + vLocalEuler << phi << tht << psi; + Rotation->SetEuler(vLocalEuler); - FDMExec->GetTranslation()->SetAB(alpha, beta); + Translation->SetAB(alpha, beta); Vt = sqrt(U*U + V*V + W*W); - FDMExec->GetTranslation()->SetVt(Vt); + Translation->SetVt(Vt); - FDMExec->GetTranslation()->SetMach(Vt/FDMExec->GetAtmosphere()->GetSoundSpeed()); + Translation->SetMach(Vt/Atmosphere->GetSoundSpeed()); - qbar = 0.5*(U*U + V*V + W*W)*FDMExec->GetAtmosphere()->GetDensity(); - FDMExec->GetTranslation()->Setqbar(qbar); + qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity(); + Translation->Setqbar(qbar); InitMatrices(phi, tht, psi); vLocalVelNED = mTb2l*vUVW; - FDMExec->GetPosition()->SetvVel(vLocalVelNED); + Position->SetvVel(vLocalVelNED); } //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% @@ -424,8 +446,8 @@ void FGState::Initialize(FGInitialCondition *FGIC) { Initialize(U, V, W, phi, tht, psi, latitude, longitude, h); - FDMExec->GetPosition()->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); - FDMExec->GetPosition()->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + + Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); + Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + FGIC->GetTerrainAltitudeFtIC() ); } @@ -436,15 +458,15 @@ bool FGState::StoreData(string fname) { ofstream datafile(fname.c_str()); if (datafile) { - datafile << (FDMExec->GetTranslation()->GetUVW())(1); - datafile << (FDMExec->GetTranslation()->GetUVW())(2); - datafile << (FDMExec->GetTranslation()->GetUVW())(3); - datafile << FDMExec->GetPosition()->GetLatitude(); - datafile << FDMExec->GetPosition()->GetLongitude(); - datafile << (FDMExec->GetRotation()->GetEuler())(1); - datafile << (FDMExec->GetRotation()->GetEuler())(2); - datafile << (FDMExec->GetRotation()->GetEuler())(3); - datafile << FDMExec->GetPosition()->Geth(); + datafile << Translation->GetUVW(eU); + datafile << Translation->GetUVW(eV); + datafile << Translation->GetUVW(eW); + datafile << Position->GetLatitude(); + datafile << Position->GetLongitude(); + datafile << Rotation->GetEuler(ePhi); + datafile << Rotation->GetEuler(eTht); + datafile << Rotation->GetEuler(ePsi); + datafile << Position->Geth(); datafile.close(); return true; } else { @@ -524,9 +546,6 @@ void FGState::CalcMatrices(void) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { - static FGColumnVector vQdot(4); - static FGColumnVector vTmp(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)); @@ -541,8 +560,6 @@ void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% FGColumnVector FGState::CalcEuler(void) { - static FGColumnVector vEuler(3); - if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001; if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001; diff --git a/src/FDM/JSBSim/FGState.h b/src/FDM/JSBSim/FGState.h index 196b4035b..9061357f4 100644 --- a/src/FDM/JSBSim/FGState.h +++ b/src/FDM/JSBSim/FGState.h @@ -62,11 +62,30 @@ INCLUDES #include "FGMatrix.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -DEFINES +DEFINITIONS %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ #define ID_STATE "$Id$" +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGAircraft; +class FGTranslation; +class FGRotation; +class FGAtmosphere; +class FGOutput; +class FGPosition; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% CLASS DECLARATION %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -78,6 +97,11 @@ public: FGState(FGFDMExec*); ~FGState(); + enum {ePhi=1, eTht, ePsi}; + enum {eP=1, eQ, eR}; + enum {eU=1, eV, eW}; + enum {eDrag=1, eSide, eLift}; + bool Reset(string, string, string); void Initialize(float, float, float, float, float, float, float, float, float); void Initialize(FGInitialCondition *FGIC); @@ -115,7 +139,9 @@ public: FGColumnVector CalcEuler(void); FGMatrix GetTs2b(float alpha, float beta); FGMatrix GetTl2b(void) { return mTl2b; } + float GetTl2b(int i, int j) { return mTl2b(i,j);} FGMatrix GetTb2l(void) { return mTb2l; } + float GetTb2l(int i, int j) { return mTb2l(i,j);} typedef map ParamMap; ParamMap paramdef; @@ -132,16 +158,35 @@ private: FGColumnVector vQtrn; FGColumnVector vlastQdot; + FGAircraft* Aircraft; + FGPosition* Position; + FGTranslation* Translation; + FGRotation* Rotation; + FGOutput* Output; + FGAtmosphere* Atmosphere; + FGFCS* FCS; + typedef map CoeffMap; CoeffMap coeffdef; void Debug(void); int ActiveEngine; -protected: - enum {ePhi=1, eTht, ePsi}; - enum {eP=1, eQ, eR}; + FGColumnVector vQdot; + FGColumnVector vTmp; + FGColumnVector vEuler; }; //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +#include "FGFDMExec.h" +#include "FGAtmosphere.h" +#include "FGFCS.h" +#include "FGTranslation.h" +#include "FGRotation.h" +#include "FGPosition.h" +//#include "FGAuxiliary.h" +#include "FGOutput.h" +#include "FGAircraft.h" + #endif diff --git a/src/FDM/JSBSim/FGThruster.h b/src/FDM/JSBSim/FGThruster.h index 723a09fc6..50043c3a3 100644 --- a/src/FDM/JSBSim/FGThruster.h +++ b/src/FDM/JSBSim/FGThruster.h @@ -66,6 +66,7 @@ public: float GetThrust(void) {return Thrust;} eType GetType(void) {return Type;} string GetName(void) {return Name;} + virtual float GetRPM(void) { return 0.0; }; protected: eType Type; diff --git a/src/FDM/JSBSim/FGTranslation.cpp b/src/FDM/JSBSim/FGTranslation.cpp index 82407997f..d19b18525 100644 --- a/src/FDM/JSBSim/FGTranslation.cpp +++ b/src/FDM/JSBSim/FGTranslation.cpp @@ -84,7 +84,9 @@ FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex), vNcg(3), vPQR(3), vForces(3), - vEuler(3) + vEuler(3), + vlastUVWdot(3), + mVel(3,3) { Name = "FGTranslation"; qbar = 0; @@ -107,8 +109,6 @@ FGTranslation::~FGTranslation() //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% bool FGTranslation::Run(void) { - static FGColumnVector vlastUVWdot(3); - static FGMatrix mVel(3,3); if (!FGModel::Run()) { diff --git a/src/FDM/JSBSim/FGTranslation.h b/src/FDM/JSBSim/FGTranslation.h index fff41ab0a..680cab4d7 100644 --- a/src/FDM/JSBSim/FGTranslation.h +++ b/src/FDM/JSBSim/FGTranslation.h @@ -81,9 +81,12 @@ public: FGTranslation(FGFDMExec*); ~FGTranslation(); - inline FGColumnVector GetUVW(void) { return vUVW; } - inline FGColumnVector GetUVWdot(void) { return vUVWdot; } - inline FGColumnVector GetNcg(void) { return vNcg; } + inline FGColumnVector GetUVW (void) { return vUVW; } + inline float GetUVW (int idx) { return vUVW(idx); } + inline FGColumnVector GetUVWdot(void) { return vUVWdot; } + inline float GetUVWdot(int idx) { return vUVWdot(idx); } + inline FGColumnVector GetNcg (void) { return vNcg; } + inline float GetNcg (int idx) { return vNcg(idx); } inline float Getalpha(void) { return alpha; } inline float Getbeta (void) { return beta; } @@ -116,6 +119,8 @@ private: FGColumnVector vPQR; FGColumnVector vForces; FGColumnVector vEuler; + FGColumnVector vlastUVWdot; + FGMatrix mVel; float Vt, qbar, Mach; float Mass, dt; diff --git a/src/FDM/JSBSim/FGTrim.cpp b/src/FDM/JSBSim/FGTrim.cpp index 67fd7d53f..6f32949f6 100644 --- a/src/FDM/JSBSim/FGTrim.cpp +++ b/src/FDM/JSBSim/FGTrim.cpp @@ -165,9 +165,9 @@ void FGTrim::ReportState(void) { cout << endl << " JSBSim State" << endl; sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n", fdmex->GetAircraft()->GetWeight(), - fdmex->GetAircraft()->GetXYZcg()(1), - fdmex->GetAircraft()->GetXYZcg()(2), - fdmex->GetAircraft()->GetXYZcg()(3) ); + fdmex->GetAircraft()->GetXYZcg(1), + fdmex->GetAircraft()->GetXYZcg(2), + fdmex->GetAircraft()->GetXYZcg(3)); cout << out; if( fdmex->GetFCS()->GetDfPos() <= 0.01) sprintf(flap,"Up"); diff --git a/src/FDM/JSBSim/FGTrimAxis.cpp b/src/FDM/JSBSim/FGTrimAxis.cpp index 692b82c5a..51ccb932d 100644 --- a/src/FDM/JSBSim/FGTrimAxis.cpp +++ b/src/FDM/JSBSim/FGTrimAxis.cpp @@ -152,12 +152,12 @@ FGTrimAxis::~FGTrimAxis() void FGTrimAxis::getState(void) { switch(state) { - case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot()(1); break; - case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot()(2); break; - case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot()(3); break; - case tQdot: state_value=fdmex->GetRotation()->GetPQRdot()(2);break; - case tPdot: state_value=fdmex->GetRotation()->GetPQRdot()(1); break; - case tRdot: state_value=fdmex->GetRotation()->GetPQRdot()(3); break; + case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot(1); break; + case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot(2); break; + case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot(3); break; + case tQdot: state_value=fdmex->GetRotation()->GetPQRdot(2);break; + case tPdot: state_value=fdmex->GetRotation()->GetPQRdot(1); break; + case tRdot: state_value=fdmex->GetRotation()->GetPQRdot(3); break; case tHmgt: state_value=computeHmgt(); break; } } @@ -246,7 +246,7 @@ void FGTrimAxis::SetThetaOnGround(float ff) { i=0; ref=-1; center=-1; while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) { if(fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) { - if(fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01) + if(fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(2)) > 0.01) ref=i; else center=i; @@ -260,9 +260,9 @@ void FGTrimAxis::SetThetaOnGround(float ff) { if(ref >= 0) { float sp=fdmex->GetRotation()->GetSinphi(); float cp=fdmex->GetRotation()->GetCosphi(); - float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1); - float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2); - float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3); + float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(1); + float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(2); + float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(3); float hagl = -1*lx*sin(ff) + ly*sp*cos(ff) + lz*cp*cos(ff); @@ -291,22 +291,22 @@ bool FGTrimAxis::initTheta(void) { //find the first wheel unit forward of the cg //the list is short so a simple linear search is fine for( i=0; iGetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) > 0 ) { + if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(1) > 0 ) { iForward=i; break; } } //now find the first wheel unit aft of the cg for( i=0; iGetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) < 0 ) { + if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(1) < 0 ) { iAft=i; break; } } // now adjust theta till the wheels are the same distance from the ground - zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3); - zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3); + zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear(3); + zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear(3); zDiff = zForward - zAft; level=false; theta=fgic->GetPitchAngleDegIC(); @@ -314,8 +314,8 @@ bool FGTrimAxis::initTheta(void) { theta+=2.0*zDiff; fgic->SetPitchAngleDegIC(theta); fdmex->RunIC(fgic); - zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3); - zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3); + zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear(3); + zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear(3); zDiff = zForward - zAft; //cout << endl << theta << " " << zDiff << endl; //cout << "0: " << fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear() << endl; @@ -345,16 +345,16 @@ void FGTrimAxis::SetPhiOnGround(float ff) { //must have an off-center unit here while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) { if( (fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) && - (fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(2)) > 0.01)) + (fabs(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(2)) > 0.01)) ref=i; i++; } if(ref >= 0) { float st=fdmex->GetRotation()->GetSintht(); float ct=fdmex->GetRotation()->GetCostht(); - float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1); - float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2); - float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3); + float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(1); + float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(2); + float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(3); float hagl = -1*lx*st + ly*sin(ff)*ct + lz*cos(ff)*ct; -- 2.39.5