fly/flightgear
1
0
Fork 0
Code Activity

Synced with latest JSBSim. X15 works for me, but C172 segfaults.

Browse source
This commit is contained in:
curt 2001-04-17 21:19:54 +00:00
parent 763193237b
commit f3ca9b2e05
30 changed files with 622 additions and 458 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

544
src/FDM/JSBSim.cxx
View file

@ -64,6 +64,17 @@ FGJSBsim::FGJSBsim( double dt )
bool result; bool result;
fdmex=new FGFDMExec; 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); fgic=new FGInitialCondition(fdmex);
needTrim=true; needTrim=true;
@ -73,22 +84,23 @@ FGJSBsim::FGJSBsim( double dt )
SGPath engine_path( globals->get_fg_root() ); SGPath engine_path( globals->get_fg_root() );
engine_path.append( "Engine" ); engine_path.append( "Engine" );
set_delta_t( dt ); set_delta_t( dt );
fdmex->GetState()->Setdt( dt ); State->Setdt( dt );
result = fdmex->LoadModel( aircraft_path.str(), result = fdmex->LoadModel( aircraft_path.str(),
engine_path.str(), engine_path.str(),
fgGetString("/sim/aircraft") ); fgGetString("/sim/aircraft") );
int Neng=fdmex->GetPropulsion()->GetNumEngines();
// int Neng=fdmex->GetAircraft()->GetNumEngines(); int Neng = Propulsion->GetNumEngines();
SG_LOG(SG_FLIGHT,SG_INFO, "Neng: " << Neng ); SG_LOG(SG_FLIGHT,SG_INFO, "Neng: " << Neng );
for(int i=0;i<Neng;i++) { for(int i=0;i<Neng;i++) {
add_engine( FGEngInterface() ); add_engine( FGEngInterface() );
} }
fgSetDouble("/fdm/trim/pitch-trim", fdmex->GetFCS()->GetPitchTrimCmd()); fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd());
fgSetDouble("/fdm/trim/throttle", fdmex->GetFCS()->GetThrottleCmd(0)); fgSetDouble("/fdm/trim/throttle", FCS->GetThrottleCmd(0));
fgSetDouble("/fdm/trim/aileron", fdmex->GetFCS()->GetDaCmd()); fgSetDouble("/fdm/trim/aileron", FCS->GetDaCmd());
fgSetDouble("/fdm/trim/rudder", fdmex->GetFCS()->GetDrCmd()); fgSetDouble("/fdm/trim/rudder", FCS->GetDrCmd());
trimmed = fgGetValue("/fdm/trim/trimmed",true); trimmed = fgGetValue("/fdm/trim/trimmed",true);
trimmed->setBoolValue(false); trimmed->setBoolValue(false);
@ -98,8 +110,8 @@ FGJSBsim::FGJSBsim( double dt )
/******************************************************************************/ /******************************************************************************/
FGJSBsim::~FGJSBsim(void) { FGJSBsim::~FGJSBsim(void) {
if(fdmex != NULL) { if(fdmex != NULL) {
delete fdmex; delete fdmex;
delete fgic; delete fgic;
} }
} }
@ -109,54 +121,53 @@ FGJSBsim::~FGJSBsim(void) {
// each subsequent iteration through the EOM // each subsequent iteration through the EOM
void FGJSBsim::init() { void FGJSBsim::init() {
// Explicitly call the superclass's // Explicitly call the superclass's
// init method first. // init method first.
FGInterface::init(); FGInterface::init();
bool result; bool result;
SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" ); SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" );
fdmex->GetAtmosphere()->UseInternal(); Atmosphere->UseInternal();
SG_LOG( SG_FLIGHT, SG_INFO, " Initializing JSBSim with:" ); SG_LOG( SG_FLIGHT, SG_INFO, " Initializing JSBSim with:" );
switch(fgic->GetSpeedSet()) { switch(fgic->GetSpeedSet()) {
case setned: case setned:
SG_LOG(SG_FLIGHT,SG_INFO, " Vn,Ve,Vd= " SG_LOG(SG_FLIGHT,SG_INFO, " Vn,Ve,Vd= "
<< fdmex->GetPosition()->GetVn() << Position->GetVn() << ", "
<< ", " << fdmex->GetPosition()->GetVe() << Position->GetVe() << ", "
<< ", " << fdmex->GetPosition()->GetVd() << Position->GetVd() << " ft/s");
<< " ft/s"); break;
break;
case setuvw: case setuvw:
SG_LOG(SG_FLIGHT,SG_INFO, " U,V,W= " SG_LOG(SG_FLIGHT,SG_INFO, " U,V,W= "
<< fdmex->GetTranslation()->GetUVW()(1) << Translation->GetUVW(1) << ", "
<< ", " << fdmex->GetTranslation()->GetUVW()(2) << Translation->GetUVW(2) << ", "
<< ", " << fdmex->GetTranslation()->GetUVW()(3) << Translation->GetUVW(3) << " ft/s");
<< " ft/s"); break;
break;
case setmach: case setmach:
SG_LOG(SG_FLIGHT,SG_INFO, " Mach: " SG_LOG(SG_FLIGHT,SG_INFO, " Mach: "
<< fdmex->GetTranslation()->GetMach() ); << Translation->GetMach() );
break; break;
case setvc: case setvc:
default: default:
SG_LOG(SG_FLIGHT,SG_INFO, " Indicated Airspeed: " SG_LOG(SG_FLIGHT,SG_INFO, " Indicated Airspeed: "
<< fdmex->GetAuxiliary()->GetVcalibratedKTS() << " knots" ); << Auxiliary->GetVcalibratedKTS() << " knots" );
break;
} }
SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: "
<< fdmex->GetRotation()->Getphi()*RADTODEG << " deg"); << Rotation->Getphi()*RADTODEG << " deg");
SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: "
<< fdmex->GetRotation()->Gettht()*RADTODEG << " deg" ); << Rotation->Gettht()*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: " SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: "
<< fdmex->GetRotation()->Getpsi()*RADTODEG << " deg" ); << Rotation->Getpsi()*RADTODEG << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: " SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: "
<< fdmex->GetPosition()->GetLatitude() << " deg" ); << Position->GetLatitude() << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: " SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: "
<< fdmex->GetPosition()->GetLongitude() << " deg" ); << Position->GetLongitude() << " deg" );
SG_LOG( SG_FLIGHT, SG_INFO, " loaded initial conditions" ); SG_LOG( SG_FLIGHT, SG_INFO, " loaded initial conditions" );
SG_LOG( SG_FLIGHT, SG_INFO, " set dt" ); SG_LOG( SG_FLIGHT, SG_INFO, " set dt" );
@ -169,61 +180,61 @@ void FGJSBsim::init() {
// Run an iteration of the EOM (equations of motion) // Run an iteration of the EOM (equations of motion)
bool FGJSBsim::update( int multiloop ) { bool FGJSBsim::update( int multiloop ) {
int i; int i;
double save_alt = 0.0; double save_alt = 0.0;
copy_to_JSBsim(); copy_to_JSBsim();
trimmed->setBoolValue(false); trimmed->setBoolValue(false);
if(needTrim && fgGetBool("/sim/startup/trim")) { 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));
controls.set_aileron(fdmex->GetFCS()->GetDaCmd()); //fgic->SetSeaLevelRadiusFtIC( get_Sea_level_radius() );
controls.set_rudder(fdmex->GetFCS()->GetDrCmd()); //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", 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; i<get_num_engines(); i++ ) { for( i=0; i<get_num_engines(); i++ ) {
get_engine(i)->set_RPM( controls.get_throttle(i)*2700 ); get_engine(i)->set_RPM( Propulsion->GetThruster(i)->GetRPM() );
get_engine(i)->set_Throttle( controls.get_throttle(i) ); get_engine(i)->set_Throttle( controls.get_throttle(i) );
} }
for ( i=0; i < multiloop; i++ ) {
for ( i = 0; i < multiloop; i++ ) { fdmex->Run();
fdmex->Run();
} }
// printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048); // 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 // autopilot (and the rest of the sim can use the updated values
copy_from_JSBsim(); 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; return true;
} }
@ -251,29 +254,33 @@ bool FGJSBsim::update( int multiloop ) {
bool FGJSBsim::copy_to_JSBsim() { bool FGJSBsim::copy_to_JSBsim() {
// copy control positions into the JSBsim structure // copy control positions into the JSBsim structure
fdmex->GetFCS()->SetDaCmd( controls.get_aileron()); FCS->SetDaCmd( controls.get_aileron());
fdmex->GetFCS()->SetDeCmd( controls.get_elevator()); FCS->SetDeCmd( controls.get_elevator());
fdmex->GetFCS()->SetPitchTrimCmd(controls.get_elevator_trim()); FCS->SetPitchTrimCmd(controls.get_elevator_trim());
fdmex->GetFCS()->SetDrCmd( -1*controls.get_rudder()); FCS->SetDrCmd( -controls.get_rudder());
fdmex->GetFCS()->SetDfCmd( controls.get_flaps() ); FCS->SetDfCmd( controls.get_flaps() );
fdmex->GetFCS()->SetDsbCmd( 0.0 ); //speedbrakes FCS->SetDsbCmd( 0.0 ); //speedbrakes
fdmex->GetFCS()->SetDspCmd( 0.0 ); //spoilers FCS->SetDspCmd( 0.0 ); //spoilers
fdmex->GetFCS()->SetThrottleCmd( FGControls::ALL_ENGINES, FCS->SetThrottleCmd( FGControls::ALL_ENGINES,
controls.get_throttle( 0 )); controls.get_throttle( 0 ));
fdmex->GetFCS()->SetLBrake( controls.get_brake( 0 ) ); FCS->SetLBrake( controls.get_brake( 0 ) );
fdmex->GetFCS()->SetRBrake( controls.get_brake( 1 ) ); FCS->SetRBrake( controls.get_brake( 1 ) );
fdmex->GetFCS()->SetCBrake( controls.get_brake( 2 ) ); FCS->SetCBrake( controls.get_brake( 2 ) );
fdmex->GetPosition()->SetSeaLevelRadius( get_Sea_level_radius() ); Position->SetSeaLevelRadius( get_Sea_level_radius() );
fdmex->GetPosition()->SetRunwayRadius( scenery.cur_elev*SG_METER_TO_FEET Position->SetRunwayRadius( scenery.cur_elev*SG_METER_TO_FEET
+ get_Sea_level_radius() ); + get_Sea_level_radius() );
fdmex->GetAtmosphere()->SetExTemperature(get_Static_temperature()); Atmosphere->SetExTemperature(get_Static_temperature());
fdmex->GetAtmosphere()->SetExPressure(get_Static_pressure()); Atmosphere->SetExPressure(get_Static_pressure());
fdmex->GetAtmosphere()->SetExDensity(get_Density()); Atmosphere->SetExDensity(get_Density());
fdmex->GetAtmosphere()->SetWindNED(get_V_north_airmass(), Atmosphere->SetWindNED(get_V_north_airmass(),
get_V_east_airmass(), get_V_east_airmass(),
get_V_down_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; return true;
} }
@ -283,114 +290,114 @@ bool FGJSBsim::copy_to_JSBsim() {
// Convert from the JSBsim generic_ struct to the FGInterface struct // Convert from the JSBsim generic_ struct to the FGInterface struct
bool FGJSBsim::copy_from_JSBsim() { bool FGJSBsim::copy_from_JSBsim() {
unsigned i, j; unsigned int i, j;
_set_Inertias( fdmex->GetAircraft()->GetMass(), _set_Inertias( Aircraft->GetMass(),
fdmex->GetAircraft()->GetIxx(), Aircraft->GetIxx(),
fdmex->GetAircraft()->GetIyy(), Aircraft->GetIyy(),
fdmex->GetAircraft()->GetIzz(), Aircraft->GetIzz(),
fdmex->GetAircraft()->GetIxz() ); Aircraft->GetIxz() );
_set_CG_Position( fdmex->GetAircraft()->GetXYZcg()(1), _set_CG_Position( Aircraft->GetXYZcg(1),
fdmex->GetAircraft()->GetXYZcg()(2), Aircraft->GetXYZcg(2),
fdmex->GetAircraft()->GetXYZcg()(3) ); Aircraft->GetXYZcg(3) );
_set_Accels_Body( fdmex->GetTranslation()->GetUVWdot()(1), _set_Accels_Body( Translation->GetUVWdot(1),
fdmex->GetTranslation()->GetUVWdot()(2), Translation->GetUVWdot(2),
fdmex->GetTranslation()->GetUVWdot()(3) ); Translation->GetUVWdot(3) );
_set_Accels_CG_Body( fdmex->GetTranslation()->GetUVWdot()(1), _set_Accels_CG_Body( Translation->GetUVWdot(1),
fdmex->GetTranslation()->GetUVWdot()(2), Translation->GetUVWdot(2),
fdmex->GetTranslation()->GetUVWdot()(3) ); Translation->GetUVWdot(3) );
//_set_Accels_CG_Body_N ( fdmex->GetTranslation()->GetNcg()(1), //_set_Accels_CG_Body_N ( Translation->GetNcg(1),
// fdmex->GetTranslation()->GetNcg()(2), // Translation->GetNcg(2),
// fdmex->GetTranslation()->GetNcg()(3) ); // Translation->GetNcg(3) );
// //
_set_Accels_Pilot_Body( fdmex->GetAuxiliary()->GetPilotAccel()(1), _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1),
fdmex->GetAuxiliary()->GetPilotAccel()(2), Auxiliary->GetPilotAccel(2),
fdmex->GetAuxiliary()->GetPilotAccel()(3) ); Auxiliary->GetPilotAccel(3) );
//_set_Accels_Pilot_Body_N( fdmex->GetAuxiliary()->GetNpilot()(1), //_set_Accels_Pilot_Body_N( Auxiliary->GetNpilot(1),
// fdmex->GetAuxiliary()->GetNpilot()(2), // Auxiliary->GetNpilot(2),
// fdmex->GetAuxiliary()->GetNpilot()(3) ); // Auxiliary->GetNpilot(3) );
_set_Nlf( fdmex->GetAircraft()->GetNlf() ); _set_Nlf( Aircraft->GetNlf() );
// Velocities // Velocities
_set_Velocities_Local( fdmex->GetPosition()->GetVn(), _set_Velocities_Local( Position->GetVn(),
fdmex->GetPosition()->GetVe(), Position->GetVe(),
fdmex->GetPosition()->GetVd() ); Position->GetVd() );
_set_Velocities_Wind_Body( fdmex->GetTranslation()->GetUVW()(1), _set_Velocities_Wind_Body( Translation->GetUVW(1),
fdmex->GetTranslation()->GetUVW()(2), Translation->GetUVW(2),
fdmex->GetTranslation()->GetUVW()(3) ); Translation->GetUVW(3) );
_set_V_rel_wind( fdmex->GetTranslation()->GetVt() );
_set_V_equiv_kts( fdmex->GetAuxiliary()->GetVequivalentKTS() );
// _set_V_calibrated( fdmex->GetAuxiliary()->GetVcalibratedFPS() ); _set_V_rel_wind( Translation->GetVt() );
_set_V_calibrated_kts( fdmex->GetAuxiliary()->GetVcalibratedKTS() ); _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() );
_set_V_ground_speed( fdmex->GetPosition()->GetVground() );
_set_Omega_Body( fdmex->GetRotation()->GetPQR()(1), // _set_V_calibrated( Auxiliary->GetVcalibratedFPS() );
fdmex->GetRotation()->GetPQR()(2),
fdmex->GetRotation()->GetPQR()(3) );
_set_Euler_Rates( fdmex->GetRotation()->GetEulerRates()(1), _set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() );
fdmex->GetRotation()->GetEulerRates()(2),
fdmex->GetRotation()->GetEulerRates()(3) );
_set_Geocentric_Rates(fdmex->GetPosition()->GetLatitudeDot(), _set_V_ground_speed( Position->GetVground() );
fdmex->GetPosition()->GetLongitudeDot(),
fdmex->GetPosition()->Gethdot() );
_set_Mach_number( fdmex->GetTranslation()->GetMach() ); _set_Omega_Body( Rotation->GetPQR(1),
Rotation->GetPQR(2),
Rotation->GetPQR(3) );
_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 // Positions
_updatePosition( fdmex->GetPosition()->GetLatitude(), _updatePosition( Position->GetLatitude(),
fdmex->GetPosition()->GetLongitude(), Position->GetLongitude(),
fdmex->GetPosition()->Geth() ); Position->Geth() );
_set_Euler_Angles( fdmex->GetRotation()->Getphi(),
fdmex->GetRotation()->Gettht(),
fdmex->GetRotation()->Getpsi() );
_set_Alpha( fdmex->GetTranslation()->Getalpha() ); _set_Euler_Angles( Rotation->Getphi(),
_set_Beta( fdmex->GetTranslation()->Getbeta() ); Rotation->Gettht(),
Rotation->Getpsi() );
_set_Alpha( Translation->Getalpha() );
_set_Gamma_vert_rad( fdmex->GetPosition()->GetGamma() ); _set_Beta( Translation->Getbeta() );
_set_Gamma_vert_rad( Position->GetGamma() );
// set_Gamma_horiz_rad( Gamma_horiz_rad ); // 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 ( i = 1; i <= 3; i++ ) {
for ( j = 1; j <= 3; j++ ) { for ( j = 1; j <= 3; j++ ) {
_set_T_Local_to_Body( i, j, fdmex->GetState()->GetTl2b()(i,j) ); _set_T_Local_to_Body( i, j, State->GetTl2b(i,j) );
} }
} }
return true; return true;
} }
void FGJSBsim::snap_shot(void) { void FGJSBsim::snap_shot(void) {
fgic->SetLatitudeRadIC(get_Lat_geocentric() ); fgic->SetLatitudeRadIC(get_Lat_geocentric() );
fgic->SetLongitudeRadIC( get_Longitude() ); fgic->SetLongitudeRadIC( get_Longitude() );
fgic->SetAltitudeFtIC( get_Altitude() ); fgic->SetAltitudeFtIC( get_Altitude() );
fgic->SetTerrainAltitudeFtIC( get_Runway_altitude() ); fgic->SetTerrainAltitudeFtIC( get_Runway_altitude() );
fgic->SetVtrueFpsIC( get_V_rel_wind() ); fgic->SetVtrueFpsIC( get_V_rel_wind() );
fgic->SetPitchAngleRadIC( get_Theta() ); fgic->SetPitchAngleRadIC( get_Theta() );
fgic->SetRollAngleRadIC( get_Phi() ); fgic->SetRollAngleRadIC( get_Phi() );
fgic->SetTrueHeadingRadIC( get_Psi() ); fgic->SetTrueHeadingRadIC( get_Psi() );
fgic->SetClimbRateFpsIC( get_Climb_Rate() ); fgic->SetClimbRateFpsIC( get_Climb_Rate() );
} }
bool FGJSBsim::ToggleDataLogging(void) { bool FGJSBsim::ToggleDataLogging(void) {
@ -412,9 +419,9 @@ bool FGJSBsim::ToggleDataLogging(bool state) {
//Positions //Positions
void FGJSBsim::set_Latitude(double lat) { void FGJSBsim::set_Latitude(double lat) {
double sea_level_radius_meters,lat_geoc; 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(); snap_shot();
sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc); sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc);
_set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); _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 fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Longitude(double lon) { void FGJSBsim::set_Longitude(double lon) {
SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon ); SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon );
snap_shot(); snap_shot();
fgic->SetLongitudeRadIC(lon); fgic->SetLongitudeRadIC(lon);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Altitude(double alt) { void FGJSBsim::set_Altitude(double alt) {
double sea_level_radius_meters,lat_geoc; 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(); snap_shot();
sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc); sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc);
_set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); _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 copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_V_calibrated_kts(double vc) { void FGJSBsim::set_V_calibrated_kts(double vc) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_V_calibrated_kts: " << vc ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_V_calibrated_kts: " << vc );
snap_shot(); snap_shot();
fgic->SetVcalibratedKtsIC(vc); fgic->SetVcalibratedKtsIC(vc);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Mach_number(double mach) { void FGJSBsim::set_Mach_number(double mach) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach );
snap_shot(); snap_shot();
fgic->SetMachIC(mach); fgic->SetMachIC(mach);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Velocities_Local( double north, double east, double down ){ void FGJSBsim::set_Velocities_Local( double north, double east, double down ){
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: "
<< north << ", " << east << ", " << down ); << north << ", " << east << ", " << down );
snap_shot(); snap_shot();
fgic->SetVnorthFpsIC(north); fgic->SetVnorthFpsIC(north);
fgic->SetVeastFpsIC(east); fgic->SetVeastFpsIC(east);
@ -483,12 +490,12 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double down ){
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){ void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: " SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Wind_Body: "
<< u << ", " << v << ", " << w ); << u << ", " << v << ", " << w );
snap_shot(); snap_shot();
fgic->SetUBodyFpsIC(u); fgic->SetUBodyFpsIC(u);
fgic->SetVBodyFpsIC(v); 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 fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
//Euler angles //Euler angles
void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: " SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: "
<< phi << ", " << theta << ", " << psi ); << phi << ", " << theta << ", " << psi );
snap_shot(); snap_shot();
fgic->SetPitchAngleRadIC(theta); fgic->SetPitchAngleRadIC(theta);
fgic->SetRollAngleRadIC(phi); fgic->SetRollAngleRadIC(phi);
fgic->SetTrueHeadingRadIC(psi); fgic->SetTrueHeadingRadIC(psi);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
//Flight Path //Flight Path
void FGJSBsim::set_Climb_Rate( double roc) { void FGJSBsim::set_Climb_Rate( double roc) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc );
snap_shot(); snap_shot();
fgic->SetClimbRateFpsIC(roc); fgic->SetClimbRateFpsIC(roc);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Gamma_vert_rad( double gamma) { void FGJSBsim::set_Gamma_vert_rad( double gamma) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma );
snap_shot(); snap_shot();
fgic->SetFlightPathAngleRadIC(gamma); fgic->SetFlightPathAngleRadIC(gamma);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
//Earth //Earth
void FGJSBsim::set_Sea_level_radius(double slr) { void FGJSBsim::set_Sea_level_radius(double slr) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Sea_level_radius: " << slr ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Sea_level_radius: " << slr );
snap_shot(); snap_shot();
fgic->SetSeaLevelRadiusFtIC(slr); fgic->SetSeaLevelRadiusFtIC(slr);
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Runway_altitude(double ralt) { void FGJSBsim::set_Runway_altitude(double ralt) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt );
snap_shot(); snap_shot();
_set_Runway_altitude( ralt ); _set_Runway_altitude( ralt );
fgic->SetTerrainAltitudeFtIC( ralt ); fgic->SetTerrainAltitudeFtIC( ralt );
fdmex->RunIC(fgic); //loop JSBSim once fdmex->RunIC(fgic); //loop JSBSim once
copy_from_JSBsim(); //update the bus copy_from_JSBsim(); //update the bus
needTrim=true; needTrim=true;
}
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;
} }
void FGJSBsim::set_Static_temperature(double T) { void FGJSBsim::set_Static_pressure(double p) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_pressure: " << p );
snap_shot();
Atmosphere->SetExPressure(p);
if(Atmosphere->External() == true)
needTrim=true;
}
void FGJSBsim::set_Static_temperature(double T) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_temperature: " << T ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_temperature: " << T );
snap_shot(); snap_shot();
fdmex->GetAtmosphere()->SetExTemperature(T); Atmosphere->SetExTemperature(T);
if(fdmex->GetAtmosphere()->External() == true) if(Atmosphere->External() == true)
needTrim=true; needTrim=true;
} }
@ -578,21 +585,22 @@ void FGJSBsim::set_Density(double rho) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho ); SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho );
snap_shot(); snap_shot();
fdmex->GetAtmosphere()->SetExDensity(rho); Atmosphere->SetExDensity(rho);
if(fdmex->GetAtmosphere()->External() == true) if(Atmosphere->External() == true)
needTrim=true; needTrim=true;
} }
void FGJSBsim::set_Velocities_Local_Airmass (double wnorth, void FGJSBsim::set_Velocities_Local_Airmass (double wnorth,
double weast, double weast,
double wdown ) { double wdown ) {
SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local_Airmass: " 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(); snap_shot();
fdmex->GetAtmosphere()->SetWindNED(wnorth, weast, wdown ); Atmosphere->SetWindNED(wnorth, weast, wdown );
if(fdmex->GetAtmosphere()->External() == true) if(Atmosphere->External() == true)
needTrim=true; needTrim=true;
} }

10
src/FDM/JSBSim.hxx
View file

@ -202,6 +202,16 @@ private:
FGInitialCondition *fgic; FGInitialCondition *fgic;
bool needTrim; bool needTrim;
FGState* State;
FGAtmosphere* Atmosphere;
FGFCS* FCS;
FGPropulsion* Propulsion;
FGAircraft* Aircraft;
FGTranslation* Translation;
FGRotation* Rotation;
FGPosition* Position;
FGAuxiliary* Auxiliary;
int runcount; int runcount;
float trim_elev; float trim_elev;
float trim_throttle; float trim_throttle;

25
src/FDM/JSBSim/FGAircraft.cpp
View file

@ -162,12 +162,15 @@ FGAircraft::FGAircraft(FGFDMExec* fdmex) : FGModel(fdmex),
vMoments(3), vMoments(3),
vForces(3), vForces(3),
vFs(3), vFs(3),
vLastFs(3),
vXYZrp(3), vXYZrp(3),
vbaseXYZcg(3), vbaseXYZcg(3),
vXYZcg(3), vXYZcg(3),
vXYZep(3), vXYZep(3),
vEuler(3) vEuler(3),
vXYZtank(3),
vDXYZcg(3),
vAeroBodyForces(3)
{ {
Name = "FGAircraft"; Name = "FGAircraft";
@ -262,8 +265,8 @@ bool FGAircraft::Run(void) {
MassChange(); MassChange();
FMProp(); FMProp();
FMAero(); FMAero();
FMGear();
FMMass(); FMMass();
FMGear();
nlf = 0; nlf = 0;
if (fabs(Position->GetGamma()) < 1.57) { if (fabs(Position->GetGamma()) < 1.57) {
@ -280,7 +283,6 @@ bool FGAircraft::Run(void) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGAircraft::MassChange() { void FGAircraft::MassChange() {
static FGColumnVector vXYZtank(3);
float Tw; float Tw;
float IXXt, IYYt, IZZt, IXZt; float IXXt, IYYt, IZZt, IXZt;
// unsigned int t; // unsigned int t;
@ -368,16 +370,14 @@ void FGAircraft::MassChange() {
Iyy = baseIyy + IYYt; Iyy = baseIyy + IYYt;
Izz = baseIzz + IZZt; Izz = baseIzz + IZZt;
Ixz = baseIxz + IXZt; Ixz = baseIxz + IXZt;
} }
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGAircraft::FMAero(void) { void FGAircraft::FMAero(void) {
static FGColumnVector vDXYZcg(3);
static FGColumnVector vAeroBodyForces(3);
unsigned int axis_ctr,ctr; unsigned int axis_ctr,ctr;
vLastFs = vFs;
for (axis_ctr=1; axis_ctr<=3; axis_ctr++) vFs(axis_ctr) = 0.0; for (axis_ctr=1; axis_ctr<=3; axis_ctr++) vFs(axis_ctr) = 0.0;
for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) { for (axis_ctr = 0; axis_ctr < 3; axis_ctr++) {
@ -749,3 +749,14 @@ void FGAircraft::Debug(void)
//TODO: Add your source code here //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;
}

16
src/FDM/JSBSim/FGAircraft.h
View file

@ -58,9 +58,9 @@ INCLUDES
#include "FGModel.h" #include "FGModel.h"
#include "FGCoefficient.h" #include "FGCoefficient.h"
#include "FGPropulsion.h" #include "FGPropulsion.h"
#include "FGLGear.h"
#include "FGConfigFile.h" #include "FGConfigFile.h"
#include "FGMatrix.h" #include "FGMatrix.h"
#include "FGLGear.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS DEFINITIONS
@ -161,7 +161,12 @@ public:
inline float GetMass(void) { return Mass; } inline float GetMass(void) { return Mass; }
inline FGColumnVector GetMoments(void) { return vMoments; } inline FGColumnVector GetMoments(void) { return vMoments; }
inline FGColumnVector GetForces(void) { return vForces; } 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 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 GetIxx(void) { return Ixx; }
inline float GetIyy(void) { return Iyy; } inline float GetIyy(void) { return Iyy; }
inline float GetIzz(void) { return Izz; } inline float GetIzz(void) { return Izz; }
@ -169,6 +174,9 @@ public:
inline FGColumnVector GetXYZcg(void) { return vXYZcg; } inline FGColumnVector GetXYZcg(void) { return vXYZcg; }
inline FGColumnVector GetXYZrp(void) { return vXYZrp; } inline FGColumnVector GetXYZrp(void) { return vXYZrp; }
inline FGColumnVector GetXYZep(void) { return vXYZep; } 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 GetNlf(void) { return nlf; }
inline float GetAlphaCLMax(void) { return alphaclmax; } inline float GetAlphaCLMax(void) { return alphaclmax; }
inline float GetAlphaCLMin(void) { return alphaclmin; } inline float GetAlphaCLMin(void) { return alphaclmin; }
@ -183,6 +191,8 @@ public:
string GetGroundReactionStrings(void); string GetGroundReactionStrings(void);
string GetGroundReactionValues(void); string GetGroundReactionValues(void);
float GetLoD(void);
/// Subsystem types for specifying which will be output in the FDM data logging /// Subsystem types for specifying which will be output in the FDM data logging
enum SubSystems { enum SubSystems {
/** Subsystem: Simulation (= 1) */ ssSimulation = 1, /** Subsystem: Simulation (= 1) */ ssSimulation = 1,
@ -210,11 +220,15 @@ private:
FGColumnVector vMoments; FGColumnVector vMoments;
FGColumnVector vForces; FGColumnVector vForces;
FGColumnVector vFs; FGColumnVector vFs;
FGColumnVector vLastFs;
FGColumnVector vXYZrp; FGColumnVector vXYZrp;
FGColumnVector vbaseXYZcg; FGColumnVector vbaseXYZcg;
FGColumnVector vXYZcg; FGColumnVector vXYZcg;
FGColumnVector vXYZep; FGColumnVector vXYZep;
FGColumnVector vEuler; FGColumnVector vEuler;
FGColumnVector vXYZtank;
FGColumnVector vDXYZcg;
FGColumnVector vAeroBodyForces;
float baseIxx, baseIyy, baseIzz, baseIxz, EmptyMass, Mass; float baseIxx, baseIyy, baseIzz, baseIxz, EmptyMass, Mass;
float Ixx, Iyy, Izz, Ixz; float Ixx, Iyy, Izz, Ixz;
float alpha, beta; float alpha, beta;

5
src/FDM/JSBSim/FGAtmosphere.cpp
View file

@ -70,7 +70,7 @@ CLASS IMPLEMENTATION
FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex), FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex),
vWindNED(3) vWindNED(3)
{ {
Name = "FGAtmosphere"; Name = "FGAtmosphere";
h = 0; h = 0;
@ -80,7 +80,6 @@ FGAtmosphere::FGAtmosphere(FGFDMExec* fdmex) : FGModel(fdmex),
SLdensity = density; SLdensity = density;
SLsoundspeed = sqrt(SHRATIO*Reng*temperature); SLsoundspeed = sqrt(SHRATIO*Reng*temperature);
useExternal=false; useExternal=false;
vWindNED(1)=0;vWindNED(2)=0;vWindNED(3)=0;
if (debug_lvl & 2) cout << "Instantiated: " << Name << endl; if (debug_lvl & 2) cout << "Instantiated: " << Name << endl;
} }
@ -110,7 +109,7 @@ bool FGAtmosphere::Run(void)
temperature = exTemperature; 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; if (psiw < 0) psiw += 2*M_PI;

4
src/FDM/JSBSim/FGAuxiliary.h
View file

@ -89,8 +89,10 @@ public:
inline float GetVequivalentKTS(void) { return veas*FPSTOKTS; } inline float GetVequivalentKTS(void) { return veas*FPSTOKTS; }
inline FGColumnVector GetPilotAccel(void) { return vPilotAccel; } inline FGColumnVector GetPilotAccel(void) { return vPilotAccel; }
inline float GetPilotAccel(int idx) { return vPilotAccel(idx); }
inline FGColumnVector GetNpilot(void) { return vPilotAccel*INVGRAVITY; } inline FGColumnVector GetNpilot(void) { return vPilotAccel*INVGRAVITY; }
inline float GetNpilot(int idx) { return (vPilotAccel*INVGRAVITY)(idx); }
inline float GetEarthPositionAngle(void) { return earthPosAngle; } inline float GetEarthPositionAngle(void) { return earthPosAngle; }
float GetHeadWind(void); float GetHeadWind(void);

10
src/FDM/JSBSim/FGCoefficient.cpp
View file

@ -48,11 +48,11 @@ INCLUDES
#include "FGState.h" #include "FGState.h"
#include "FGFDMExec.h" #include "FGFDMExec.h"
#ifndef FGFS #ifndef FGFS
# include <iomanip.h> # include <iomanip>
#else #else
# include STL_IOMANIP # include STL_IOMANIP
#endif #endif
static const char *IdSrc = "$Id$"; static const char *IdSrc = "$Id$";
static const char *IdHdr = "ID_COEFFICIENT"; static const char *IdHdr = "ID_COEFFICIENT";

2
src/FDM/JSBSim/FGConfigFile.cpp
View file

@ -32,7 +32,7 @@ CLASS IMPLEMENTATION
FGConfigFile::FGConfigFile(string cfgFileName) FGConfigFile::FGConfigFile(string cfgFileName)
{ {
cfgfile.open(cfgFileName.c_str()); cfgfile.open(cfgFileName.c_str(), ios::in | ios::binary );
CommentsOn = false; CommentsOn = false;
CurrentIndex = 0; CurrentIndex = 0;
Opened = true; Opened = true;

4
src/FDM/JSBSim/FGDefs.h
View file

@ -71,9 +71,13 @@ enum eParam {
FG_ALPHADOT, FG_ALPHADOT,
FG_BETA, FG_BETA,
FG_BETADOT, FG_BETADOT,
FG_PHI,
FG_THT,
FG_PSI,
FG_PITCHRATE, FG_PITCHRATE,
FG_ROLLRATE, FG_ROLLRATE,
FG_YAWRATE, FG_YAWRATE,
FG_CL_SQRD,
FG_MACH, FG_MACH,
FG_ALTITUDE, FG_ALTITUDE,
FG_BI2VEL, FG_BI2VEL,

41
src/FDM/JSBSim/FGFCS.cpp
View file

@ -65,7 +65,8 @@ extern short debug_lvl;
CLASS IMPLEMENTATION CLASS IMPLEMENTATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex) { FGFCS::FGFCS(FGFDMExec* fdmex) : FGModel(fdmex)
{
Name = "FGFCS"; Name = "FGFCS";
DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = PTrimCmd = 0.0; DaCmd = DeCmd = DrCmd = DfCmd = DsbCmd = DspCmd = PTrimCmd = 0.0;
@ -82,7 +83,9 @@ FGFCS::~FGFCS()
ThrottleCmd.clear(); ThrottleCmd.clear();
ThrottlePos.clear(); ThrottlePos.clear();
for(unsigned int i=0;i<Components.size();i++) delete Components[i]; unsigned int i;
for(i=0;i<Components.size();i++) delete Components[i];
if (debug_lvl & 2) cout << "Destroyed: FGFCS" << endl; if (debug_lvl & 2) cout << "Destroyed: FGFCS" << endl;
} }
@ -90,9 +93,11 @@ FGFCS::~FGFCS()
bool FGFCS::Run(void) bool FGFCS::Run(void)
{ {
unsigned int i;
if (!FGModel::Run()) { if (!FGModel::Run()) {
for (unsigned int i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i]; for (i=0; i<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
for (unsigned int i=0; i<Components.size(); i++) Components[i]->Run(); for (i=0; i<Components.size(); i++) Components[i]->Run();
} else { } 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) { if (engineNum < 0) {
for (unsigned int ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting; for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
} else { } else {
ThrottleCmd[engineNum] = setting; ThrottleCmd[engineNum] = setting;
} }
@ -111,9 +119,12 @@ void FGFCS::SetThrottleCmd(int engineNum, float setting) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGFCS::SetThrottlePos(int engineNum, float setting) { void FGFCS::SetThrottlePos(int engineNum, float setting)
{
unsigned int ctr;
if (engineNum < 0) { if (engineNum < 0) {
for (unsigned int ctr=0;ctr<=ThrottleCmd.size();ctr++) ThrottlePos[ctr] = ThrottleCmd[ctr]; for (ctr=0;ctr<=ThrottleCmd.size();ctr++) ThrottlePos[ctr] = ThrottleCmd[ctr];
} else { } else {
ThrottlePos[engineNum] = setting; ThrottlePos[engineNum] = setting;
} }
@ -121,7 +132,8 @@ void FGFCS::SetThrottlePos(int engineNum, float setting) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGFCS::LoadFCS(FGConfigFile* AC_cfg) { bool FGFCS::LoadFCS(FGConfigFile* AC_cfg)
{
string token; string token;
FCSName = AC_cfg->GetValue("NAME"); FCSName = AC_cfg->GetValue("NAME");
@ -193,11 +205,14 @@ float FGFCS::GetBrake(FGLGear::BrakeGroup bg) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
string FGFCS::GetComponentStrings(void){ string FGFCS::GetComponentStrings(void)
{
unsigned int comp;
string CompStrings = ""; string CompStrings = "";
bool firstime = true; bool firstime = true;
for (unsigned int comp = 0; comp < Components.size(); comp++) { for (comp = 0; comp < Components.size(); comp++) {
if (firstime) firstime = false; if (firstime) firstime = false;
else CompStrings += ", "; else CompStrings += ", ";
@ -211,11 +226,13 @@ string FGFCS::GetComponentStrings(void){
string FGFCS::GetComponentValues(void) string FGFCS::GetComponentValues(void)
{ {
unsigned int comp;
string CompValues = ""; string CompValues = "";
char buffer[10]; char buffer[10];
bool firstime = true; bool firstime = true;
for (unsigned int comp = 0; comp < Components.size(); comp++) { for (comp = 0; comp < Components.size(); comp++) {
if (firstime) firstime = false; if (firstime) firstime = false;
else CompValues += ", "; else CompValues += ", ";

4
src/FDM/JSBSim/FGFCS.h
View file

@ -289,7 +289,7 @@ public:
/** Sets the throttle command for the specified engine /** Sets the throttle command for the specified engine
@param engine engine ID number @param engine engine ID number
@param cmd throttle command in percent (0 - 100)*/ @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 /// @name Aerosurface position setting
@ -321,7 +321,7 @@ public:
/** Sets the actual throttle setting for the specified engine /** Sets the actual throttle setting for the specified engine
@param engine engine ID number @param engine engine ID number
@param cmd throttle setting in percent (0 - 100)*/ @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 /// @name Landing Gear brakes

4
src/FDM/JSBSim/FGFDMExec.cpp
View file

@ -177,7 +177,9 @@ bool FGFDMExec::Allocate(void) {
Auxiliary = new FGAuxiliary(this); Auxiliary = new FGAuxiliary(this);
Output = new FGOutput(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 // Initialize models so they can communicate with each other

6
src/FDM/JSBSim/FGForce.cpp
View file

@ -88,9 +88,9 @@ FGColumnVector FGForce::GetBodyForces(void) {
//find the distance from this vector's location to the cg //find the distance from this vector's location to the cg
//needs to be done like this to convert from structural to body coords //needs to be done like this to convert from structural to body coords
vDXYZ(1) = -(vXYZn(1) - fdmex->GetAircraft()->GetXYZcg()(1))*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(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(3) = -(vXYZn(3) - fdmex->GetAircraft()->GetXYZcg(3))*INCHTOFT;
vM=vMn +vDXYZ*vFb; vM=vMn +vDXYZ*vFb;

28
src/FDM/JSBSim/FGLGear.cpp
View file

@ -327,9 +327,14 @@ FGColumnVector FGLGear::Force(void)
FCoeff = dynamicFCoeff*fabs(WheelSlip)/WheelSlip; 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))); MaximumStrutForce = max(MaximumStrutForce, fabs(vLocalForce(eZ)));
MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength)); MaximumStrutTravel = max(MaximumStrutTravel, fabs(compressLength));
@ -347,14 +352,17 @@ FGColumnVector FGLGear::Force(void)
vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel; vLocalForce(eX) = RollingForce*CosWheel - SideForce*SinWheel;
vLocalForce(eY) = SideForce*CosWheel + RollingForce*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 // Note to Jon: At this point the forces will be too big when the airplane is
// rolling to a stop. We need to make sure that the gear forces just balance out the non-gear forces // stopped or rolling to a stop. We need to make sure that the gear forces just
// when the airplane is stopped. That way the airplane won't start to accelerate until the non-gear // balance out the non-gear forces when the airplane is stopped. That way the
// forces are larger than the gear forces. I think that the proper fix should go into FGAircraft::FMGear. // airplane won't start to accelerate until the non-gear/ forces are larger than
// This routine would only compute the local strut forces and return them to FMGear. All of the gear // the gear forces. I think that the proper fix should go into FGAircraft::FMGear.
// forces would get adjusted in FMGear using the total non-gear forces. Then the gear moments would be // This routine would only compute the local strut forces and return them to
// calculated. If strange things start happening to the airplane during testing as it rolls to a stop, // FMGear. All of the gear forces would get adjusted in FMGear using the total
// then we need to implement this change. I ran out of time to do it now but have the equations. // 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. // Transform the forces back to the body frame and compute the moment.

11
src/FDM/JSBSim/FGLGear.h
View file

@ -46,7 +46,6 @@ INCLUDES
#include "FGConfigFile.h" #include "FGConfigFile.h"
#include "FGMatrix.h" #include "FGMatrix.h"
#include "FGFDMExec.h" #include "FGFDMExec.h"
#include "FGState.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINITIONS DEFINITIONS
@ -62,6 +61,7 @@ class FGAircraft;
class FGPosition; class FGPosition;
class FGRotation; class FGRotation;
class FGFCS; class FGFCS;
class FGState;
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs]
@ -163,6 +163,8 @@ CLASS DOCUMENTATION
NASA-Ames", NASA CR-2497, January 1975 NASA-Ames", NASA CR-2497, January 1975
@see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics", @see Barnes W. McCormick, "Aerodynamics, Aeronautics, and Flight Mechanics",
Wiley & Sons, 1979 ISBN 0-471-03032-5 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 /// Gets the location of the gear in Body axes
FGColumnVector GetBodyLocation(void) { return vWhlBodyVec; } FGColumnVector GetBodyLocation(void) { return vWhlBodyVec; }
float GetBodyLocation(int idx) { return vWhlBodyVec(idx); }
FGColumnVector GetLocalGear(void) { return vLocalGear; } FGColumnVector GetLocalGear(void) { return vLocalGear; }
float GetLocalGear(int idx) { return vLocalGear(idx); }
/// Gets the name of the gear /// Gets the name of the gear
inline string GetName(void) {return name; } inline string GetName(void) {return name; }
@ -264,4 +268,7 @@ private:
#include "FGFCS.h" #include "FGFCS.h"
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
#include "FGState.h"
#endif #endif

8
src/FDM/JSBSim/FGMatrix.cpp
View file

@ -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) { if (Rows() != 3 || V.Rows() != 3) {
MatrixException mE; 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) { if (Rows() != 3 || V.Rows() != 3) {
MatrixException mE; MatrixException mE;

4
src/FDM/JSBSim/FGMatrix.h
View file

@ -142,7 +142,7 @@ public:
~FGColumnVector(void); ~FGColumnVector(void);
FGColumnVector operator*(const double scalar); 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 double scalar);
FGColumnVector operator+(const FGColumnVector& B); // must not return reference FGColumnVector operator+(const FGColumnVector& B); // must not return reference
FGColumnVector operator-(const FGColumnVector& B); FGColumnVector operator-(const FGColumnVector& B);
@ -154,7 +154,7 @@ public:
double& operator()(int m) const; double& operator()(int m) const;
FGColumnVector& multElementWise(const FGColumnVector& V); FGColumnVector multElementWise(const FGColumnVector& V);
private: private:
void Debug(void); void Debug(void);

1
src/FDM/JSBSim/FGNozzle.cpp
View file

@ -74,6 +74,7 @@ FGNozzle::FGNozzle(FGFDMExec* FDMExec, FGConfigFile* Nzl_cfg) : FGThruster(FDMEx
} }
Thrust = 0; Thrust = 0;
Type = ttNozzle;
if (debug_lvl & 2) cout << "Instantiated: FGNozzle" << endl; if (debug_lvl & 2) cout << "Instantiated: FGNozzle" << endl;
} }

8
src/FDM/JSBSim/FGOutput.cpp
View file

@ -161,7 +161,8 @@ void FGOutput::DelimitedOutput(void)
} }
if (SubSystems & FGAircraft::ssForces) { if (SubSystems & FGAircraft::ssForces) {
cout << ", "; cout << ", ";
cout << "XsForce, YsForce, ZsForce, "; cout << "Drag, Side, Lift, ";
cout << "L/D, ";
cout << "Xforce, Yforce, Zforce"; cout << "Xforce, Yforce, Zforce";
} }
if (SubSystems & FGAircraft::ssMoments) { if (SubSystems & FGAircraft::ssMoments) {
@ -235,6 +236,7 @@ void FGOutput::DelimitedOutput(void)
if (SubSystems & FGAircraft::ssForces) { if (SubSystems & FGAircraft::ssForces) {
cout << ", "; cout << ", ";
cout << Aircraft->GetvFs() << ", "; cout << Aircraft->GetvFs() << ", ";
cout << Aircraft->GetLoD() << ", ";
cout << Aircraft->GetForces(); cout << Aircraft->GetForces();
} }
if (SubSystems & FGAircraft::ssMoments) { if (SubSystems & FGAircraft::ssMoments) {
@ -313,7 +315,8 @@ void FGOutput::DelimitedOutput(string fname)
} }
if (SubSystems & FGAircraft::ssForces) { if (SubSystems & FGAircraft::ssForces) {
datafile << ", "; datafile << ", ";
datafile << "XsForce, YsForce, ZsForce, "; datafile << "Drag, Side, Lift, ";
datafile << "L/D, ";
datafile << "Xforce, Yforce, Zforce"; datafile << "Xforce, Yforce, Zforce";
} }
if (SubSystems & FGAircraft::ssMoments) { if (SubSystems & FGAircraft::ssMoments) {
@ -390,6 +393,7 @@ void FGOutput::DelimitedOutput(string fname)
if (SubSystems & FGAircraft::ssForces) { if (SubSystems & FGAircraft::ssForces) {
datafile << ", "; datafile << ", ";
datafile << Aircraft->GetvFs() << ", "; datafile << Aircraft->GetvFs() << ", ";
datafile << Aircraft->GetLoD() << ", ";
datafile << Aircraft->GetForces(); datafile << Aircraft->GetForces();
} }
if (SubSystems & FGAircraft::ssMoments) { if (SubSystems & FGAircraft::ssMoments) {

1
src/FDM/JSBSim/FGPiston.cpp
View file

@ -84,6 +84,7 @@ FGPiston::FGPiston(FGFDMExec* exec, FGConfigFile* Eng_cfg) : FGEngine(exec)
} }
} }
Type = etPiston;
EngineNumber = 0; EngineNumber = 0;
if (debug_lvl & 2) cout << "Instantiated: FGPiston" << endl; if (debug_lvl & 2) cout << "Instantiated: FGPiston" << endl;

5
src/FDM/JSBSim/FGPropeller.cpp
View file

@ -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; if (debug_lvl & 2) cout << "Instantiated: FGPropeller" << endl;
} }
@ -146,7 +149,7 @@ float FGPropeller::Calculate(float PowerAvailable)
vFn(1) = Thrust; vFn(1) = Thrust;
omega = RPS*2.0*M_PI; omega = RPS*2.0*M_PI;
if (omega <= 500) omega = 1.0; if (omega <= 5) omega = 1.0;
Torque = PowerAvailable / omega; Torque = PowerAvailable / omega;
RPM = (RPS + ((Torque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0; RPM = (RPS + ((Torque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0;

4
src/FDM/JSBSim/FGRotation.cpp
View file

@ -81,7 +81,8 @@ FGRotation::FGRotation(FGFDMExec* fdmex) : FGModel(fdmex),
vPQRdot(3), vPQRdot(3),
vMoments(3), vMoments(3),
vEuler(3), vEuler(3),
vEulerRates(3) vEulerRates(3),
vlastPQRdot(3)
{ {
Name = "FGRotation"; Name = "FGRotation";
cTht=cPhi=cPsi=1.0; cTht=cPhi=cPsi=1.0;
@ -103,7 +104,6 @@ bool FGRotation::Run(void)
{ {
float L2, N1; float L2, N1;
float tTheta; float tTheta;
static FGColumnVector vlastPQRdot(3);
if (!FGModel::Run()) { if (!FGModel::Run()) {
GetState(); GetState();

5
src/FDM/JSBSim/FGRotation.h
View file

@ -90,9 +90,13 @@ public:
bool Run(void); bool Run(void);
inline FGColumnVector GetPQR(void) {return vPQR;} inline FGColumnVector GetPQR(void) {return vPQR;}
inline float GetPQR(int axis) {return vPQR(axis);}
inline FGColumnVector GetPQRdot(void) {return vPQRdot;} inline FGColumnVector GetPQRdot(void) {return vPQRdot;}
inline float GetPQRdot(int idx) {return vPQRdot(idx);}
inline FGColumnVector GetEuler(void) {return vEuler;} inline FGColumnVector GetEuler(void) {return vEuler;}
inline float GetEuler(int axis) {return vEuler(axis);}
inline FGColumnVector GetEulerRates(void) { return vEulerRates; } inline FGColumnVector GetEulerRates(void) { return vEulerRates; }
inline float GetEulerRates(int axis) { return vEulerRates(axis); }
inline void SetPQR(FGColumnVector tt) {vPQR = tt;} inline void SetPQR(FGColumnVector tt) {vPQR = tt;}
inline void SetEuler(FGColumnVector tt) {vEuler = tt;} inline void SetEuler(FGColumnVector tt) {vEuler = tt;}
@ -114,6 +118,7 @@ private:
FGColumnVector vMoments; FGColumnVector vMoments;
FGColumnVector vEuler; FGColumnVector vEuler;
FGColumnVector vEulerRates; FGColumnVector vEulerRates;
FGColumnVector vlastPQRdot;
float cTht,sTht; float cTht,sTht;
float cPhi,sPhi; float cPhi,sPhi;

213
src/FDM/JSBSim/FGState.cpp
View file

@ -53,15 +53,6 @@ INCLUDES
#endif #endif
#include "FGState.h" #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 *IdSrc = "$Id$";
static const char *IdHdr = ID_STATE; static const char *IdHdr = ID_STATE;
@ -89,7 +80,10 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
mTl2b(3,3), mTl2b(3,3),
mTs2b(3,3), mTs2b(3,3),
vQtrn(4), vQtrn(4),
vlastQdot(4) vlastQdot(4),
vQdot(4),
vTmp(4),
vEuler(3)
{ {
FDMExec = fdex; FDMExec = fdex;
@ -97,7 +91,15 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
sim_time = 0.0; sim_time = 0.0;
dt = 1.0/120.0; dt = 1.0/120.0;
ActiveEngine = -1; 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_TIME, " time " );
RegisterVariable(FG_QBAR, " qbar " ); RegisterVariable(FG_QBAR, " qbar " );
RegisterVariable(FG_WINGAREA, " wing_area " ); RegisterVariable(FG_WINGAREA, " wing_area " );
@ -107,9 +109,13 @@ FGState::FGState(FGFDMExec* fdex) : mTb2l(3,3),
RegisterVariable(FG_ALPHADOT, " alphadot " ); RegisterVariable(FG_ALPHADOT, " alphadot " );
RegisterVariable(FG_BETA, " beta " ); RegisterVariable(FG_BETA, " beta " );
RegisterVariable(FG_BETADOT, " betadot " ); 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_PITCHRATE, " pitch_rate " );
RegisterVariable(FG_ROLLRATE, " roll_rate " ); RegisterVariable(FG_ROLLRATE, " roll_rate " );
RegisterVariable(FG_YAWRATE, " yaw_rate " ); RegisterVariable(FG_YAWRATE, " yaw_rate " );
RegisterVariable(FG_CL_SQRD, " Clift_sqrd " );
RegisterVariable(FG_MACH, " mach " ); RegisterVariable(FG_MACH, " mach " );
RegisterVariable(FG_ALTITUDE, " altitude " ); RegisterVariable(FG_ALTITUDE, " altitude " );
RegisterVariable(FG_BI2VEL, " BI2Vel " ); RegisterVariable(FG_BI2VEL, " BI2Vel " );
@ -149,77 +155,93 @@ FGState::~FGState()
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
float FGState::GetParameter(eParam val_idx) { float FGState::GetParameter(eParam val_idx) {
float scratch;
switch(val_idx) { switch(val_idx) {
case FG_TIME: case FG_TIME:
return sim_time; return sim_time;
case FG_QBAR: case FG_QBAR:
return FDMExec->GetTranslation()->Getqbar(); return Translation->Getqbar();
case FG_WINGAREA: case FG_WINGAREA:
return FDMExec->GetAircraft()->GetWingArea(); return Aircraft->GetWingArea();
case FG_WINGSPAN: case FG_WINGSPAN:
return FDMExec->GetAircraft()->GetWingSpan(); return Aircraft->GetWingSpan();
case FG_CBAR: case FG_CBAR:
return FDMExec->GetAircraft()->Getcbar(); return Aircraft->Getcbar();
case FG_ALPHA: case FG_ALPHA:
return FDMExec->GetTranslation()->Getalpha(); return Translation->Getalpha();
case FG_ALPHADOT: case FG_ALPHADOT:
return FDMExec->GetTranslation()->Getadot(); return Translation->Getadot();
case FG_BETA: case FG_BETA:
return FDMExec->GetTranslation()->Getbeta(); return Translation->Getbeta();
case FG_BETADOT: 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: case FG_PITCHRATE:
return (FDMExec->GetRotation()->GetPQR())(2); return Rotation->GetPQR(eQ);
case FG_ROLLRATE: case FG_ROLLRATE:
return (FDMExec->GetRotation()->GetPQR())(1); return Rotation->GetPQR(eP);
case FG_YAWRATE: 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: case FG_ELEVATOR_POS:
return FDMExec->GetFCS()->GetDePos(); return FCS->GetDePos();
case FG_AILERON_POS: case FG_AILERON_POS:
return FDMExec->GetFCS()->GetDaPos(); return FCS->GetDaPos();
case FG_RUDDER_POS: case FG_RUDDER_POS:
return FDMExec->GetFCS()->GetDrPos(); return FCS->GetDrPos();
case FG_SPDBRAKE_POS: case FG_SPDBRAKE_POS:
return FDMExec->GetFCS()->GetDsbPos(); return FCS->GetDsbPos();
case FG_SPOILERS_POS: case FG_SPOILERS_POS:
return FDMExec->GetFCS()->GetDspPos(); return FCS->GetDspPos();
case FG_FLAPS_POS: case FG_FLAPS_POS:
return FDMExec->GetFCS()->GetDfPos(); return FCS->GetDfPos();
case FG_ELEVATOR_CMD: case FG_ELEVATOR_CMD:
return FDMExec->GetFCS()->GetDeCmd(); return FCS->GetDeCmd();
case FG_AILERON_CMD: case FG_AILERON_CMD:
return FDMExec->GetFCS()->GetDaCmd(); return FCS->GetDaCmd();
case FG_RUDDER_CMD: case FG_RUDDER_CMD:
return FDMExec->GetFCS()->GetDrCmd(); return FCS->GetDrCmd();
case FG_SPDBRAKE_CMD: case FG_SPDBRAKE_CMD:
return FDMExec->GetFCS()->GetDsbCmd(); return FCS->GetDsbCmd();
case FG_SPOILERS_CMD: case FG_SPOILERS_CMD:
return FDMExec->GetFCS()->GetDspCmd(); return FCS->GetDspCmd();
case FG_FLAPS_CMD: case FG_FLAPS_CMD:
return FDMExec->GetFCS()->GetDfCmd(); return FCS->GetDfCmd();
case FG_MACH: case FG_MACH:
return FDMExec->GetTranslation()->GetMach(); return Translation->GetMach();
case FG_ALTITUDE: case FG_ALTITUDE:
return FDMExec->GetPosition()->Geth(); return Position->Geth();
case FG_BI2VEL: case FG_BI2VEL:
if(FDMExec->GetTranslation()->GetVt() > 0) if(Translation->GetVt() > 0)
return FDMExec->GetAircraft()->GetWingSpan()/(2.0 * FDMExec->GetTranslation()->GetVt()); return Aircraft->GetWingSpan()/(2.0 * Translation->GetVt());
else else
return 0; return 0;
case FG_CI2VEL: case FG_CI2VEL:
if(FDMExec->GetTranslation()->GetVt() > 0) if(Translation->GetVt() > 0)
return FDMExec->GetAircraft()->Getcbar()/(2.0 * FDMExec->GetTranslation()->GetVt()); return Aircraft->Getcbar()/(2.0 * Translation->GetVt());
else else
return 0; return 0;
case FG_THROTTLE_CMD: 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: case FG_THROTTLE_POS:
return FDMExec->GetFCS()->GetThrottlePos(0); if (ActiveEngine < 0) return FCS->GetThrottlePos(0);
else return FCS->GetThrottlePos(ActiveEngine);
case FG_HOVERB: case FG_HOVERB:
return FDMExec->GetPosition()->GetHOverB(); return Position->GetHOverB();
case FG_PITCH_TRIM_CMD: case FG_PITCH_TRIM_CMD:
return FDMExec->GetFCS()->GetPitchTrimCmd(); return FCS->GetPitchTrimCmd();
default: default:
cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl; cerr << "FGState::GetParameter() - No handler for parameter " << val_idx << endl;
return 0.0; return 0.0;
@ -244,47 +266,47 @@ eParam FGState::GetParameterIndex(string val_string) {
void FGState::SetParameter(eParam val_idx, float val) { void FGState::SetParameter(eParam val_idx, float val) {
switch(val_idx) { switch(val_idx) {
case FG_ELEVATOR_POS: case FG_ELEVATOR_POS:
FDMExec->GetFCS()->SetDePos(val); FCS->SetDePos(val);
break; break;
case FG_AILERON_POS: case FG_AILERON_POS:
FDMExec->GetFCS()->SetDaPos(val); FCS->SetDaPos(val);
break; break;
case FG_RUDDER_POS: case FG_RUDDER_POS:
FDMExec->GetFCS()->SetDrPos(val); FCS->SetDrPos(val);
break; break;
case FG_SPDBRAKE_POS: case FG_SPDBRAKE_POS:
FDMExec->GetFCS()->SetDsbPos(val); FCS->SetDsbPos(val);
break; break;
case FG_SPOILERS_POS: case FG_SPOILERS_POS:
FDMExec->GetFCS()->SetDspPos(val); FCS->SetDspPos(val);
break; break;
case FG_FLAPS_POS: case FG_FLAPS_POS:
FDMExec->GetFCS()->SetDfPos(val); FCS->SetDfPos(val);
break; break;
case FG_THROTTLE_POS: case FG_THROTTLE_POS:
FDMExec->GetFCS()->SetThrottlePos(ActiveEngine,val); FCS->SetThrottlePos(ActiveEngine,val);
break; break;
case FG_ELEVATOR_CMD: case FG_ELEVATOR_CMD:
FDMExec->GetFCS()->SetDeCmd(val); FCS->SetDeCmd(val);
break; break;
case FG_AILERON_CMD: case FG_AILERON_CMD:
FDMExec->GetFCS()->SetDaCmd(val); FCS->SetDaCmd(val);
break; break;
case FG_RUDDER_CMD: case FG_RUDDER_CMD:
FDMExec->GetFCS()->SetDrCmd(val); FCS->SetDrCmd(val);
break; break;
case FG_SPDBRAKE_CMD: case FG_SPDBRAKE_CMD:
FDMExec->GetFCS()->SetDsbCmd(val); FCS->SetDsbCmd(val);
break; break;
case FG_SPOILERS_CMD: case FG_SPOILERS_CMD:
FDMExec->GetFCS()->SetDspCmd(val); FCS->SetDspCmd(val);
break; break;
case FG_FLAPS_CMD: case FG_FLAPS_CMD:
FDMExec->GetFCS()->SetDfCmd(val); FCS->SetDfCmd(val);
break; break;
case FG_THROTTLE_CMD: case FG_THROTTLE_CMD:
FDMExec->GetFCS()->SetThrottleCmd(ActiveEngine,val); FCS->SetThrottleCmd(ActiveEngine,val);
break; break;
case FG_ACTIVE_ENGINE: case FG_ACTIVE_ENGINE:
@ -292,19 +314,19 @@ void FGState::SetParameter(eParam val_idx, float val) {
break; break;
case FG_LEFT_BRAKE_CMD: case FG_LEFT_BRAKE_CMD:
FDMExec->GetFCS()->SetLBrake(val); FCS->SetLBrake(val);
break; break;
case FG_CENTER_BRAKE_CMD: case FG_CENTER_BRAKE_CMD:
FDMExec->GetFCS()->SetCBrake(val); FCS->SetCBrake(val);
break; break;
case FG_RIGHT_BRAKE_CMD: case FG_RIGHT_BRAKE_CMD:
FDMExec->GetFCS()->SetRBrake(val); FCS->SetRBrake(val);
break; break;
case FG_SET_LOGGING: case FG_SET_LOGGING:
if (val < -0.01) FDMExec->GetOutput()->Disable(); if (val < -0.01) Output->Disable();
else if (val > 0.01) FDMExec->GetOutput()->Enable(); else if (val > 0.01) Output->Enable();
else FDMExec->GetOutput()->Toggle(); else Output->Toggle();
break; break;
default: default:
@ -325,7 +347,7 @@ bool FGState::Reset(string path, string acname, string fname) {
resetDef = path + "/" + acname + "/" + fname + ".xml"; resetDef = path + "/" + acname + "/" + fname + ".xml";
ifstream resetfile(resetDef.c_str()); ifstream resetfile(resetDef.c_str(), ios::in | ios::binary );
if (resetfile) { if (resetfile) {
resetfile >> U; resetfile >> U;
@ -339,9 +361,9 @@ bool FGState::Reset(string path, string acname, string fname) {
resetfile >> h; resetfile >> h;
resetfile.close(); resetfile.close();
FDMExec->GetPosition()->SetLatitude(latitude*DEGTORAD); Position->SetLatitude(latitude*DEGTORAD);
FDMExec->GetPosition()->SetLongitude(longitude*DEGTORAD); Position->SetLongitude(longitude*DEGTORAD);
FDMExec->GetPosition()->Seth(h); Position->Seth(h);
Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD, Initialize(U, V, W, phi*DEGTORAD, tht*DEGTORAD, psi*DEGTORAD,
latitude*DEGTORAD, longitude*DEGTORAD, h); latitude*DEGTORAD, longitude*DEGTORAD, h);
@ -363,15 +385,15 @@ void FGState::Initialize(float U, float V, float W,
float Latitude, float Longitude, float H) { float Latitude, float Longitude, float H) {
FGColumnVector vUVW(3); FGColumnVector vUVW(3);
FGColumnVector vLocalVelNED(3); FGColumnVector vLocalVelNED(3);
FGColumnVector vEuler(3); FGColumnVector vLocalEuler(3);
float alpha, beta; float alpha, beta;
float qbar, Vt; float qbar, Vt;
FDMExec->GetPosition()->SetLatitude(Latitude); Position->SetLatitude(Latitude);
FDMExec->GetPosition()->SetLongitude(Longitude); Position->SetLongitude(Longitude);
FDMExec->GetPosition()->Seth(H); Position->Seth(H);
FDMExec->GetAtmosphere()->Run(); Atmosphere->Run();
if (W != 0.0) if (W != 0.0)
alpha = U*U > 0.0 ? atan2(W, U) : 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; beta = 0.0;
vUVW << U << V << W; vUVW << U << V << W;
FDMExec->GetTranslation()->SetUVW(vUVW); Translation->SetUVW(vUVW);
vEuler << phi << tht << psi; vLocalEuler << phi << tht << psi;
FDMExec->GetRotation()->SetEuler(vEuler); Rotation->SetEuler(vLocalEuler);
FDMExec->GetTranslation()->SetAB(alpha, beta); Translation->SetAB(alpha, beta);
Vt = sqrt(U*U + V*V + W*W); 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(); qbar = 0.5*(U*U + V*V + W*W)*Atmosphere->GetDensity();
FDMExec->GetTranslation()->Setqbar(qbar); Translation->Setqbar(qbar);
InitMatrices(phi, tht, psi); InitMatrices(phi, tht, psi);
vLocalVelNED = mTb2l*vUVW; 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); Initialize(U, V, W, phi, tht, psi, latitude, longitude, h);
FDMExec->GetPosition()->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() ); Position->SetSeaLevelRadius( FGIC->GetSeaLevelRadiusFtIC() );
FDMExec->GetPosition()->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() + Position->SetRunwayRadius( FGIC->GetSeaLevelRadiusFtIC() +
FGIC->GetTerrainAltitudeFtIC() ); FGIC->GetTerrainAltitudeFtIC() );
} }
@ -436,15 +458,15 @@ bool FGState::StoreData(string fname) {
ofstream datafile(fname.c_str()); ofstream datafile(fname.c_str());
if (datafile) { if (datafile) {
datafile << (FDMExec->GetTranslation()->GetUVW())(1); datafile << Translation->GetUVW(eU);
datafile << (FDMExec->GetTranslation()->GetUVW())(2); datafile << Translation->GetUVW(eV);
datafile << (FDMExec->GetTranslation()->GetUVW())(3); datafile << Translation->GetUVW(eW);
datafile << FDMExec->GetPosition()->GetLatitude(); datafile << Position->GetLatitude();
datafile << FDMExec->GetPosition()->GetLongitude(); datafile << Position->GetLongitude();
datafile << (FDMExec->GetRotation()->GetEuler())(1); datafile << Rotation->GetEuler(ePhi);
datafile << (FDMExec->GetRotation()->GetEuler())(2); datafile << Rotation->GetEuler(eTht);
datafile << (FDMExec->GetRotation()->GetEuler())(3); datafile << Rotation->GetEuler(ePsi);
datafile << FDMExec->GetPosition()->Geth(); datafile << Position->Geth();
datafile.close(); datafile.close();
return true; return true;
} else { } else {
@ -524,9 +546,6 @@ void FGState::CalcMatrices(void) {
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void FGState::IntegrateQuat(FGColumnVector vPQR, int rate) { 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(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(2) = 0.5*(vQtrn(1)*vPQR(eP) + vQtrn(3)*vPQR(eR) - vQtrn(4)*vPQR(eQ));
vQdot(3) = 0.5*(vQtrn(1)*vPQR(eQ) + vQtrn(4)*vPQR(eP) - vQtrn(2)*vPQR(eR)); vQdot(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) { FGColumnVector FGState::CalcEuler(void) {
static FGColumnVector vEuler(3);
if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001; if (mTl2b(3,3) == 0.0) mTl2b(3,3) = 0.0000001;
if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001; if (mTl2b(1,1) == 0.0) mTl2b(1,1) = 0.0000001;

53
src/FDM/JSBSim/FGState.h
View file

@ -62,11 +62,30 @@ INCLUDES
#include "FGMatrix.h" #include "FGMatrix.h"
/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
DEFINES DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
#define ID_STATE "$Id$" #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 CLASS DECLARATION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/
@ -78,6 +97,11 @@ public:
FGState(FGFDMExec*); FGState(FGFDMExec*);
~FGState(); ~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); bool Reset(string, string, string);
void Initialize(float, float, float, float, float, float, float, float, float); void Initialize(float, float, float, float, float, float, float, float, float);
void Initialize(FGInitialCondition *FGIC); void Initialize(FGInitialCondition *FGIC);
@ -115,7 +139,9 @@ public:
FGColumnVector CalcEuler(void); FGColumnVector CalcEuler(void);
FGMatrix GetTs2b(float alpha, float beta); FGMatrix GetTs2b(float alpha, float beta);
FGMatrix GetTl2b(void) { return mTl2b; } FGMatrix GetTl2b(void) { return mTl2b; }
float GetTl2b(int i, int j) { return mTl2b(i,j);}
FGMatrix GetTb2l(void) { return mTb2l; } FGMatrix GetTb2l(void) { return mTb2l; }
float GetTb2l(int i, int j) { return mTb2l(i,j);}
typedef map<eParam, string> ParamMap; typedef map<eParam, string> ParamMap;
ParamMap paramdef; ParamMap paramdef;
@ -132,16 +158,35 @@ private:
FGColumnVector vQtrn; FGColumnVector vQtrn;
FGColumnVector vlastQdot; FGColumnVector vlastQdot;
FGAircraft* Aircraft;
FGPosition* Position;
FGTranslation* Translation;
FGRotation* Rotation;
FGOutput* Output;
FGAtmosphere* Atmosphere;
FGFCS* FCS;
typedef map<string, eParam> CoeffMap; typedef map<string, eParam> CoeffMap;
CoeffMap coeffdef; CoeffMap coeffdef;
void Debug(void); void Debug(void);
int ActiveEngine; int ActiveEngine;
protected: FGColumnVector vQdot;
enum {ePhi=1, eTht, ePsi}; FGColumnVector vTmp;
enum {eP=1, eQ, eR}; 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 #endif

1
src/FDM/JSBSim/FGThruster.h
View file

@ -66,6 +66,7 @@ public:
float GetThrust(void) {return Thrust;} float GetThrust(void) {return Thrust;}
eType GetType(void) {return Type;} eType GetType(void) {return Type;}
string GetName(void) {return Name;} string GetName(void) {return Name;}
virtual float GetRPM(void) { return 0.0; };
protected: protected:
eType Type; eType Type;

6
src/FDM/JSBSim/FGTranslation.cpp
View file

@ -84,7 +84,9 @@ FGTranslation::FGTranslation(FGFDMExec* fdmex) : FGModel(fdmex),
vNcg(3), vNcg(3),
vPQR(3), vPQR(3),
vForces(3), vForces(3),
vEuler(3) vEuler(3),
vlastUVWdot(3),
mVel(3,3)
{ {
Name = "FGTranslation"; Name = "FGTranslation";
qbar = 0; qbar = 0;
@ -107,8 +109,6 @@ FGTranslation::~FGTranslation()
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
bool FGTranslation::Run(void) { bool FGTranslation::Run(void) {
static FGColumnVector vlastUVWdot(3);
static FGMatrix mVel(3,3);
if (!FGModel::Run()) { if (!FGModel::Run()) {

11
src/FDM/JSBSim/FGTranslation.h
View file

@ -81,9 +81,12 @@ public:
FGTranslation(FGFDMExec*); FGTranslation(FGFDMExec*);
~FGTranslation(); ~FGTranslation();
inline FGColumnVector GetUVW(void) { return vUVW; } inline FGColumnVector GetUVW (void) { return vUVW; }
inline FGColumnVector GetUVWdot(void) { return vUVWdot; } inline float GetUVW (int idx) { return vUVW(idx); }
inline FGColumnVector GetNcg(void) { return vNcg; } 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 Getalpha(void) { return alpha; }
inline float Getbeta (void) { return beta; } inline float Getbeta (void) { return beta; }
@ -116,6 +119,8 @@ private:
FGColumnVector vPQR; FGColumnVector vPQR;
FGColumnVector vForces; FGColumnVector vForces;
FGColumnVector vEuler; FGColumnVector vEuler;
FGColumnVector vlastUVWdot;
FGMatrix mVel;
float Vt, qbar, Mach; float Vt, qbar, Mach;
float Mass, dt; float Mass, dt;

6
src/FDM/JSBSim/FGTrim.cpp
View file

@ -165,9 +165,9 @@ void FGTrim::ReportState(void) {
cout << endl << " JSBSim State" << endl; cout << endl << " JSBSim State" << endl;
sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n", sprintf(out," Weight: %7.0f lbs. CG: %5.1f, %5.1f, %5.1f inches\n",
fdmex->GetAircraft()->GetWeight(), fdmex->GetAircraft()->GetWeight(),
fdmex->GetAircraft()->GetXYZcg()(1), fdmex->GetAircraft()->GetXYZcg(1),
fdmex->GetAircraft()->GetXYZcg()(2), fdmex->GetAircraft()->GetXYZcg(2),
fdmex->GetAircraft()->GetXYZcg()(3) ); fdmex->GetAircraft()->GetXYZcg(3));
cout << out; cout << out;
if( fdmex->GetFCS()->GetDfPos() <= 0.01) if( fdmex->GetFCS()->GetDfPos() <= 0.01)
sprintf(flap,"Up"); sprintf(flap,"Up");

40
src/FDM/JSBSim/FGTrimAxis.cpp
View file

@ -152,12 +152,12 @@ FGTrimAxis::~FGTrimAxis()
void FGTrimAxis::getState(void) { void FGTrimAxis::getState(void) {
switch(state) { switch(state) {
case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot()(1); break; case tUdot: state_value=fdmex->GetTranslation()->GetUVWdot(1); break;
case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot()(2); break; case tVdot: state_value=fdmex->GetTranslation()->GetUVWdot(2); break;
case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot()(3); break; case tWdot: state_value=fdmex->GetTranslation()->GetUVWdot(3); break;
case tQdot: state_value=fdmex->GetRotation()->GetPQRdot()(2);break; case tQdot: state_value=fdmex->GetRotation()->GetPQRdot(2);break;
case tPdot: state_value=fdmex->GetRotation()->GetPQRdot()(1); break; case tPdot: state_value=fdmex->GetRotation()->GetPQRdot(1); break;
case tRdot: state_value=fdmex->GetRotation()->GetPQRdot()(3); break; case tRdot: state_value=fdmex->GetRotation()->GetPQRdot(3); break;
case tHmgt: state_value=computeHmgt(); break; case tHmgt: state_value=computeHmgt(); break;
} }
} }
@ -246,7 +246,7 @@ void FGTrimAxis::SetThetaOnGround(float ff) {
i=0; ref=-1; center=-1; i=0; ref=-1; center=-1;
while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) { while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
if(fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) { 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; ref=i;
else else
center=i; center=i;
@ -260,9 +260,9 @@ void FGTrimAxis::SetThetaOnGround(float ff) {
if(ref >= 0) { if(ref >= 0) {
float sp=fdmex->GetRotation()->GetSinphi(); float sp=fdmex->GetRotation()->GetSinphi();
float cp=fdmex->GetRotation()->GetCosphi(); float cp=fdmex->GetRotation()->GetCosphi();
float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1); float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(1);
float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2); float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(2);
float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3); float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(3);
float hagl = -1*lx*sin(ff) + float hagl = -1*lx*sin(ff) +
ly*sp*cos(ff) + ly*sp*cos(ff) +
lz*cp*cos(ff); lz*cp*cos(ff);
@ -291,22 +291,22 @@ bool FGTrimAxis::initTheta(void) {
//find the first wheel unit forward of the cg //find the first wheel unit forward of the cg
//the list is short so a simple linear search is fine //the list is short so a simple linear search is fine
for( i=0; i<N; i++ ) { for( i=0; i<N; i++ ) {
if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) > 0 ) { if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(1) > 0 ) {
iForward=i; iForward=i;
break; break;
} }
} }
//now find the first wheel unit aft of the cg //now find the first wheel unit aft of the cg
for( i=0; i<N; i++ ) { for( i=0; i<N; i++ ) {
if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation()(1) < 0 ) { if(fdmex->GetAircraft()->GetGearUnit(i)->GetBodyLocation(1) < 0 ) {
iAft=i; iAft=i;
break; break;
} }
} }
// now adjust theta till the wheels are the same distance from the ground // now adjust theta till the wheels are the same distance from the ground
zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3); zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear(3);
zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3); zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear(3);
zDiff = zForward - zAft; zDiff = zForward - zAft;
level=false; level=false;
theta=fgic->GetPitchAngleDegIC(); theta=fgic->GetPitchAngleDegIC();
@ -314,8 +314,8 @@ bool FGTrimAxis::initTheta(void) {
theta+=2.0*zDiff; theta+=2.0*zDiff;
fgic->SetPitchAngleDegIC(theta); fgic->SetPitchAngleDegIC(theta);
fdmex->RunIC(fgic); fdmex->RunIC(fgic);
zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear()(3); zAft=fdmex->GetAircraft()->GetGearUnit(1)->GetLocalGear(3);
zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear()(3); zForward=fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear(3);
zDiff = zForward - zAft; zDiff = zForward - zAft;
//cout << endl << theta << " " << zDiff << endl; //cout << endl << theta << " " << zDiff << endl;
//cout << "0: " << fdmex->GetAircraft()->GetGearUnit(0)->GetLocalGear() << 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 //must have an off-center unit here
while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) { while( (ref < 0) && (i < fdmex->GetAircraft()->GetNumGearUnits()) ) {
if( (fdmex->GetAircraft()->GetGearUnit(i)->GetWOW()) && 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; ref=i;
i++; i++;
} }
if(ref >= 0) { if(ref >= 0) {
float st=fdmex->GetRotation()->GetSintht(); float st=fdmex->GetRotation()->GetSintht();
float ct=fdmex->GetRotation()->GetCostht(); float ct=fdmex->GetRotation()->GetCostht();
float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(1); float lx=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(1);
float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(2); float ly=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(2);
float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation()(3); float lz=fdmex->GetAircraft()->GetGearUnit(ref)->GetBodyLocation(3);
float hagl = -1*lx*st + float hagl = -1*lx*st +
ly*sin(ff)*ct + ly*sin(ff)*ct +
lz*cos(ff)*ct; lz*cos(ff)*ct;