From: curt Date: Sun, 24 Feb 2002 20:54:23 +0000 (+0000) Subject: Moved to JSBSim subdirectory. X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;ds=sidebyside;h=1b6cde22dcf46d9b44c99ee979c14b9317c47b8d;p=flightgear.git Moved to JSBSim subdirectory. --- diff --git a/src/FDM/JSBSim/JSBSim.cxx b/src/FDM/JSBSim/JSBSim.cxx new file mode 100644 index 000000000..11e00bfb5 --- /dev/null +++ b/src/FDM/JSBSim/JSBSim.cxx @@ -0,0 +1,759 @@ +// JSBsim.cxx -- interface to the JSBsim flight model +// +// Written by Curtis Olson, started February 1999. +// +// Copyright (C) 1999 Curtis L. Olson - curt@flightgear.org +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#include + +#ifdef SG_MATH_EXCEPTION_CLASH +# include +#endif + +#include STL_STRING + +#include +#include +#include +#include + +#include + +#include +#include +#include
+#include
+ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "JSBSim.hxx" + +/******************************************************************************/ + +FGJSBsim::FGJSBsim( double dt ) + : FGInterface(dt) +{ + bool result; + + fdmex = new FGFDMExec; + + State = fdmex->GetState(); + Atmosphere = fdmex->GetAtmosphere(); + FCS = fdmex->GetFCS(); + MassBalance = fdmex->GetMassBalance(); + Propulsion = fdmex->GetPropulsion(); + Aircraft = fdmex->GetAircraft(); + Translation = fdmex->GetTranslation(); + Rotation = fdmex->GetRotation(); + Position = fdmex->GetPosition(); + Auxiliary = fdmex->GetAuxiliary(); + Aerodynamics = fdmex->GetAerodynamics(); + GroundReactions = fdmex->GetGroundReactions(); + + + Atmosphere->UseInternal(); + + fgic=new FGInitialCondition(fdmex); + needTrim=true; + + SGPath aircraft_path( globals->get_fg_root() ); + aircraft_path.append( "Aircraft" ); + + SGPath engine_path( globals->get_fg_root() ); + engine_path.append( "Engine" ); + set_delta_t( dt ); + State->Setdt( dt ); + + result = fdmex->LoadModel( aircraft_path.str(), + engine_path.str(), + fgGetString("/sim/aero") ); + + if (result) { + SG_LOG( SG_FLIGHT, SG_INFO, " loaded aero."); + } else { + SG_LOG( SG_FLIGHT, SG_INFO, + " aero does not exist (you may have mis-typed the name)."); + throw(-1); + } + + SG_LOG( SG_FLIGHT, SG_INFO, "" ); + SG_LOG( SG_FLIGHT, SG_INFO, "" ); + SG_LOG( SG_FLIGHT, SG_INFO, "After loading aero definition file ..." ); + + int Neng = Propulsion->GetNumEngines(); + SG_LOG( SG_FLIGHT, SG_INFO, "num engines = " << Neng ); + + if ( GroundReactions->GetNumGearUnits() <= 0 ) { + SG_LOG( SG_FLIGHT, SG_ALERT, "num gear units = " + << GroundReactions->GetNumGearUnits() ); + SG_LOG( SG_FLIGHT, SG_ALERT, "This is a very bad thing because with 0 gear units, the ground trimming"); + SG_LOG( SG_FLIGHT, SG_ALERT, "routine (coming up later in the code) will core dump."); + SG_LOG( SG_FLIGHT, SG_ALERT, "Halting the sim now, and hoping a solution will present itself soon!"); + exit(-1); + } + + + init_gear(); + + // 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); + } + + 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()); + + startup_trim = fgGetNode("/sim/startup/trim", true); + + trimmed = fgGetNode("/fdm/trim/trimmed", true); + trimmed->setBoolValue(false); + + pitch_trim = fgGetNode("/fdm/trim/pitch-trim", true ); + throttle_trim = fgGetNode("/fdm/trim/throttle", true ); + aileron_trim = fgGetNode("/fdm/trim/aileron", true ); + rudder_trim = fgGetNode("/fdm/trim/rudder", true ); + + + stall_warning = fgGetNode("/sim/aero/alarms/stall-warning",true); + stall_warning->setDoubleValue(0); +} + +/******************************************************************************/ +FGJSBsim::~FGJSBsim(void) { + if (fdmex != NULL) { + delete fdmex; fdmex=NULL; + delete fgic; fgic=NULL; + } +} + +/******************************************************************************/ + +// Initialize the JSBsim flight model, dt is the time increment for +// each subsequent iteration through the EOM + +void FGJSBsim::init() { + + SG_LOG( SG_FLIGHT, SG_INFO, "Starting and initializing JSBsim" ); + + // Explicitly call the superclass's + // init method first. + common_init(); + copy_to_JSBsim(); + + fdmex->RunIC(fgic); //loop JSBSim once w/o integrating + copy_from_JSBsim(); //update the bus + + SG_LOG( SG_FLIGHT, SG_INFO, " Initialized JSBSim with:" ); + + switch(fgic->GetSpeedSet()) { + case setned: + 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= " + << Translation->GetUVW(1) << ", " + << Translation->GetUVW(2) << ", " + << Translation->GetUVW(3) << " ft/s"); + break; + case setmach: + SG_LOG(SG_FLIGHT,SG_INFO, " Mach: " + << Translation->GetMach() ); + break; + case setvc: + default: + SG_LOG(SG_FLIGHT,SG_INFO, " Indicated Airspeed: " + << Auxiliary->GetVcalibratedKTS() << " knots" ); + break; + } + + stall_warning->setDoubleValue(0); + + 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, " Altitude: " + << Position->Geth() << " feet" ); + SG_LOG( SG_FLIGHT, SG_INFO, " loaded initial conditions" ); + + SG_LOG( SG_FLIGHT, SG_INFO, " set dt" ); + + SG_LOG( SG_FLIGHT, SG_INFO, "Finished initializing JSBSim" ); + + SG_LOG( SG_FLIGHT, SG_INFO, "FGControls::get_gear_down()= " << + globals->get_controls()->get_gear_down() ); + + + +} + +/******************************************************************************/ + +// Run an iteration of the EOM (equations of motion) + +void +FGJSBsim::update( int multiloop ) { + + int i; + + // double save_alt = 0.0; + + copy_to_JSBsim(); + + trimmed->setBoolValue(false); + + if ( needTrim ) { + if ( startup_trim->getBoolValue() ) { + SG_LOG(SG_FLIGHT, SG_INFO, + "Ready to trim, terrain altitude is: " + << scenery.get_cur_elev() * SG_METER_TO_FEET ); + SG_LOG( SG_FLIGHT, SG_INFO, "position = " + << fgGetDouble("/position/longitude-deg") << ", " + << fgGetDouble("/position/latitude-deg") ); + fgic->SetTerrainAltitudeFtIC( scenery.get_cur_elev() * SG_METER_TO_FEET ); + do_trim(); + } else { + fdmex->RunIC(fgic); //apply any changes made through the set_ functions + } + needTrim = false; + } + + for ( i=0; i < multiloop; i++ ) { + fdmex->Run(); + } + + FGJSBBase::Message* msg; + while (fdmex->ReadMessage()) { + msg = fdmex->ProcessMessage(); + switch (msg->type) { + case FGJSBBase::Message::eText: + SG_LOG( SG_FLIGHT, SG_INFO, msg->messageId << ": " << msg->text ); + break; + case FGJSBBase::Message::eBool: + SG_LOG( SG_FLIGHT, SG_INFO, msg->messageId << ": " << msg->text << " " << msg->bVal ); + break; + case FGJSBBase::Message::eInteger: + SG_LOG( SG_FLIGHT, SG_INFO, msg->messageId << ": " << msg->text << " " << msg->iVal ); + break; + case FGJSBBase::Message::eDouble: + SG_LOG( SG_FLIGHT, SG_INFO, msg->messageId << ": " << msg->text << " " << msg->dVal ); + break; + default: + SG_LOG( SG_FLIGHT, SG_INFO, "Unrecognized message type." ); + break; + } + } + + // translate JSBsim back to FG structure so that the + // autopilot (and the rest of the sim can use the updated values + copy_from_JSBsim(); +} + +/******************************************************************************/ + +// Convert from the FGInterface struct to the JSBsim generic_ struct + +bool FGJSBsim::copy_to_JSBsim() { + unsigned int i; + + // copy control positions into the JSBsim structure + + 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( globals->get_controls()->get_brake( 0 ) ); + FCS->SetRBrake( globals->get_controls()->get_brake( 1 ) ); + 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); + 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 ); + 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()); +// 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; +} + +/******************************************************************************/ + +// Convert from the JSBsim generic_ struct to the FGInterface struct + +bool FGJSBsim::copy_from_JSBsim() { + unsigned int i, j; + + _set_Inertias( MassBalance->GetMass(), + MassBalance->GetIxx(), + MassBalance->GetIyy(), + MassBalance->GetIzz(), + MassBalance->GetIxz() ); + + _set_CG_Position( MassBalance->GetXYZcg(1), + MassBalance->GetXYZcg(2), + MassBalance->GetXYZcg(3) ); + + _set_Accels_Body( Aircraft->GetBodyAccel()(1), + Aircraft->GetBodyAccel()(2), + Aircraft->GetBodyAccel()(3) ); + + //_set_Accels_CG_Body( Aircraft->GetBodyAccel()(1), + // Aircraft->GetBodyAccel()(2), + // Aircraft->GetBodyAccel()(3) ); + // + _set_Accels_CG_Body_N ( Aircraft->GetNcg()(1), + Aircraft->GetNcg()(2), + Aircraft->GetNcg()(3) ); + + _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel()(1), + Auxiliary->GetPilotAccel()(2), + Auxiliary->GetPilotAccel()(3) ); + + // _set_Accels_Pilot_Body_N( Auxiliary->GetPilotAccel()(1)/32.1739, + // Auxiliary->GetNpilot(2)/32.1739, + // Auxiliary->GetNpilot(3)/32.1739 ); + + _set_Nlf( Aircraft->GetNlf() ); + + // Velocities + + _set_Velocities_Local( Position->GetVn(), + Position->GetVe(), + Position->GetVd() ); + + _set_Velocities_Wind_Body( Translation->GetUVW(1), + Translation->GetUVW(2), + Translation->GetUVW(3) ); + + _set_V_rel_wind( Translation->GetVt() ); + + _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() ); + + // _set_V_calibrated( Auxiliary->GetVcalibratedFPS() ); + + _set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() ); + + _set_V_ground_speed( Position->GetVground() ); + + _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 + _updateGeocentricPosition( Position->GetLatitude(), + Position->GetLongitude(), + Position->Geth() ); + + _set_Altitude_AGL( Position->GetDistanceAGL() ); + + _set_Euler_Angles( Rotation->Getphi(), + Rotation->Gettht(), + Rotation->Getpsi() ); + + _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( Auxiliary->GetEarthPositionAngle() ); + + _set_Climb_Rate( Position->Gethdot() ); + + + for ( i = 1; i <= 3; i++ ) { + for ( j = 1; j <= 3; j++ ) { + _set_T_Local_to_Body( i, j, State->GetTl2b(i,j) ); + } + } + + // 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); + 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"); + + // Copy the fuel levels from JSBSim if fuel + // freeze not enabled. + if ( ! fuel_freeze->getBoolValue() ) { + 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() ); + + return true; +} + +bool FGJSBsim::ToggleDataLogging(void) { + return fdmex->GetOutput()->Toggle(); +} + + +bool FGJSBsim::ToggleDataLogging(bool state) { + if (state) { + fdmex->GetOutput()->Enable(); + return true; + } else { + fdmex->GetOutput()->Disable(); + return false; + } +} + + +//Positions +void FGJSBsim::set_Latitude(double lat) { + static const SGPropertyNode *altitude = fgGetNode("/position/altitude-ft"); + double alt; + double sea_level_radius_meters, lat_geoc; + + if ( altitude->getDoubleValue() > -9990 ) { + alt = altitude->getDoubleValue(); + } else { + alt = 0.0; + } + + update_ic(); + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Latitude: " << lat ); + SG_LOG(SG_FLIGHT,SG_INFO," cur alt (ft) = " << alt ); + + sgGeodToGeoc( lat, alt * SG_FEET_TO_METER, + &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 ); + fgic->SetLatitudeRadIC( lat_geoc ); + needTrim=true; +} + +void FGJSBsim::set_Longitude(double lon) { + + 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 ); + needTrim=true; +} + +void FGJSBsim::set_Altitude(double alt) { + static const SGPropertyNode *latitude = fgGetNode("/position/latitude-deg"); + + double sea_level_radius_meters,lat_geoc; + + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Altitude: " << alt ); + SG_LOG(SG_FLIGHT,SG_INFO, " lat (deg) = " << latitude->getDoubleValue() ); + + update_ic(); + sgGeodToGeoc( latitude->getDoubleValue() * SGD_DEGREES_TO_RADIANS, alt, + &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 ); + SG_LOG(SG_FLIGHT, SG_INFO, + "Terrain altitude: " << scenery.get_cur_elev() * SG_METER_TO_FEET ); + SG_LOG( SG_FLIGHT, SG_INFO, "position = " + << fgGetDouble("/position/longitude-deg") << ", " + << fgGetDouble("/position/latitude-deg") ); + fgic->SetLatitudeRadIC( lat_geoc ); + fgic->SetAltitudeFtIC(alt); + needTrim=true; +} + +void FGJSBsim::set_V_calibrated_kts(double vc) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_V_calibrated_kts: " << vc ); + + update_ic(); + fgic->SetVcalibratedKtsIC(vc); + needTrim=true; +} + +void FGJSBsim::set_Mach_number(double mach) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); + + update_ic(); + fgic->SetMachIC(mach); + 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 ); + + update_ic(); + fgic->SetVnorthFpsIC(north); + fgic->SetVeastFpsIC(east); + fgic->SetVdownFpsIC(down); + 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 ); + + update_ic(); + fgic->SetUBodyFpsIC(u); + fgic->SetVBodyFpsIC(v); + fgic->SetWBodyFpsIC(w); + needTrim=true; +} + +//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 ); + + update_ic(); + fgic->SetPitchAngleRadIC(theta); + fgic->SetRollAngleRadIC(phi); + fgic->SetTrueHeadingRadIC(psi); + needTrim=true; +} + +//Flight Path +void FGJSBsim::set_Climb_Rate( double roc) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc ); + + update_ic(); + //since both climb rate and flight path angle are set in the FG + //startup sequence, something is needed to keep one from cancelling + //out the other. + if( !(fabs(roc) > 1 && fabs(fgic->GetFlightPathAngleRadIC()) < 0.01) ) { + fgic->SetClimbRateFpsIC(roc); + } + needTrim=true; +} + +void FGJSBsim::set_Gamma_vert_rad( double gamma) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma ); + + update_ic(); + if( !(fabs(gamma) < 0.01 && fabs(fgic->GetClimbRateFpsIC()) > 1) ) { + fgic->SetFlightPathAngleRadIC(gamma); + } + 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_DEBUG, "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(); + int Ngear=GroundReactions->GetNumGearUnits(); + for (int i=0;isetDoubleValue("xoffset-in", + gr->GetGearUnit(i)->GetBodyLocation()(1)); + node->setDoubleValue("yoffset-in", + gr->GetGearUnit(i)->GetBodyLocation()(2)); + node->setDoubleValue("zoffset-in", + gr->GetGearUnit(i)->GetBodyLocation()(3)); + node->setBoolValue("wow", gr->GetGearUnit(i)->GetWOW()); + node->setBoolValue("has-brake", gr->GetGearUnit(i)->GetBrakeGroup() > 0); + node->setDoubleValue("position", FCS->GetGearPos()); + } +} + +void FGJSBsim::update_gear(void) { + + FGGroundReactions* gr=fdmex->GetGroundReactions(); + int Ngear=GroundReactions->GetNumGearUnits(); + for (int i=0;igetChild("wow", 0, true) + ->setBoolValue(gr->GetGearUnit(i)->GetWOW()); + node->getChild("position", 0, true) + ->setDoubleValue(FCS->GetGearPos()); + } +} + +void FGJSBsim::do_trim(void) { + + 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); + } + State->ReportState(); + delete fgtrim; + pitch_trim->setDoubleValue( FCS->GetPitchTrimCmd() ); + throttle_trim->setDoubleValue( FCS->GetThrottleCmd(0) ); + aileron_trim->setDoubleValue( FCS->GetDaCmd() ); + rudder_trim->setDoubleValue( FCS->GetDrCmd() ); + + 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)); + + globals->get_controls()->set_aileron(FCS->GetDaCmd()); + globals->get_controls()->set_rudder( FCS->GetDrCmd()); + + SG_LOG( SG_FLIGHT, SG_INFO, " Trim complete" ); +} + +void FGJSBsim::update_ic(void) { + if( !needTrim ) { + fgic->SetLatitudeRadIC(get_Lat_geocentric() ); + fgic->SetLongitudeRadIC( get_Longitude() ); + fgic->SetAltitudeFtIC( get_Altitude() ); + fgic->SetVcalibratedKtsIC( get_V_calibrated_kts() ); + fgic->SetPitchAngleRadIC( get_Theta() ); + fgic->SetRollAngleRadIC( get_Phi() ); + fgic->SetTrueHeadingRadIC( get_Psi() ); + fgic->SetClimbRateFpsIC( get_Climb_Rate() ); + } +} diff --git a/src/FDM/JSBSim/JSBSim.hxx b/src/FDM/JSBSim/JSBSim.hxx new file mode 100644 index 000000000..55dcbc453 --- /dev/null +++ b/src/FDM/JSBSim/JSBSim.hxx @@ -0,0 +1,252 @@ +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + Header: JSBSim.hxx + Author: Curtis L. Olson + Maintained by: Tony Peden, Curt Olson + Date started: 02/01/1999 + +------ Copyright (C) 1999 - 2000 Curtis L. Olson (curt@flightgear.org) ------ + + This program is free software; you can redistribute it and/or + modify it under the terms of the GNU General Public License as + published by the Free Software Foundation; either version 2 of the + License, or (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + +HISTORY +-------------------------------------------------------------------------------- +02/01/1999 CLO Created +Additional log messages stored in CVS + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +SENTRY +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#ifndef _JSBSIM_HXX +#define _JSBSIM_HXX + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +INCLUDES +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#undef MAX_ENGINES +#include + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +DEFINITIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#define ID_JSBSIMXX "$Header JSBSim.hxx,v 1.4 2000/10/22 14:02:16 jsb Exp $" + +#define METERS_TO_FEET 3.2808398950 +#define RADTODEG 57.2957795 + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +FORWARD DECLARATIONS +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +#include + +#include + +class FGState; +class FGAtmosphere; +class FGFCS; +class FGPropulsion; +class FGMassBalance; +class FGAerodynamics; +class FGInertial; +class FGAircraft; +class FGTranslation; +class FGRotation; +class FGPosition; +class FGAuxiliary; +class FGOutput; +class FGInitialCondition; + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +COMMENTS, REFERENCES, and NOTES [use "class documentation" below for API docs] +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DOCUMENTATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +/** FGFS / JSBSim interface (aka "The Bus"). + This class provides for an interface between FlightGear and its data + structures and JSBSim and its data structures. This is the class which is + used to command JSBSim when integrated with FlightGear. See the + documentation for main for direction on running JSBSim apart from FlightGear. + @author Curtis L. Olson (original) + @author Tony Peden (Maintained and refined) + @version $Id$ + @see main in file JSBSim.cpp (use main() wrapper for standalone usage) +*/ + +/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +CLASS DECLARATION +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ + +class FGJSBsim: public FGInterface { + +public: + /// Constructor + FGJSBsim( double dt ); + /// Destructor + ~FGJSBsim(); + + /// copy FDM state to LaRCsim structures + bool copy_to_JSBsim(); + + /// copy FDM state from LaRCsim structures + bool copy_from_JSBsim(); + + /// Reset flight params to a specific position + void init(); + + /// @name Position Parameter Set + //@{ + /** Set geocentric latitude + @param lat latitude in radians measured from the 0 meridian where + the westerly direction is positive and east is negative */ + void set_Latitude(double lat); // geocentric + + /** Set longitude + @param lon longitude in radians measured from the equator where + the northerly direction is positive and south is negative */ + void set_Longitude(double lon); + + /** Set altitude + Note: this triggers a recalculation of AGL altitude + @param alt altitude in feet */ + void set_Altitude(double alt); // triggers re-calc of AGL altitude + //@} + + //void set_AltitudeAGL(double altagl); // and vice-versa + + /// @name Velocity Parameter Set + //@{ + /** Sets calibrated airspeed + Setting this will trigger a recalc of the other velocity terms. + @param vc Calibrated airspeed in ft/sec */ + void set_V_calibrated_kts(double vc); + + /** Sets Mach number. + Setting this will trigger a recalc of the other velocity terms. + @param mach Mach number */ + void set_Mach_number(double mach); + + /** Sets velocity in N-E-D coordinates. + Setting this will trigger a recalc of the other velocity terms. + @param north velocity northward in ft/sec + @param east velocity eastward in ft/sec + @param down velocity downward in ft/sec */ + void set_Velocities_Local( double north, double east, double down ); + + /** Sets aircraft velocity in stability frame. + Setting this will trigger a recalc of the other velocity terms. + @param u X velocity in ft/sec + @param v Y velocity in ft/sec + @param w Z velocity in ft/sec */ + void set_Velocities_Wind_Body( double u, double v, double w); + //@} + + /** Euler Angle Parameter Set + @param phi roll angle in radians + @param theta pitch angle in radians + @param psi heading angle in radians */ + void set_Euler_Angles( double phi, double theta, double psi ); + + /// @name Flight Path Parameter Set + //@{ + /** Sets rate of climb + @param roc Rate of climb in ft/sec */ + void set_Climb_Rate( double roc); + + /** Sets the flight path angle in radians + @param gamma flight path angle in radians. */ + void set_Gamma_vert_rad( double gamma); + //@} + + + /// @name Atmospheric Parameter Set + //@{ + /** Sets the atmospheric static pressure + @param p pressure in psf */ + void set_Static_pressure(double p); + + /** Sets the atmospheric temperature + @param T temperature in degrees rankine */ + void set_Static_temperature(double T); + + /** Sets the atmospheric density. + @param rho air density slugs/cubic foot */ + void set_Density(double rho); + + /** Sets the velocity of the local airmass for wind modeling. + @param wnorth velocity north in fps + @param weast velocity east in fps + @param wdown velocity down in fps*/ + void set_Velocities_Local_Airmass (double wnorth, + double weast, + double wdown ); + /// @name Position Parameter Update + //@{ + + + /** Update the position based on inputs, positions, velocities, etc. + @param multiloop number of times to loop through the FDM + @return true if successful */ + void update( int multiloop ); + bool ToggleDataLogging(bool state); + bool ToggleDataLogging(void); + void do_trim(void); + void update_ic(void); + +private: + FGFDMExec *fdmex; + FGInitialCondition *fgic; + bool needTrim; + + FGState* State; + FGAtmosphere* Atmosphere; + FGFCS* FCS; + FGPropulsion* Propulsion; + FGMassBalance* MassBalance; + FGAircraft* Aircraft; + FGTranslation* Translation; + FGRotation* Rotation; + FGPosition* Position; + FGAuxiliary* Auxiliary; + FGAerodynamics* Aerodynamics; + FGGroundReactions *GroundReactions; + + int runcount; + float trim_elev; + float trim_throttle; + + SGPropertyNode *startup_trim; + SGPropertyNode *trimmed; + SGPropertyNode *pitch_trim; + SGPropertyNode *throttle_trim; + SGPropertyNode *aileron_trim; + SGPropertyNode *rudder_trim; + SGPropertyNode *stall_warning; + + void init_gear(void); + void update_gear(void); + +}; + + +#endif // _JSBSIM_HXX + +