diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml index a5f1812b4d..91d0e086c5 100644 --- a/conf/airframes/jsbsim.xml +++ b/conf/airframes/jsbsim.xml @@ -240,7 +240,7 @@ sim.srcs = $(SRC_ARCH)/sim_jsbsim.c $(SRC_ARCH)/ivy_transport.c sim.srcs += latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c sim.srcs += nav_line.c nav_survey_rectangle.c -sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c +sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_booz.c diff --git a/sw/simulator/nps_fdm.h b/sw/simulator/nps_fdm.h index de0a5ea0f2..8f4a48c88b 100644 --- a/sw/simulator/nps_fdm.h +++ b/sw/simulator/nps_fdm.h @@ -12,11 +12,11 @@ struct NpsFdm { VEC* ecef_pos; VEC* ecef_vel; - VEC* ecef_accel; + VEC* body_accel; VEC* ltp_to_body_quat; - VEC* ltp_body_rate; - VEC* ltp_body_accel; + VEC* body_rate; + VEC* body_rate_dot; }; diff --git a/sw/simulator/nps_fdm_jsbsim.c b/sw/simulator/nps_fdm_jsbsim.c index 44fd3cd2bf..055d63198e 100644 --- a/sw/simulator/nps_fdm_jsbsim.c +++ b/sw/simulator/nps_fdm_jsbsim.c @@ -8,15 +8,21 @@ using namespace JSBSim; static void feed_jsbsim(double* commands); static void fetch_state(void); +static void jsbsimvec_to_vec(VEC* vector, const FGColumnVector3* jsb_vector); +static void jsbsimloc_to_vec(VEC* vector, FGLocation* location); +static void jsbsimquat_to_vec(VEC* ltp_to_body_quat, FGQuaternion* quat); +struct NpsFdm fdm; FGFDMExec* FDMExec; void nps_fdm_init(double dt) { #if 0 + char rootdir[1024]; JSBSim::FGState* State; + sprintf(rootdir,"%s/conf/simulator",getenv("PPRZ_HOME")); FDMExec = new FGFDMExec(); State = FDMExec->GetState(); @@ -28,17 +34,17 @@ void nps_fdm_init(double dt) { FDMExec->DisableOutput(); FDMExec->SetDebugLevel(0); // No DEBUG messages - if ( ! FDMExec->LoadModel( RootDir + "aircraft", - RootDir + "engine", - RootDir + "systems", - AircraftName)){ + if ( ! FDMExec->LoadModel( rootdir + "aircraft", + rootdir + "engine", + rootdir + "systems", + AIRFRAME_NAME)){ cerr << " JSBSim could not be started" << endl << endl; delete FDMExec; exit(-1); } JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); - if ( ! IC->Load(ICName)) { + if ( ! IC->Load(NPS_INITIAL_CONDITITONS)) { delete FDMExec; cerr << "Initialization unsuccessful" << endl; exit(-1); @@ -72,7 +78,7 @@ static void feed_jsbsim(double* commands) { char buf[64]; for (int i=0; iGetPropertyManager()->SetDouble(buf,commands[i]); } #endif @@ -81,23 +87,45 @@ static void feed_jsbsim(double* commands) { static void fetch_state(void) { -#if 0 FGGroundReactions* ground_reactions; - FGPropertyManager* cur_node; - double cur_value - - ground_reactions = FGGroundReactions(FDMExec); + FGPropagate* propagate; + FGPropagate::VehicleState* VState; - bool_t fdm.on_ground /* with */ bool ground_reactions->GetWOW(); - VEC* ecef_pos /* with */ bool ground_reactions->GetWOW(); + ground_reactions = FDMExec->GetGroundReactions(); + propagate = FDMExec->GetPropagate(); + VState = propagate->GetVState(); - VEC* ecef_vel; - VEC* ecef_accel; + fdm.on_ground = ground_reactions->GetWOW(); - VEC* ltp_to_body_quat; - VEC* ltp_body_rate; - VEC* ltp_body_accel; -#endif - /* JSBSim to fdm */ + jsbsimloc_to_vec(fdm.ecef_pos,&VState->vLocation); + jsbsimvec_to_vec(fdm.ecef_vel,&VState->vUVW); + + jsbsimvec_to_vec(fdm.body_accel,&propagate->GetUVWdot()); + + jsbsimquat_to_vec(fdm.ltp_to_body_quat,&VState->vQtrn); + jsbsimvec_to_vec(fdm.body_rate,&VState->vPQR); + jsbsimvec_to_vec(fdm.body_rate_dot,&propagate->GetPQRdot()); } + +static void jsbsimloc_to_vec(VEC* vector, FGLocation* location) { + int i; + for (i=0; i<3; i++) { + vector->ve[i] = location->Entry(i+1); + } +} + +static void jsbsimquat_to_vec(VEC* vector, FGQuaternion* quat) { + int i; + for (i=0; i<3; i++) { + vector->ve[i] = quat->Entry(i+1); + } +} + +static void jsbsimvec_to_vec(VEC* vector, const FGColumnVector3* jsb_vector) { + int i; + for (i=0; i<3; i++) { + vector->ve[i] = jsb_vector->Entry(i+1); + } +} +