diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index f4cb0e5484..9ee10eb74a 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -93,8 +93,8 @@ CFLAGS = -Wall -I $(JSBSIM)/src -I../include LDFLAGS = -L $(JSBSIM)/lib -lJSBSim CFLAGS += -I /usr/include/meschach LDFLAGS += -lmeschach -#CFLAGS += `pkg-config glib-2.0 --cflags` -#LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy +CFLAGS += `pkg-config glib-2.0 --cflags` +LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lpcre -lglibivy test1: nps_test1.c nps_jsbsim.c $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) diff --git a/sw/simulator/nps_jsbsim.c b/sw/simulator/nps_jsbsim.c new file mode 100644 index 0000000000..770808f2e2 --- /dev/null +++ b/sw/simulator/nps_jsbsim.c @@ -0,0 +1,98 @@ +#include "nps_jsbsim.h" + +#include +#include +#include +#include + + + +static void cp_fgvec_to_vec(JSBSim::FGColumnVector3 jsbvec, VEC* vec); +static void cp_fgquat_to_quat(JSBSim::FGQuaternion jsbquat, VEC* quat); + + +static void cp_fgvec_to_vec(JSBSim::FGColumnVector3 jsbvec, VEC* vec) { + while(int i=0 < 3) {vec->ve[i] = jsbvec.Entry(i); } +} + +static void cp_fgquat_to_quat(JSBSim::FGQuaternion jsbquat, VEC* quat) { + while(int i=0 < 4) {quat->ve[i] = jsbquat.Entry(i); } +} + +int JSBInit(double sim_dt) { + + bool result = false; + + JSBSim::FGFDMExec* fdmex; + fdmex = new JSBSim::FGFDMExec(); + + fdmex->DisableOutput(); + fdmex->SetDebugLevel(0); + + JSBSim::FGState State (fdmex); + State.Setdt(sim_dt); + + if ( ! fdmex->LoadModel( RootDir + "aircraft", + RootDir + "engine", + RootDir + "systems", + AircraftName, + 1)) { + cerr << " JSBSim could not be started" << endl << endl; + delete fdmex; + exit(-1); + } + + JSBSim::FGInitialCondition* IC = fdmex->GetIC(); + if ( ! IC->Load(ResetName)) { + delete fdmex; + cerr << "Initialization unsuccessful" << endl; + exit(-1); + } + + result = fdmex->RunIC(); + return(result); + +} + +void nps_jsbsim_feed_inputs(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state) { + + fdmex->SetPropertyValue("/fdm/jsbsim/fcs/control_force", fdm_state->vehicle.dummy.f_input); + +} + +void nps_jsbsim_fetch_state(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state) { + + double radius; + double lon; + double lat; + + radius = FT2M*(fdmex->GetPropagate()->GetRadius()); // meters + lon = fdmex->GetPropagate()->GetLongitude(); // radians + lat = fdmex->GetPropagate()->GetLatitude(); // radians + + JSBSim::FGColumnVector3 ecef_pos (radius*cos(lon)*cos(lat), + radius*sin(lon)*cos(lat),radius*sin(lat)); + + JSBSim::FGColumnVector3 bdy_vel; + JSBSim::FGColumnVector3 bdy_accel; + JSBSim::FGMatrix33 Tb2ec; + JSBSim::FGColumnVector3 ecef_vel; + JSBSim::FGColumnVector3 ecef_accel; + + bdy_vel = fdmex->GetPropagate()->GetUVW(); + bdy_accel = fdmex->GetPropagate()->GetUVWdot(); + Tb2ec = fdmex->GetPropagate()->GetTb2ec(); + + ecef_vel = (Tb2ec*bdy_vel)*FT2M; + ecef_accel = (Tb2ec*bdy_accel)*FT2M; + + fdm_state->on_ground = fdmex->GetPropertyValue("gear/unit[0]/WOW"); + + cp_fgvec_to_vec(ecef_pos, fdm_state->ecef_pos); + cp_fgvec_to_vec(ecef_vel, fdm_state->ecef_vel); + cp_fgvec_to_vec(ecef_accel, fdm_state->ecef_accel); + + cp_fgquat_to_quat(fdmex->GetPropagate()->GetQuaternion(), fdm_state->ltp_to_body_quat); + cp_fgvec_to_vec(fdmex->GetPropagate()->GetPQR(), fdm_state->ltp_body_rate); + cp_fgvec_to_vec(fdmex->GetPropagate()->GetPQRdot(), fdm_state->ltp_body_accel); +} diff --git a/sw/simulator/nps_jsbsim.h b/sw/simulator/nps_jsbsim.h new file mode 100644 index 0000000000..2cb935e962 --- /dev/null +++ b/sw/simulator/nps_jsbsim.h @@ -0,0 +1,12 @@ +#ifndef NPS_JSBSIM_H +#define NPS_JSBSIM_H + +#include +#include "nps_fdm.h" + +extern void nps_jsbsim_feed_inputs(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state); +extern void nps_jsbsim_fetch_state(JSBSim::FGFDMExec* fdmex, struct NpsFdmState* fdm_state); +extern int JSBInit(double sim_dt); + +#endif /* NPS_JSBSIM_H */ + diff --git a/sw/simulator/nps_test2.c b/sw/simulator/nps_test2.c index 214a7f0ff7..b696590f82 100644 --- a/sw/simulator/nps_test2.c +++ b/sw/simulator/nps_test2.c @@ -1,4 +1,4 @@ -#include +#include #include #include @@ -45,7 +45,7 @@ static void sim_run_one_step(void) { result = fdmex->Run(); nps_jsbsim_fetch_state(fdmex, &fdm_state); - // Balancer les messages + // balancer les messages // IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", // AC_ID,