From c8f21829d4aa7992ca58b3d1501b4e67a1d37527 Mon Sep 17 00:00:00 2001 From: curt Date: Mon, 29 Oct 2001 04:38:25 +0000 Subject: [PATCH] Attempting to simplify JSBSim initialization. --- src/FDM/JSBSim.cxx | 255 +++++++++++++++++++++++++++++++++++++-------- src/FDM/JSBSim.hxx | 96 +++++++++++++++++ src/FDM/flight.cxx | 7 +- 3 files changed, 314 insertions(+), 44 deletions(-) diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index 183e8dac3..0c617a5fe 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -148,7 +148,12 @@ void FGJSBsim::init() { // init method first. FGInterface::init(); - SG_LOG( SG_FLIGHT, SG_INFO, " Initializing JSBSim with:" ); + fdmex->GetState()->Initialize(fgic); + // fdmex->RunIC(fgic); //loop JSBSim once w/o integrating + fdmex->Run(); //loop JSBSim once + copy_from_JSBsim(); //update the bus + + SG_LOG( SG_FLIGHT, SG_INFO, " Initialized JSBSim with:" ); switch(fgic->GetSpeedSet()) { case setned: @@ -175,16 +180,17 @@ void FGJSBsim::init() { } SG_LOG( SG_FLIGHT, SG_INFO, " Bank Angle: " - << Rotation->Getphi()*RADTODEG << " deg"); + << Rotation->Getphi()*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Pitch Angle: " - << Rotation->Gettht()*RADTODEG << " deg" ); + << Rotation->Gettht()*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " True Heading: " - << Rotation->Getpsi()*RADTODEG << " deg" ); + << Rotation->Getpsi()*RADTODEG << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Latitude: " - << Position->GetLatitude() << " deg" ); + << Position->GetLatitude() << " deg" ); SG_LOG( SG_FLIGHT, SG_INFO, " Longitude: " - << Position->GetLongitude() << " deg" ); - + << 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" ); @@ -385,8 +391,8 @@ bool FGJSBsim::copy_from_JSBsim() { _set_Mach_number( Translation->GetMach() ); // Positions - _updatePosition( Position->GetLatitude(), - Position->GetLongitude(), + _updatePosition( Position->GetLatitude() * SGD_DEGREES_TO_RADIANS, + Position->GetLongitude() * SGD_DEGREES_TO_RADIANS, Position->Geth() ); _set_Altitude_AGL( Position->GetDistanceAGL() ); @@ -446,12 +452,178 @@ bool FGJSBsim::ToggleDataLogging(bool state) { //Positions void FGJSBsim::set_Latitude(double lat) { - double sea_level_radius_meters,lat_geoc; + static const SGPropertyNode *altitude + = fgGetNode("/position/altitude-ft"); + double alt; + if ( altitude->getDoubleValue() > -9990 ) { + alt = altitude->getDoubleValue(); + } else { + alt = 0.0; + } + + double sea_level_radius_meters, lat_geoc; 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 ); + fgic->SetLatitudeRadIC( lat_geoc * SGD_RADIANS_TO_DEGREES ); + needTrim=true; +} + +void FGJSBsim::set_Longitude(double lon) { + + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon ); + + fgic->SetLongitudeRadIC(lon * SGD_RADIANS_TO_DEGREES ); + 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() ); + + 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 ); + fgic->SetLatitudeRadIC( lat_geoc * SGD_RADIANS_TO_DEGREES ); + 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 ); + + fgic->SetVcalibratedKtsIC(vc); + needTrim=true; +} + +void FGJSBsim::set_Mach_number(double mach) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); + + 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 ); + + 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 ); + + 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 ); + + 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 ); + + 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 ); + + fgic->SetFlightPathAngleRadIC(gamma); + needTrim=true; +} + +//Earth +void FGJSBsim::set_Sea_level_radius(double slr) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Sea_level_radius: " << slr ); + + fgic->SetSeaLevelRadiusFtIC(slr); + needTrim=true; +} + +void FGJSBsim::set_Runway_altitude(double ralt) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt ); + + _set_Runway_altitude( ralt ); + fgic->SetTerrainAltitudeFtIC( ralt ); + needTrim=true; +} + +void FGJSBsim::set_Static_pressure(double p) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_pressure: " << p ); + + 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 ); + Atmosphere->SetWindNED(wnorth, weast, wdown ); + if(Atmosphere->External() == true) + needTrim=true; +} + + +//Positions +void FGJSBsim::update_Latitude(double lat) { + double sea_level_radius_meters, lat_geoc; + + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::update_Latitude: " << lat ); + SG_LOG(SG_FLIGHT,SG_INFO," cur alt = " << get_Altitude() ); snap_shot(); - sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc); + sgGeodToGeoc( lat, get_Altitude(), &sea_level_radius_meters, &lat_geoc); _set_Sea_level_radius( sea_level_radius_meters * SG_METER_TO_FEET ); fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET ); fgic->SetLatitudeRadIC( lat_geoc ); @@ -460,9 +632,10 @@ void FGJSBsim::set_Latitude(double lat) { needTrim=true; } -void FGJSBsim::set_Longitude(double lon) { +void FGJSBsim::update_Longitude(double lon) { - SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::set_Longitude: " << lon ); + SG_LOG(SG_FLIGHT,SG_INFO,"FGJSBsim::update_Longitude: " << lon ); + SG_LOG(SG_FLIGHT,SG_INFO," cur alt = " << get_Altitude() ); snap_shot(); fgic->SetLongitudeRadIC(lon); @@ -471,10 +644,10 @@ void FGJSBsim::set_Longitude(double lon) { needTrim=true; } -void FGJSBsim::set_Altitude(double alt) { +void FGJSBsim::update_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::update_Altitude: " << alt ); snap_shot(); sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc); @@ -487,8 +660,8 @@ void FGJSBsim::set_Altitude(double alt) { needTrim=true; } -void FGJSBsim::set_V_calibrated_kts(double vc) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_V_calibrated_kts: " << vc ); +void FGJSBsim::update_V_calibrated_kts(double vc) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_V_calibrated_kts: " << vc ); snap_shot(); fgic->SetVcalibratedKtsIC(vc); @@ -497,8 +670,8 @@ void FGJSBsim::set_V_calibrated_kts(double vc) { needTrim=true; } -void FGJSBsim::set_Mach_number(double mach) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Mach_number: " << mach ); +void FGJSBsim::update_Mach_number(double mach) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Mach_number: " << mach ); snap_shot(); fgic->SetMachIC(mach); @@ -507,8 +680,8 @@ void FGJSBsim::set_Mach_number(double mach) { needTrim=true; } -void FGJSBsim::set_Velocities_Local( double north, double east, double down ){ - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " +void FGJSBsim::update_Velocities_Local( double north, double east, double down ){ + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Velocities_Local: " << north << ", " << east << ", " << down ); snap_shot(); @@ -520,8 +693,8 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double 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: " +void FGJSBsim::update_Velocities_Wind_Body( double u, double v, double w){ + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Velocities_Wind_Body: " << u << ", " << v << ", " << w ); snap_shot(); @@ -534,8 +707,8 @@ void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){ } //Euler angles -void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Euler_Angles: " +void FGJSBsim::update_Euler_Angles( double phi, double theta, double psi ) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Euler_Angles: " << phi << ", " << theta << ", " << psi ); snap_shot(); @@ -548,8 +721,8 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { } //Flight Path -void FGJSBsim::set_Climb_Rate( double roc) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Climb_Rate: " << roc ); +void FGJSBsim::update_Climb_Rate( double roc) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Climb_Rate: " << roc ); snap_shot(); fgic->SetClimbRateFpsIC(roc); @@ -558,8 +731,8 @@ void FGJSBsim::set_Climb_Rate( double roc) { needTrim=true; } -void FGJSBsim::set_Gamma_vert_rad( double gamma) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Gamma_vert_rad: " << gamma ); +void FGJSBsim::update_Gamma_vert_rad( double gamma) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Gamma_vert_rad: " << gamma ); snap_shot(); fgic->SetFlightPathAngleRadIC(gamma); @@ -569,8 +742,8 @@ void FGJSBsim::set_Gamma_vert_rad( double gamma) { } //Earth -void FGJSBsim::set_Sea_level_radius(double slr) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Sea_level_radius: " << slr ); +void FGJSBsim::update_Sea_level_radius(double slr) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Sea_level_radius: " << slr ); snap_shot(); fgic->SetSeaLevelRadiusFtIC(slr); @@ -579,8 +752,8 @@ void FGJSBsim::set_Sea_level_radius(double slr) { needTrim=true; } -void FGJSBsim::set_Runway_altitude(double ralt) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt ); +void FGJSBsim::update_Runway_altitude(double ralt) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Runway_altitude: " << ralt ); snap_shot(); _set_Runway_altitude( ralt ); @@ -590,8 +763,8 @@ void FGJSBsim::set_Runway_altitude(double ralt) { needTrim=true; } -void FGJSBsim::set_Static_pressure(double p) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_pressure: " << p ); +void FGJSBsim::update_Static_pressure(double p) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Static_pressure: " << p ); snap_shot(); Atmosphere->SetExPressure(p); @@ -599,8 +772,8 @@ void FGJSBsim::set_Static_pressure(double p) { needTrim=true; } -void FGJSBsim::set_Static_temperature(double T) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_temperature: " << T ); +void FGJSBsim::update_Static_temperature(double T) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Static_temperature: " << T ); snap_shot(); Atmosphere->SetExTemperature(T); @@ -609,8 +782,8 @@ void FGJSBsim::set_Static_temperature(double T) { } -void FGJSBsim::set_Density(double rho) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho ); +void FGJSBsim::update_Density(double rho) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Density: " << rho ); snap_shot(); Atmosphere->SetExDensity(rho); @@ -619,10 +792,10 @@ void FGJSBsim::set_Density(double rho) { } -void FGJSBsim::set_Velocities_Local_Airmass (double wnorth, +void FGJSBsim::update_Velocities_Local_Airmass (double wnorth, double weast, double wdown ) { - SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local_Airmass: " + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Velocities_Local_Airmass: " << wnorth << ", " << weast << ", " << wdown ); _set_Velocities_Local_Airmass( wnorth, weast, wdown ); diff --git a/src/FDM/JSBSim.hxx b/src/FDM/JSBSim.hxx index bb6cdccbb..c3ee32905 100644 --- a/src/FDM/JSBSim.hxx +++ b/src/FDM/JSBSim.hxx @@ -205,6 +205,102 @@ public: void set_Velocities_Local_Airmass (double wnorth, double weast, double wdown ); + /// @name Position Parameter Update + //@{ + /** Update geocentric latitude + @param lat latitude in radians measured from the 0 meridian where + the westerly direction is positive and east is negative */ + void update_Latitude(double lat); // geocentric + + /** Update longitude + @param lon longitude in radians measured from the equator where + the northerly direction is positive and south is negative */ + void update_Longitude(double lon); + + /** Update altitude + Note: this triggers a recalculation of AGL altitude + @param alt altitude in feet */ + void update_Altitude(double alt); // triggers re-calc of AGL altitude + //@} + + //void update_AltitudeAGL(double altagl); // and vice-versa + + /// @name Velocity Parameter Update + //@{ + /** Updates calibrated airspeed + Updateting this will trigger a recalc of the other velocity terms. + @param vc Calibrated airspeed in ft/sec */ + void update_V_calibrated_kts(double vc); + + /** Updates Mach number. + Updateting this will trigger a recalc of the other velocity terms. + @param mach Mach number */ + void update_Mach_number(double mach); + + /** Updates velocity in N-E-D coordinates. + Updateting 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 update_Velocities_Local( double north, double east, double down ); + + /** Updates aircraft velocity in stability frame. + Updateting 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 update_Velocities_Wind_Body( double u, double v, double w); + //@} + + /** Euler Angle Parameter Update + @param phi roll angle in radians + @param theta pitch angle in radians + @param psi heading angle in radians */ + void update_Euler_Angles( double phi, double theta, double psi ); + + /// @name Flight Path Parameter Update + //@{ + /** Updates rate of climb + @param roc Rate of climb in ft/sec */ + void update_Climb_Rate( double roc); + + /** Updates the flight path angle in radians + @param gamma flight path angle in radians. */ + void update_Gamma_vert_rad( double gamma); + //@} + + /// @name Earth Parameter Update + //@{ + /** Updates the sea level radius in feet. + @param slr Sea Level Radius in feet */ + void update_Sea_level_radius(double slr); + + /** Updates the runway altitude in feet above sea level. + @param ralt Runway altitude in feet above sea level. */ + void update_Runway_altitude(double ralt); + //@} + + /// @name Atmospheric Parameter Update + //@{ + /** Updates the atmospheric static pressure + @param p pressure in psf */ + void update_Static_pressure(double p); + + /** Updates the atmospheric temperature + @param T temperature in degrees rankine */ + void update_Static_temperature(double T); + + /** Updates the atmospheric density. + @param rho air density slugs/cubic foot */ + void update_Density(double rho); + + /** Updates 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 update_Velocities_Local_Airmass (double wnorth, + double weast, + double wdown ); //@} /** Update the position based on inputs, positions, velocities, etc. diff --git a/src/FDM/flight.cxx b/src/FDM/flight.cxx index 29c62d05a..2adf802db 100644 --- a/src/FDM/flight.cxx +++ b/src/FDM/flight.cxx @@ -205,11 +205,12 @@ FGInterface::init () // Set sea-level radius SG_LOG(SG_FLIGHT, SG_INFO, "...initializing sea-level radius..."); - SG_LOG(SG_FLIGHT, SG_INFO, " lat = " << get_Latitude() << " alt = " - << get_Altitude() ); + SG_LOG(SG_FLIGHT, SG_INFO, " lat = " << fgGetDouble("/position/latitude-deg") + << " alt = " << fgGetDouble("/position/altitude-ft") ); double sea_level_radius_meters; double lat_geoc; - sgGeodToGeoc(get_Latitude(), get_Altitude(), + sgGeodToGeoc(fgGetDouble("/position/latitude-deg") * SGD_DEGREES_TO_RADIANS, + fgGetDouble("/position/altitude-ft") * SG_FEET_TO_METER, &sea_level_radius_meters, &lat_geoc); set_Sea_level_radius(sea_level_radius_meters * SG_METER_TO_FEET); -- 2.39.5