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,17 +452,25 @@ 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 ); - snap_shot(); - sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc); + 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 ); - fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus + fgic->SetLatitudeRadIC( lat_geoc * SGD_RADIANS_TO_DEGREES ); needTrim=true; } @@ -464,46 +478,39 @@ 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 + 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() ); - snap_shot(); - sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc); + 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 ); + fgic->SetLatitudeRadIC( lat_geoc * SGD_RADIANS_TO_DEGREES ); fgic->SetAltitudeFtIC(alt); - fdmex->RunIC(fgic); //loop JSBSim once - 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; } @@ -511,12 +518,9 @@ void FGJSBsim::set_Velocities_Local( double north, double east, double down ){ SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Velocities_Local: " << north << ", " << east << ", " << down ); - snap_shot(); fgic->SetVnorthFpsIC(north); fgic->SetVeastFpsIC(east); fgic->SetVdownFpsIC(down); - fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus needTrim=true; } @@ -524,12 +528,9 @@ 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 ); - snap_shot(); fgic->SetUBodyFpsIC(u); fgic->SetVBodyFpsIC(v); fgic->SetWBodyFpsIC(w); - fdmex->RunIC(fgic); //loop JSBSim once - copy_from_JSBsim(); //update the bus needTrim=true; } @@ -538,12 +539,9 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double 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; } @@ -551,20 +549,14 @@ void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { 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; } 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; } @@ -572,28 +564,21 @@ void FGJSBsim::set_Gamma_vert_rad( double gamma) { 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; } 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(); Atmosphere->SetExPressure(p); if(Atmosphere->External() == true) needTrim=true; @@ -602,7 +587,6 @@ void FGJSBsim::set_Static_pressure(double p) { void FGJSBsim::set_Static_temperature(double T) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Static_temperature: " << T ); - snap_shot(); Atmosphere->SetExTemperature(T); if(Atmosphere->External() == true) needTrim=true; @@ -612,7 +596,6 @@ void FGJSBsim::set_Static_temperature(double T) { void FGJSBsim::set_Density(double rho) { SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::set_Density: " << rho ); - snap_shot(); Atmosphere->SetExDensity(rho); if(Atmosphere->External() == true) needTrim=true; @@ -625,6 +608,196 @@ void FGJSBsim::set_Velocities_Local_Airmass (double wnorth, 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); + _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 ); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::update_Longitude(double 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); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::update_Altitude(double alt) { + double sea_level_radius_meters,lat_geoc; + + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_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 ); + fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * SG_METER_TO_FEET ); + fgic->SetLatitudeRadIC( lat_geoc ); + fgic->SetAltitudeFtIC(alt); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +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); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::update_Mach_number(double mach) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Mach_number: " << mach ); + + snap_shot(); + fgic->SetMachIC(mach); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +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(); + fgic->SetVnorthFpsIC(north); + fgic->SetVeastFpsIC(east); + fgic->SetVdownFpsIC(down); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +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(); + fgic->SetUBodyFpsIC(u); + fgic->SetVBodyFpsIC(v); + fgic->SetWBodyFpsIC(w); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +//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(); + fgic->SetPitchAngleRadIC(theta); + fgic->SetRollAngleRadIC(phi); + fgic->SetTrueHeadingRadIC(psi); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +//Flight Path +void FGJSBsim::update_Climb_Rate( double roc) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Climb_Rate: " << roc ); + + snap_shot(); + fgic->SetClimbRateFpsIC(roc); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +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); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +//Earth +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); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::update_Runway_altitude(double ralt) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_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::update_Static_pressure(double p) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Static_pressure: " << p ); + + snap_shot(); + Atmosphere->SetExPressure(p); + if(Atmosphere->External() == true) + needTrim=true; +} + +void FGJSBsim::update_Static_temperature(double T) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Static_temperature: " << T ); + + snap_shot(); + Atmosphere->SetExTemperature(T); + if(Atmosphere->External() == true) + needTrim=true; +} + + +void FGJSBsim::update_Density(double rho) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Density: " << rho ); + + snap_shot(); + Atmosphere->SetExDensity(rho); + if(Atmosphere->External() == true) + needTrim=true; +} + + +void FGJSBsim::update_Velocities_Local_Airmass (double wnorth, + double weast, + double wdown ) { + SG_LOG(SG_FLIGHT,SG_INFO, "FGJSBsim::update_Velocities_Local_Airmass: " + << wnorth << ", " << weast << ", " << wdown ); + _set_Velocities_Local_Airmass( wnorth, weast, wdown ); snap_shot(); Atmosphere->SetWindNED(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);