#include <simgear/math/sg_geodesy.hxx>
#include <simgear/misc/sg_path.hxx>
-#include <Scenery/scenery.hxx>
+#include <FDM/flight.hxx>
#include <Aircraft/aircraft.hxx>
#include <Controls/controls.hxx>
#include <FDM/JSBSim/FGPropertyManager.h>
#include "JSBSim.hxx"
+static inline double
+FMAX (double a, double b)
+{
+ return a > b ? a : b;
+}
+
/******************************************************************************/
Position = fdmex->GetPosition();
Auxiliary = fdmex->GetAuxiliary();
Aerodynamics = fdmex->GetAerodynamics();
- GroundReactions = fdmex->GetGroundReactions();
-
+ GroundReactions = fdmex->GetGroundReactions();
- Atmosphere->UseInternal();
-
- fgic=new FGInitialCondition(fdmex);
+ fgic=fdmex->GetIC();
needTrim=true;
SGPath aircraft_path( globals->get_fg_root() );
SGPath engine_path( globals->get_fg_root() );
engine_path.append( "Engine" );
- set_delta_t( dt );
State->Setdt( dt );
result = fdmex->LoadModel( aircraft_path.str(),
init_gear();
- // Set initial fuel levels if provided.
+ // Set initial fuel levels if provided.
for (unsigned int i = 0; i < Propulsion->GetNumTanks(); i++) {
SGPropertyNode * node = fgGetNode("/consumables/fuel/tank", i, true);
if (node->getChild("level-gal_us", 0, false) != 0)
- Propulsion->GetTank(i)
- ->SetContents(node->getDoubleValue("level-gal_us") * 6.6);
+ Propulsion->GetTank(i)
+ ->SetContents(node->getDoubleValue("level-gal_us") * 6.6);
}
fgSetDouble("/fdm/trim/pitch-trim", FCS->GetPitchTrimCmd());
rudder_pos_pct->setDoubleValue(0);
flap_pos_pct->setDoubleValue(0);
+ temperature = fgGetNode("/environment/temperature-degc",true);
+ pressure = fgGetNode("/environment/pressure-inhg",true);
+ density = fgGetNode("/environment/density-slugft3",true);
+
+ wind_from_north= fgGetNode("/environment/wind-from-north-fps",true);
+ wind_from_east = fgGetNode("/environment/wind-from-east-fps" ,true);
+ wind_from_down = fgGetNode("/environment/wind-from-down-fps" ,true);
+
}
/******************************************************************************/
void FGJSBsim::init() {
SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" );
-
+
// Explicitly call the superclass's
// init method first.
- common_init();
-
- int i;
- ntanks = Propulsion->GetNumTanks();
- tanks = new PROP_NODE_PTR [ntanks];
- for (i = 0; i < ntanks; i++) {
- tanks[i] = fgGetNode("/consumables/fuel/tank", i, true);
- }
-
- nengines = Propulsion->GetNumEngines();
- engines = new engine_props [nengines];
-
- for (i = 0; i < nengines; i++) {
- SGPropertyNode *eng = fgGetNode("engines/engine", i);
- engines[i].mp_osi = eng->getNode("mp-osi");
- engines[i].egt_degf = eng->getNode("egt-degf");
- engines[i].fuel_flow_gph = eng->getNode("fuel-flow-gph");
- engines[i].cht_degf = eng->getNode("cht-degf");
- engines[i].oil_temperature_degf = eng->getNode("oil-temperature-degf");
- engines[i].oil_pressure_psi = eng->getNode("oil-pressure-psi");
- engines[i].running = eng->getNode("running");
- engines[i].cranking = eng->getNode("cranking");
- engines[i].rpm = eng->getNode("rpm");
+
+#ifdef FG_WEATHERCM
+ Atmosphere->UseInternal();
+#else
+ if (fgGetBool("/environment/params/control-fdm-atmosphere")) {
+ Atmosphere->UseExternal();
+ Atmosphere->SetExTemperature(
+ 9.0/5.0*(temperature->getDoubleValue()+273.15) );
+ Atmosphere->SetExPressure(pressure->getDoubleValue()*70.726566);
+ Atmosphere->SetExDensity(density->getDoubleValue());
+ } else {
+ Atmosphere->UseInternal();
}
+#endif
- copy_to_JSBsim();
+ fgic->SetVnorthFpsIC( wind_from_north->getDoubleValue() );
+ fgic->SetVeastFpsIC( wind_from_east->getDoubleValue() );
+ fgic->SetVdownFpsIC( wind_from_down->getDoubleValue() );
+
+ //Atmosphere->SetExTemperature(get_Static_temperature());
+ //Atmosphere->SetExPressure(get_Static_pressure());
+ //Atmosphere->SetExDensity(get_Density());
+ SG_LOG(SG_FLIGHT,SG_INFO,"T,p,rho: " << fdmex->GetAtmosphere()->GetTemperature()
+ << ", " << fdmex->GetAtmosphere()->GetPressure()
+ << ", " << fdmex->GetAtmosphere()->GetDensity() );
- fdmex->RunIC(fgic); //loop JSBSim once w/o integrating
+ common_init();
+ copy_to_JSBsim();
+
+ fdmex->RunIC(); //loop JSBSim once w/o integrating
copy_from_JSBsim(); //update the bus
SG_LOG( SG_FLIGHT, SG_INFO, " Initialized JSBSim with:" );
// Run an iteration of the EOM (equations of motion)
void
-FGJSBsim::update( int multiloop ) {
+FGJSBsim::update( double dt ) {
+
+ if (is_suspended())
+ return;
+
+ int multiloop = _calc_multiloop(dt);
int i;
+ // double save_alt = 0.0;
+
copy_to_JSBsim();
trimmed->setBoolValue(false);
if ( startup_trim->getBoolValue() ) {
SG_LOG(SG_FLIGHT, SG_INFO,
"Ready to trim, terrain altitude is: "
- << scenery.get_cur_elev() * SG_METER_TO_FEET );
- fgic->SetTerrainAltitudeFtIC( scenery.get_cur_elev() * SG_METER_TO_FEET );
+ << cur_fdm_state->get_Runway_altitude() * SG_METER_TO_FEET );
+ fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
do_trim();
} else {
- fdmex->RunIC(fgic); //apply any changes made through the set_ functions
+ fdmex->RunIC(); //apply any changes made through the set_ functions
}
needTrim = false;
}
bool FGJSBsim::copy_to_JSBsim() {
unsigned int i;
-
+
// copy control positions into the JSBsim structure
- FGControls *controls = globals->get_controls();
- FCS->SetDaCmd( controls->get_aileron());
- FCS->SetRollTrimCmd( controls->get_aileron_trim() );
- FCS->SetDeCmd( controls->get_elevator());
- FCS->SetPitchTrimCmd( controls->get_elevator_trim() );
- FCS->SetDrCmd( -controls->get_rudder() );
- FCS->SetYawTrimCmd( -controls->get_rudder_trim() );
- FCS->SetDfCmd( controls->get_flaps() );
+ FCS->SetDaCmd( globals->get_controls()->get_aileron());
+ FCS->SetRollTrimCmd( globals->get_controls()->get_aileron_trim() );
+ FCS->SetDeCmd( globals->get_controls()->get_elevator());
+ FCS->SetPitchTrimCmd( globals->get_controls()->get_elevator_trim() );
+ FCS->SetDrCmd( -globals->get_controls()->get_rudder() );
+ FCS->SetYawTrimCmd( -globals->get_controls()->get_rudder_trim() );
+ FCS->SetDfCmd( globals->get_controls()->get_flaps() );
FCS->SetDsbCmd( 0.0 ); //speedbrakes
FCS->SetDspCmd( 0.0 ); //spoilers
- FCS->SetLBrake( controls->get_brake( 0 ) );
- FCS->SetRBrake( controls->get_brake( 1 ) );
- FCS->SetCBrake( controls->get_brake( 2 ) );
- FCS->SetGearCmd( controls->get_gear_down());
- for (i = 0; i < nengines; i++) {
+
+ // Parking brake sets minimum braking
+ // level for mains.
+ double parking_brake = globals->get_controls()->get_parking_brake();
+ FCS->SetLBrake(FMAX(globals->get_controls()->get_brake(0), parking_brake));
+ FCS->SetRBrake(FMAX(globals->get_controls()->get_brake(1), parking_brake));
+ FCS->SetCBrake( globals->get_controls()->get_brake( 2 ) );
+
+ FCS->SetGearCmd( globals->get_controls()->get_gear_down());
+ for (i = 0; i < Propulsion->GetNumEngines(); i++) {
FGEngine * eng = Propulsion->GetEngine(i);
- FCS->SetThrottleCmd(i, controls->get_throttle(i));
- FCS->SetMixtureCmd(i, controls->get_mixture(i));
- FCS->SetPropAdvanceCmd(i, controls->get_prop_advance(i));
- Propulsion->GetThruster(i)->SetRPM(engines[i].rpm->getDoubleValue());
- eng->SetMagnetos( controls->get_magnetos(i) );
- eng->SetStarter( controls->get_starter(i) );
+ SGPropertyNode * node = fgGetNode("engines/engine", i, true);
+ FCS->SetThrottleCmd(i, globals->get_controls()->get_throttle(i));
+ FCS->SetMixtureCmd(i, globals->get_controls()->get_mixture(i));
+ FCS->SetPropAdvanceCmd(i, globals->get_controls()->get_prop_advance(i));
+ Propulsion->GetThruster(i)->SetRPM(node->getDoubleValue("rpm"));
+ eng->SetMagnetos( globals->get_controls()->get_magnetos(i) );
+ eng->SetStarter( globals->get_controls()->get_starter(i) );
}
- _set_Runway_altitude( scenery.get_cur_elev() * SG_METER_TO_FEET );
+ _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
Position->SetSeaLevelRadius( get_Sea_level_radius() );
Position->SetRunwayRadius( get_Runway_altitude()
+ 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());
+ Atmosphere->SetExTemperature(
+ 9.0/5.0*(temperature->getDoubleValue()+273.15) );
+ Atmosphere->SetExPressure(pressure->getDoubleValue()*70.726566);
+ Atmosphere->SetExDensity(density->getDoubleValue());
- for (i = 0; i < ntanks; i++) {
- Propulsion->GetTank(i)->SetContents(tanks[i]->getDoubleValue("level-gal_us") * 6.6);
+ Atmosphere->SetWindNED( wind_from_north->getDoubleValue(),
+ wind_from_east->getDoubleValue(),
+ wind_from_down->getDoubleValue() );
+// SG_LOG(SG_FLIGHT,SG_INFO, "Wind NED: "
+// << get_V_north_airmass() << ", "
+// << get_V_east_airmass() << ", "
+// << get_V_down_airmass() );
+
+ for (i = 0; i < Propulsion->GetNumTanks(); i++) {
+ SGPropertyNode * node = fgGetNode("/consumables/fuel/tank", i, true);
+ FGTank * tank = Propulsion->GetTank(i);
+ tank->SetContents(node->getDoubleValue("level-gal_us") * 6.6);
+// tank->SetContents(node->getDoubleValue("level-lb"));
}
return true;
// Positions
_updateGeocentricPosition( Position->GetLatitude(),
- Position->GetLongitude(),
- Position->Geth() );
+ Position->GetLongitude(),
+ Position->Geth() );
_set_Altitude_AGL( Position->GetDistanceAGL() );
}
}
- // Copy the engine values from JSBSim.
- for( i=0; i < nengines; i++ ) {
+ // Copy the engine values from JSBSim.
+ for( i=0; i < Propulsion->GetNumEngines(); i++ ) {
+ SGPropertyNode * node = fgGetNode("engines/engine", i, true);
FGEngine * eng = Propulsion->GetEngine(i);
- if(engines[i].mp_osi)
- engines[i].mp_osi->setDoubleValue( eng->getManifoldPressure_inHg());
- if(engines[i].egt_degf)
- engines[i].egt_degf->setDoubleValue( eng->getExhaustGasTemp_degF());
- if(engines[i].fuel_flow_gph)
- engines[i].fuel_flow_gph->setDoubleValue( eng->getFuelFlow_gph());
- if(engines[i].cht_degf)
- engines[i].cht_degf->setDoubleValue( eng->getCylinderHeadTemp_degF());
- if(engines[i].oil_temperature_degf)
- engines[i].oil_temperature_degf->setDoubleValue( eng->getOilTemp_degF());
- if(engines[i].oil_pressure_psi)
- engines[i].oil_pressure_psi->setDoubleValue( eng->getOilPressure_psi());
- if(engines[i].running)
- engines[i].running->setBoolValue( eng->GetRunning());
- if(engines[i].cranking)
- engines[i].cranking->setBoolValue( eng->GetCranking());
- if(engines[i].rpm)
- engines[i].rpm->setDoubleValue( Propulsion->GetThruster(i)->GetRPM());
+ FGThruster * thrust = Propulsion->GetThruster(i);
+
+ node->setDoubleValue("mp-osi", eng->getManifoldPressure_inHg());
+ node->setDoubleValue("rpm", thrust->GetRPM());
+ node->setDoubleValue("egt-degf", eng->getExhaustGasTemp_degF());
+ node->setDoubleValue("fuel-flow-gph", eng->getFuelFlow_gph());
+ node->setDoubleValue("cht-degf", eng->getCylinderHeadTemp_degF());
+ node->setDoubleValue("oil-temperature-degf", eng->getOilTemp_degF());
+ node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi());
+ node->setBoolValue("running", eng->GetRunning());
+ node->setBoolValue("cranking", eng->GetCranking());
}
- static const SGPropertyNode
- *fuel_freeze = fgGetNode("/sim/freeze/fuel");
+ static const SGPropertyNode *fuel_freeze
+ = fgGetNode("/sim/freeze/fuel");
- // Copy the fuel levels from JSBSim if fuel
- // freeze not enabled.
+ // Copy the fuel levels from JSBSim if fuel
+ // freeze not enabled.
if ( ! fuel_freeze->getBoolValue() ) {
- for (i = 0; i < ntanks; i++) {
- double contents = Propulsion->GetTank(i)->GetContents();
- tanks[i]->setDoubleValue("level-gal_us", contents/6.6);
- }
+ for (i = 0; i < Propulsion->GetNumTanks(); i++) {
+ SGPropertyNode * node
+ = fgGetNode("/consumables/fuel/tank", i, true);
+ double contents = Propulsion->GetTank(i)->GetContents();
+ node->setDoubleValue("level-gal_us", contents/6.6);
+ // node->setDoubleValue("level-lb", contents);
+ }
}
update_gear();
- stall_warning->setDoubleValue( Aircraft->GetStallWarn() );
+ stall_warning->setDoubleValue( Aerodynamics->GetStallWarn() );
+
+ /* elevator_pos_deg->setDoubleValue( FCS->GetDePos()*SG_RADIANS_TO_DEGREES );
+ left_aileron_pos_deg->setDoubleValue( FCS->GetDaLPos()*SG_RADIANS_TO_DEGREES );
+ right_aileron_pos_deg->setDoubleValue( FCS->GetDaRPos()*SG_RADIANS_TO_DEGREES );
+ rudder_pos_deg->setDoubleValue( -1*FCS->GetDrPos()*SG_RADIANS_TO_DEGREES );
+ flap_pos_deg->setDoubleValue( FCS->GetDfPos() ); */
+
elevator_pos_pct->setDoubleValue( FCS->GetDePos(ofNorm) );
left_aileron_pos_pct->setDoubleValue( FCS->GetDaLPos(ofNorm) );
right_aileron_pos_pct->setDoubleValue( -1*FCS->GetDaLPos(ofNorm) );
- rudder_pos_pct->setDoubleValue( FCS->GetDrPos(ofNorm) );
+ rudder_pos_pct->setDoubleValue( -1*FCS->GetDrPos(ofNorm) );
flap_pos_pct->setDoubleValue( FCS->GetDfPos(ofNorm) );
&sea_level_radius_meters, &lat_geoc );
_set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET );
fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET );
- _set_Runway_altitude( scenery.get_cur_elev() * SG_METER_TO_FEET );
- fgic->SetTerrainAltitudeFtIC( scenery.get_cur_elev() * SG_METER_TO_FEET );
+ _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
+ fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
fgic->SetLatitudeRadIC( lat_geoc );
needTrim=true;
}
SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon );
update_ic();
fgic->SetLongitudeRadIC( lon );
- _set_Runway_altitude( scenery.get_cur_elev() * SG_METER_TO_FEET );
- fgic->SetTerrainAltitudeFtIC( scenery.get_cur_elev() * SG_METER_TO_FEET );
+ _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
+ fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
needTrim=true;
}
&sea_level_radius_meters, &lat_geoc);
_set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET );
fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET );
- _set_Runway_altitude( scenery.get_cur_elev() * SG_METER_TO_FEET );
- fgic->SetTerrainAltitudeFtIC( scenery.get_cur_elev() * SG_METER_TO_FEET );
+ _set_Runway_altitude( cur_fdm_state->get_Runway_altitude() );
+ fgic->SetTerrainAltitudeFtIC( cur_fdm_state->get_ground_elev_ft() );
SG_LOG(SG_FLIGHT, SG_INFO,
- "Terrain altitude: " << scenery.get_cur_elev() * SG_METER_TO_FEET );
+ "Terrain altitude: " << cur_fdm_state->get_Runway_altitude() * SG_METER_TO_FEET );
fgic->SetLatitudeRadIC( lat_geoc );
fgic->SetAltitudeFtIC(alt);
needTrim=true;
needTrim=true;
}
-void FGJSBsim::set_Static_pressure(double p) {
- SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_pressure: " << p );
-
- update_ic();
- 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 );
-
- Atmosphere->SetExTemperature(T);
- if(Atmosphere->External() == true)
- needTrim=true;
-}
-
-
-void FGJSBsim::set_Density(double rho) {
- SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho );
-
- Atmosphere->SetExDensity(rho);
- if(Atmosphere->External() == true)
- needTrim=true;
-}
-
-void FGJSBsim::set_Velocities_Local_Airmass (double wnorth,
- double weast,
- double wdown ) {
- //SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local_Airmass: "
- // << wnorth << ", " << weast << ", " << wdown );
-
- _set_Velocities_Local_Airmass( wnorth, weast, wdown );
- fgic->SetWindNEDFpsIC( wnorth, weast, wdown );
- if(Atmosphere->External() == true)
- needTrim=true;
-}
-
void FGJSBsim::init_gear(void ) {
FGGroundReactions* gr=fdmex->GetGroundReactions();
for (int i=0;i<Ngear;i++) {
SGPropertyNode * node = fgGetNode("gear/gear", i, true);
node->setDoubleValue("xoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(1));
+ gr->GetGearUnit(i)->GetBodyLocation()(1));
node->setDoubleValue("yoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(2));
+ gr->GetGearUnit(i)->GetBodyLocation()(2));
node->setDoubleValue("zoffset-in",
- gr->GetGearUnit(i)->GetBodyLocation()(3));
+ gr->GetGearUnit(i)->GetBodyLocation()(3));
node->setBoolValue("wow", gr->GetGearUnit(i)->GetWOW());
node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0);
node->setDoubleValue("position-norm", FCS->GetGearPos());
}
void FGJSBsim::update_gear(void) {
-
- static myNode *nodes;
- static int been_here = 0;
+
FGGroundReactions* gr=fdmex->GetGroundReactions();
int Ngear=GroundReactions->GetNumGearUnits();
- int i;
- if( !been_here ) {
- nodes = new myNode[Ngear];
- for (i=0;i<Ngear;i++) {
- SGPropertyNode * node = fgGetNode("gear/gear", i, true);
- nodes[i][0] = node -> getChild("wow", 0, true);
- nodes[i][1] = node -> getChild("position-norm", 0, true);
- }
- been_here = ~been_here;
- }
- for (i=0;i<Ngear;i++) {
- nodes[i][0] -> setBoolValue(gr->GetGearUnit(i)->GetWOW());
- nodes[i][1] -> setDoubleValue(FCS->GetGearPos());
+ for (int i=0;i<Ngear;i++) {
+ SGPropertyNode * node = fgGetNode("gear/gear", i, true);
+ node->getChild("wow", 0, true)
+ ->setBoolValue(gr->GetGearUnit(i)->GetWOW());
+ node->getChild("position-norm", 0, true)
+ ->setDoubleValue(FCS->GetGearPos());
}
}
void FGJSBsim::do_trim(void) {
FGTrim *fgtrim;
- if(fgic->GetVcalibratedKtsIC() < 10 ) {
+ if( fgGetBool("/sim/startup/onground") ) {
fgic->SetVcalibratedKtsIC(0.0);
- fgtrim=new FGTrim(fdmex,fgic,tGround);
+ fgtrim=new FGTrim(fdmex,tGround);
} else {
- fgtrim=new FGTrim(fdmex,fgic,tLongitudinal);
+ fgtrim=new FGTrim(fdmex,tLongitudinal);
}
if( !fgtrim->DoTrim() ) {
fgtrim->Report();
aileron_trim->setDoubleValue( FCS->GetDaCmd() );
rudder_trim->setDoubleValue( FCS->GetDrCmd() );
- FGControls *controls = globals->get_controls();
- controls->set_elevator_trim(FCS->GetPitchTrimCmd());
- controls->set_elevator(FCS->GetDeCmd());
- controls->set_throttle(FGControls::ALL_ENGINES,
- FCS->GetThrottleCmd(0));
+ globals->get_controls()->set_elevator_trim(FCS->GetPitchTrimCmd());
+ globals->get_controls()->set_elevator(FCS->GetDeCmd());
+ globals->get_controls()->set_throttle(FGControls::ALL_ENGINES,
+ FCS->GetThrottleCmd(0));
- controls->set_aileron(FCS->GetDaCmd());
- controls->set_rudder( FCS->GetDrCmd());
+ globals->get_controls()->set_aileron(FCS->GetDaCmd());
+ globals->get_controls()->set_rudder( FCS->GetDrCmd());
SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" );
}
projects / flightgear.git / blobdiff