X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FFDM%2FJSBSim.cxx;h=68997a3424f31b35727eae8bcbd45ab1fed87632;hb=407dcaff6236e7217eff16ac253218d3e51bce6d;hp=7216e05945f2a765fa11b6fb93f11a8413f0ed34;hpb=64d6f149e0c82a426d6bf69fc898ea9c35a337a9;p=flightgear.git diff --git a/src/FDM/JSBSim.cxx b/src/FDM/JSBSim.cxx index 7216e0594..68997a342 100644 --- a/src/FDM/JSBSim.cxx +++ b/src/FDM/JSBSim.cxx @@ -31,14 +31,14 @@ #include #include -#include +#include #include #include #include #include -#include
+#include
#include #include @@ -50,268 +50,553 @@ #include #include #include -#include +#include #include #include "JSBSim.hxx" /******************************************************************************/ -// Initialize the JSBsim flight model, dt is the time increment for -// each subsequent iteration through the EOM +FGJSBsim::FGJSBsim(void) { + bool result; + + fdmex=new FGFDMExec; + fgic=new FGInitialCondition(fdmex); + needTrim=true; + + FGPath aircraft_path( globals->get_options()->get_fg_root() ); + aircraft_path.append( "Aircraft" ); + + FGPath engine_path( globals->get_options()->get_fg_root() ); + engine_path.append( "Engine" ); + float dt = 1.0 / globals->get_options()->get_model_hz(); + fdmex->GetState()->Setdt( dt ); + + result = fdmex->LoadModel( aircraft_path.str(), + engine_path.str(), + globals->get_options()->get_aircraft() ); + int Neng=fdmex->GetAircraft()->GetNumEngines(); + FG_LOG(FG_FLIGHT,FG_INFO, "Neng: " << Neng ); + for(int i=0;iSetdt( dt ); - - result = FDMExec.GetAircraft()->LoadAircraft( aircraft_path.str(), - engine_path.str(), - current_options.get_aircraft() ); - - if (result) { - FG_LOG( FG_FLIGHT, FG_INFO, " loaded aircraft" << current_options.get_aircraft() ); - } else { - FG_LOG( FG_FLIGHT, FG_INFO, " aircraft" << current_options.get_aircraft() - << " does not exist"); - return 0; - } - - FDMExec.GetAtmosphere()->UseInternal(); - - FG_LOG( FG_FLIGHT, FG_INFO, " Initializing JSBsim with:" ); - - FGInitialCondition *fgic = new FGInitialCondition(&FDMExec); - fgic->SetAltitudeFtIC(get_Altitude()); - if((current_options.get_mach() < 0) && (current_options.get_vc() < 0 )) { - fgic->SetUBodyFpsIC(current_options.get_uBody()); - fgic->SetVBodyFpsIC(current_options.get_vBody()); - fgic->SetWBodyFpsIC(current_options.get_wBody()); - FG_LOG(FG_FLIGHT,FG_INFO, " u,v,w= " << current_options.get_uBody() - << ", " << current_options.get_vBody() - << ", " << current_options.get_wBody()); - } else if (current_options.get_vc() < 0) { - fgic->SetMachIC(current_options.get_mach()); - FG_LOG(FG_FLIGHT,FG_INFO, " mach: " << current_options.get_mach() ); - } else { - fgic->SetVcalibratedKtsIC(current_options.get_vc()); - FG_LOG(FG_FLIGHT,FG_INFO, " vc: " << current_options.get_vc() ); - //this should cover the case in which no speed switches are used - //current_options.get_vc() will return zero by default - } - - - fgic->SetRollAngleRadIC(get_Phi()); - fgic->SetPitchAngleRadIC(get_Theta()); - fgic->SetHeadingRadIC(get_Psi()); -// fgic->SetLatitudeRadIC(get_Latitude()); - fgic->SetLatitudeRadIC(get_Lat_geocentric()); - fgic->SetLongitudeRadIC(get_Longitude()); - - FDMExec.GetPosition()->SetRunwayRadius(scenery.cur_radius*METER_TO_FEET); - FDMExec.GetPosition()->SetSeaLevelRadius(get_Sea_level_radius()); - - FG_LOG( FG_FLIGHT, FG_INFO, " phi: " << get_Phi()); - FG_LOG( FG_FLIGHT, FG_INFO, " theta: " << get_Theta() ); - FG_LOG( FG_FLIGHT, FG_INFO, " psi: " << get_Psi() ); - FG_LOG( FG_FLIGHT, FG_INFO, " lat: " << get_Latitude() ); - FG_LOG( FG_FLIGHT, FG_INFO, " lon: " << get_Longitude() ); - FG_LOG( FG_FLIGHT, FG_INFO, " alt: " << get_Altitude() ); - - //must check > 0, != 0 will give bad result if --notrim set - if(current_options.get_trim_mode() > 0) { - FDMExec.RunIC(fgic); - FG_LOG( FG_FLIGHT, FG_INFO, " Starting trim..." ); - FGTrimLong *fgtrim=new FGTrimLong(&FDMExec,fgic); - fgtrim->DoTrim(); - fgtrim->Report(); - fgtrim->TrimStats(); - fgtrim->ReportState(); - - controls.set_elevator_trim(FDMExec.GetFCS()->GetPitchTrimCmd()); - controls.set_throttle(FGControls::ALL_ENGINES,FDMExec.GetFCS()->GetThrottleCmd(0)/100); - //the trimming routine only knows how to get 1 value for throttle +} - - delete fgtrim; - FG_LOG( FG_FLIGHT, FG_INFO, " Trim complete." ); - } else { - FG_LOG( FG_FLIGHT, FG_INFO, " Initializing without trim" ); - FDMExec.GetState()->Initialize(fgic); +/******************************************************************************/ +FGJSBsim::~FGJSBsim(void) { + if(fdmex != NULL) { + delete fdmex; + delete fgic; + } +} - } +/******************************************************************************/ - delete fgic; +// Initialize the JSBsim flight model, dt is the time increment for +// each subsequent iteration through the EOM - FG_LOG( FG_FLIGHT, FG_INFO, " loaded initial conditions" ); +bool FGJSBsim::init( double dt ) { - FG_LOG( FG_FLIGHT, FG_INFO, " set dt" ); + bool result; - FG_LOG( FG_FLIGHT, FG_INFO, "Finished initializing JSBsim" ); + FG_LOG( FG_FLIGHT, FG_INFO, "Starting and initializing JSBsim" ); - copy_from_JSBsim(); +#if 0 + FGPath aircraft_path( globals->get_options()->get_fg_root() ); + aircraft_path.append( "Aircraft" ); - return 1; -} + FGPath engine_path( globals->get_options()->get_fg_root() ); + engine_path.append( "Engine" ); -/******************************************************************************/ + fdmex->GetState()->Setdt( dt ); -// Run an iteration of the EOM (equations of motion) + result = fdmex->LoadModel( aircraft_path.str(), + engine_path.str(), + globals->get_options()->get_aircraft() ); +#endif -int FGJSBsim::update( int multiloop ) { - double save_alt = 0.0; - double time_step = (1.0 / current_options.get_model_hz()) * multiloop; - double start_elev = get_Altitude(); + if (result) { + FG_LOG( FG_FLIGHT, FG_INFO, " loaded aircraft " << globals->get_options()->get_aircraft() ); + } else { + FG_LOG( FG_FLIGHT, FG_INFO, " aircraft " + << globals->get_options()->get_aircraft() + << " does not exist" ); + return false; + } + + fdmex->GetAtmosphere()->UseInternal(); + + FG_LOG( FG_FLIGHT, FG_INFO, " Initializing JSBSim with:" ); + switch(fgic->GetSpeedSet()) { + case setned: + FG_LOG(FG_FLIGHT,FG_INFO, " Vn,Ve,Vd= " + << fdmex->GetPosition()->GetVn() + << ", " << fdmex->GetPosition()->GetVe() + << ", " << fdmex->GetPosition()->GetVd() + << " ft/s"); + break; + case setuvw: + FG_LOG(FG_FLIGHT,FG_INFO, " U,V,W= " + << fdmex->GetTranslation()->GetUVW()(1) + << ", " << fdmex->GetTranslation()->GetUVW()(2) + << ", " << fdmex->GetTranslation()->GetUVW()(3) + << " ft/s"); + break; + case setmach: + FG_LOG(FG_FLIGHT,FG_INFO, " Mach: " + << fdmex->GetTranslation()->GetMach() ); + break; + case setvc: + default: + FG_LOG(FG_FLIGHT,FG_INFO, " Indicated Airspeed: " + << fdmex->GetAuxiliary()->GetVcalibratedKTS() << " knots" ); + + } + + //FG_LOG( FG_FLIGHT, FG_INFO, " gamma: " << globals->get_options()->get_Gamma()); + FG_LOG( FG_FLIGHT, FG_INFO, " Bank Angle: " + << fdmex->GetRotation()->Getphi()*RADTODEG << " deg"); + FG_LOG( FG_FLIGHT, FG_INFO, " Pitch Angle: " + << fdmex->GetRotation()->Gettht()*RADTODEG << " deg" ); + FG_LOG( FG_FLIGHT, FG_INFO, " True Heading: " + << fdmex->GetRotation()->Getpsi()*RADTODEG << " deg" ); + FG_LOG( FG_FLIGHT, FG_INFO, " Latitude: " + << fdmex->GetPosition()->GetLatitude() << " deg" ); + FG_LOG( FG_FLIGHT, FG_INFO, " Longitude: " + << fdmex->GetPosition()->GetLongitude() << " deg" ); + + // for debug only + /* FG_LOG( FG_FLIGHT, FG_DEBUG, " FGJSBSim::get_Altitude(): " << get_Altitude() ); + FG_LOG( FG_FLIGHT, FG_DEBUG, " FGJSBSim::get_Sea_level_radius(): " << get_Sea_level_radius() ); + FG_LOG( FG_FLIGHT, FG_DEBUG, " scenery.cur_radius*METER_TO_FEET: " + << scenery.cur_radius*METER_TO_FEET ); + FG_LOG( FG_FLIGHT, FG_DEBUG, " Calculated Terrain ASL: " << endl + << " " << "scenery.cur_radius*METER_TO_FEET -get_Sea_level_radius()= " + << scenery.cur_radius*METER_TO_FEET - get_Sea_level_radius() ); - // lets try to avoid really screwing up the JSBsim model - if ( get_Altitude() < -9000 ) { - save_alt = get_Altitude(); - set_Altitude( 0.0 ); - } + FG_LOG( FG_FLIGHT, FG_DEBUG, " Calculated Aircraft AGL: " << endl + << " " << "get_Altitude() + get_Sea_level_radius() - scenery.cur_radius*METER_TO_FEET= " + << get_Altitude() + get_Sea_level_radius()- scenery.cur_radius*METER_TO_FEET ); + FG_LOG( FG_FLIGHT, FG_DEBUG, " globals->get_options()->get_altitude(): " + << globals->get_options()->get_altitude() ); + FG_LOG( FG_FLIGHT, FG_DEBUG, " FGBFI::getAltitude(): " + << FGBFI::getAltitude() ); */ - // copy control positions into the JSBsim structure - FDMExec.GetFCS()->SetDaCmd( controls.get_aileron()); - FDMExec.GetFCS()->SetDeCmd( controls.get_elevator()); - FDMExec.GetFCS()->SetPitchTrimCmd(controls.get_elevator_trim()); - FDMExec.GetFCS()->SetDrCmd( controls.get_rudder()); - FDMExec.GetFCS()->SetDfCmd( controls.get_flaps() ); - FDMExec.GetFCS()->SetDsbCmd( 0.0 ); - FDMExec.GetFCS()->SetDspCmd( 0.0 ); - FDMExec.GetFCS()->SetThrottleCmd( FGControls::ALL_ENGINES, - controls.get_throttle( 0 ) * 100.0 ); + FG_LOG( FG_FLIGHT, FG_INFO, " loaded initial conditions" ); - // FCS->SetBrake( controls.get_brake( 0 ) ); + FG_LOG( FG_FLIGHT, FG_INFO, " set dt" ); - // Inform JSBsim of the local terrain altitude; uncommented 5/3/00 - // FDMExec.GetPosition()->SetRunwayElevation(get_Runway_altitude()); // seems to work - FDMExec.GetPosition()->SetRunwayRadius(scenery.cur_radius*METER_TO_FEET); - FDMExec.GetPosition()->SetSeaLevelRadius(get_Sea_level_radius()); + FG_LOG( FG_FLIGHT, FG_INFO, "Finished initializing JSBSim" ); - FDMExec.GetAtmosphere()->SetExTemperature(get_Static_temperature()); - FDMExec.GetAtmosphere()->SetExPressure(get_Static_pressure()); - FDMExec.GetAtmosphere()->SetExDensity(get_Density()); - FDMExec.GetAtmosphere()->SetWindNED(get_V_north_airmass(), - get_V_east_airmass(), - get_V_down_airmass()); + return true; +} - for ( int i = 0; i < multiloop; i++ ) { - FDMExec.Run(); - } +/******************************************************************************/ - // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048); - // printf("%d Altitude = %.2f\n", i, Altitude * 0.3048); +// Run an iteration of the EOM (equations of motion) - // translate JSBsim back to FG structure so that the - // autopilot (and the rest of the sim can use the updated values +bool FGJSBsim::update( int multiloop ) { + + int i; + + double save_alt = 0.0; + + + // lets try to avoid really screwing up the JSBsim model + if ( get_Altitude() < -9000 ) { + save_alt = get_Altitude(); + set_Altitude( 0.0 ); + } + + if(needTrim && (globals->get_options()->get_trim_mode() > 0)) { + 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(); + } + fgtrim->ReportState(); + delete fgtrim; + + needTrim=false; + + + controls.set_elevator_trim(fdmex->GetFCS()->GetPitchTrimCmd()); + controls.set_elevator(fdmex->GetFCS()->GetDeCmd()); + controls.set_throttle(FGControls::ALL_ENGINES, + fdmex->GetFCS()->GetThrottleCmd(0)/100.0); + controls.set_aileron(fdmex->GetFCS()->GetDaCmd()); + controls.set_rudder(fdmex->GetFCS()->GetDrCmd()); + + FG_LOG( FG_FLIGHT, FG_INFO, " Trim complete" ); + } + + for( i=0; iset_RPM( controls.get_throttle(i)*2700 ); + get_engine(i)->set_Throttle( controls.get_throttle(i) ); + } + copy_to_JSBsim(); + + for ( int i = 0; i < multiloop; i++ ) { + fdmex->Run(); + } - copy_from_JSBsim(); + // printf("%d FG_Altitude = %.2f\n", i, FG_Altitude * 0.3048); + // printf("%d Altitude = %.2f\n", i, Altitude * 0.3048); - // but lets restore our original bogus altitude when we are done + // translate JSBsim back to FG structure so that the + // autopilot (and the rest of the sim can use the updated values - if ( save_alt < -9000.0 ) { - set_Altitude( save_alt ); - } + copy_from_JSBsim(); + + - double end_elev = get_Altitude(); - //if ( time_step > 0.0 ) { - // feet per second - // set_Climb_Rate( (end_elev - start_elev) / time_step ); - //} + // but lets restore our original bogus altitude when we are done - return 1; + if ( save_alt < -9000.0 ) { + set_Altitude( save_alt ); + } + + //climb rate now set from FDM in copy_from_x() + return true; } /******************************************************************************/ // Convert from the FGInterface struct to the JSBsim generic_ struct -int FGJSBsim::copy_to_JSBsim() { - return 1; +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 ) * 100.0 ); + + fdmex->GetFCS()->SetLBrake( controls.get_brake( 0 ) ); + fdmex->GetFCS()->SetRBrake( controls.get_brake( 1 ) ); + fdmex->GetFCS()->SetCBrake( controls.get_brake( 2 ) ); + + fdmex->GetPosition()->SetRunwayRadius(scenery.cur_radius*METER_TO_FEET); + fdmex->GetPosition()->SetSeaLevelRadius(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()); + + return true; } /******************************************************************************/ // Convert from the JSBsim generic_ struct to the FGInterface struct -int FGJSBsim::copy_from_JSBsim() { - - // Velocities - - set_Velocities_Local( FDMExec.GetPosition()->GetVn(), - FDMExec.GetPosition()->GetVe(), - FDMExec.GetPosition()->GetVd() ); - - set_V_equiv_kts( FDMExec.GetAuxiliary()->GetVequivalentKTS() ); - - //set_V_calibrated( FDMExec.GetAuxiliary()->GetVcalibratedFPS() ); +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) ); + // + _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() ); + + // Velocities - set_V_calibrated_kts( FDMExec.GetAuxiliary()->GetVcalibratedKTS() ); + _set_Velocities_Local( fdmex->GetPosition()->GetVn(), + fdmex->GetPosition()->GetVe(), + fdmex->GetPosition()->GetVd() ); - set_Omega_Body( FDMExec.GetState()->GetParameter(FG_ROLLRATE), - FDMExec.GetState()->GetParameter(FG_PITCHRATE), - FDMExec.GetState()->GetParameter(FG_YAWRATE) ); + _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_Euler_Rates( FDMExec.GetRotation()->Getphi(), - FDMExec.GetRotation()->Gettht(), - FDMExec.GetRotation()->Getpsi() ); + // _set_V_calibrated( fdmex->GetAuxiliary()->GetVcalibratedFPS() ); - // ***FIXME*** set_Geocentric_Rates( Latitude_dot, Longitude_dot, Radius_dot ); + _set_V_calibrated_kts( fdmex->GetAuxiliary()->GetVcalibratedKTS() ); + + _set_V_ground_speed( fdmex->GetPosition()->GetVground() ); - set_Mach_number( FDMExec.GetTranslation()->GetMach()); + _set_Omega_Body( fdmex->GetRotation()->GetPQR()(1), + fdmex->GetRotation()->GetPQR()(2), + fdmex->GetRotation()->GetPQR()(3) ); - // Positions + _set_Euler_Rates( fdmex->GetRotation()->GetEulerRates()(1), + fdmex->GetRotation()->GetEulerRates()(2), + fdmex->GetRotation()->GetEulerRates()(3) ); - double lat_geoc = FDMExec.GetPosition()->GetLatitude(); - double lon = FDMExec.GetPosition()->GetLongitude(); - double alt = FDMExec.GetPosition()->Geth(); - double lat_geod, tmp_alt, sl_radius1, sl_radius2, tmp_lat_geoc; + _set_Geocentric_Rates(fdmex->GetPosition()->GetLatitudeDot(), + fdmex->GetPosition()->GetLongitudeDot(), + fdmex->GetPosition()->Gethdot() ); - fgGeocToGeod( lat_geoc, EQUATORIAL_RADIUS_M + alt * FEET_TO_METER, - &lat_geod, &tmp_alt, &sl_radius1 ); - fgGeodToGeoc( lat_geod, alt * FEET_TO_METER, &sl_radius2, &tmp_lat_geoc ); + _set_Mach_number( fdmex->GetTranslation()->GetMach() ); - FG_LOG( FG_FLIGHT, FG_DEBUG, "lon = " << lon << " lat_geod = " << lat_geod - << " lat_geoc = " << lat_geoc - << " alt = " << alt << " tmp_alt = " << tmp_alt * METER_TO_FEET - << " sl_radius1 = " << sl_radius1 * METER_TO_FEET - << " sl_radius2 = " << sl_radius2 * METER_TO_FEET - << " Equator = " << EQUATORIAL_RADIUS_FT ); + // Positions + _updatePosition( fdmex->GetPosition()->GetLatitude(), + fdmex->GetPosition()->GetLongitude(), + fdmex->GetPosition()->Geth() ); + + _set_Euler_Angles( fdmex->GetRotation()->Getphi(), + fdmex->GetRotation()->Gettht(), + fdmex->GetRotation()->Getpsi() ); - set_Geocentric_Position( lat_geoc, lon, - sl_radius2 * METER_TO_FEET + alt ); - set_Geodetic_Position( lat_geod, lon, alt ); - set_Euler_Angles( FDMExec.GetRotation()->Getphi(), - FDMExec.GetRotation()->Gettht(), - FDMExec.GetRotation()->Getpsi() ); + _set_Alpha( fdmex->GetTranslation()->Getalpha() ); + _set_Beta( fdmex->GetTranslation()->Getbeta() ); - set_Alpha( FDMExec.GetTranslation()->Getalpha() ); - set_Beta( FDMExec.GetTranslation()->Getbeta() ); + + _set_Gamma_vert_rad( fdmex->GetPosition()->GetGamma() ); + // set_Gamma_horiz_rad( Gamma_horiz_rad ); - set_Gamma_vert_rad( FDMExec.GetPosition()->GetGamma() ); - // set_Gamma_horiz_rad( Gamma_horiz_rad ); + _set_Earth_position_angle( fdmex->GetAuxiliary()->GetEarthPositionAngle() ); - /* **FIXME*** */ set_Sea_level_radius( sl_radius2 * METER_TO_FEET ); - /* **FIXME*** */ set_Earth_position_angle( 0.0 ); + _set_Climb_Rate( fdmex->GetPosition()->Gethdot() ); + - // /* ***FIXME*** */ set_Runway_altitude( 0.0 ); + for ( i = 1; i <= 3; i++ ) { + for ( j = 1; j <= 3; j++ ) { + _set_T_Local_to_Body( i, j, fdmex->GetState()->GetTl2b()(i,j) ); + } + } + return true; +} - set_sin_lat_geocentric( lat_geoc ); - set_cos_lat_geocentric( lat_geoc ); - set_sin_cos_longitude( lon ); - set_sin_cos_latitude( lat_geod ); +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() ); +} + + +//Positions +void FGJSBsim::set_Latitude(double lat) { + double sea_level_radius_meters,lat_geoc; + + FG_LOG(FG_FLIGHT,FG_INFO,"FGJSBsim::set_Latitude: " << lat ); + + snap_shot(); + sgGeodToGeoc( lat, get_Altitude() , &sea_level_radius_meters, &lat_geoc); + fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * METER_TO_FEET ); + fgic->SetLatitudeRadIC( lat_geoc ); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::set_Longitude(double lon) { + + FG_LOG(FG_FLIGHT,FG_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; + + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Altitude: " << alt ); + + snap_shot(); + sgGeodToGeoc( get_Latitude(), alt , &sea_level_radius_meters, &lat_geoc); + fgic->SetSeaLevelRadiusFtIC( sea_level_radius_meters * METER_TO_FEET ); + fgic->SetLatitudeRadIC( lat_geoc ); + fgic->SetAltitudeFtIC(alt); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} - set_Climb_Rate(FDMExec.GetPosition()->Gethdot()); +void FGJSBsim::set_V_calibrated_kts(double vc) { + FG_LOG(FG_FLIGHT,FG_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) { + FG_LOG(FG_FLIGHT,FG_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 ){ + FG_LOG(FG_FLIGHT,FG_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; +} + +void FGJSBsim::set_Velocities_Wind_Body( double u, double v, double w){ + FG_LOG(FG_FLIGHT,FG_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; +} + +//Euler angles +void FGJSBsim::set_Euler_Angles( double phi, double theta, double psi ) { + FG_LOG(FG_FLIGHT,FG_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; +} + +//Flight Path +void FGJSBsim::set_Climb_Rate( double roc) { + FG_LOG(FG_FLIGHT,FG_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) { + FG_LOG(FG_FLIGHT,FG_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; +} + +//Earth +void FGJSBsim::set_Sea_level_radius(double slr) { + FG_LOG(FG_FLIGHT,FG_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) { + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Runway_altitude: " << ralt ); + + snap_shot(); + _set_Runway_altitude( ralt ); + fdmex->RunIC(fgic); //loop JSBSim once + copy_from_JSBsim(); //update the bus + needTrim=true; +} + +void FGJSBsim::set_Static_pressure(double p) { + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Static_pressure: " << p ); + + snap_shot(); + fdmex->GetAtmosphere()->SetExPressure(p); + if(fdmex->GetAtmosphere()->External() == true) + needTrim=true; +} + +void FGJSBsim::set_Static_temperature(double T) { + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Static_temperature: " << T ); + + snap_shot(); + fdmex->GetAtmosphere()->SetExTemperature(T); + if(fdmex->GetAtmosphere()->External() == true) + needTrim=true; +} + - return 1; +void FGJSBsim::set_Density(double rho) { + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Density: " << rho ); + + snap_shot(); + fdmex->GetAtmosphere()->SetExDensity(rho); + if(fdmex->GetAtmosphere()->External() == true) + needTrim=true; } + + +void FGJSBsim::set_Velocities_Local_Airmass (double wnorth, + double weast, + double wdown ) { + FG_LOG(FG_FLIGHT,FG_INFO, "FGJSBsim::set_Velocities_Local_Airmass: " + << wnorth << ", " << weast << ", " << wdown ); + + snap_shot(); + fdmex->GetAtmosphere()->SetWindNED(wnorth, weast, wdown ); + if(fdmex->GetAtmosphere()->External() == true) + needTrim=true; +}