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;
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;i<Neng;i++) {
add_engine( FGEngInterface() );
add_engine( FGEngInterface() );
}
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());
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")) {
controls.set_aileron(fdmex->GetFCS()->GetDaCmd());
controls.set_rudder(fdmex->GetFCS()->GetDrCmd());
//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", 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++ ) {
get_engine(i)->set_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 ) );
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 ) );
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());
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_Omega_Body( fdmex->GetRotation()->GetPQR()(1),
fdmex->GetRotation()->GetPQR()(2),
fdmex->GetRotation()->GetPQR()(3) );
// _set_V_calibrated( Auxiliary->GetVcalibratedFPS() );
_set_Euler_Rates( fdmex->GetRotation()->GetEulerRates()(1),
fdmex->GetRotation()->GetEulerRates()(2),
fdmex->GetRotation()->GetEulerRates()(3) );
_set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() );
_set_Geocentric_Rates(fdmex->GetPosition()->GetLatitudeDot(),
fdmex->GetPosition()->GetLongitudeDot(),
fdmex->GetPosition()->Gethdot() );
_set_V_ground_speed( Position->GetVground() );
_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
_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;
}
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;
copy_from_JSBsim(); //update the bus
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 );
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;
}

10
src/FDM/JSBSim.hxx
View file

@ -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;

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

@ -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;
}

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

@ -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;

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

@ -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;

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

@ -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);

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

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

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

@ -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;

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

@ -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,

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

@ -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;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;
}
@ -90,9 +93,11 @@ FGFCS::~FGFCS()
bool FGFCS::Run(void)
{
unsigned int i;
if (!FGModel::Run()) {
for (unsigned int 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<ThrottlePos.size(); i++) ThrottlePos[i] = ThrottleCmd[i];
for (i=0; i<Components.size(); i++) Components[i]->Run();
} 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;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
for (ctr=0;ctr<ThrottleCmd.size();ctr++) ThrottleCmd[ctr] = setting;
} else {
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) {
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 {
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;
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 = "";
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 += ", ";

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

@ -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

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

@ -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

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
//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;

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

@ -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.

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

@ -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

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) {
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;

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

@ -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);

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

@ -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;
}

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

@ -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) {

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;
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;
}
@ -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;

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

@ -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();

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

@ -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;

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

@ -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;

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

@ -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<eParam, string> 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<string, eParam> 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

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

@ -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;

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

@ -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()) {

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

@ -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;

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

@ -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");

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

@ -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; i<N; i++ ) {
if(fdmex->GetAircraft()->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; i<N; i++ ) {
if(fdmex->GetAircraft()->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;