diff --git a/conf/airframes/jsbsim.xml b/conf/airframes/jsbsim.xml index b8430c233b..a5f1812b4d 100644 --- a/conf/airframes/jsbsim.xml +++ b/conf/airframes/jsbsim.xml @@ -213,6 +213,8 @@ CONFIG = \"tiny_2_1.h\" +MY_JSBSIM_ROOT = /home/violato/enac/programs/JSBSim +MY_JSBSIM_LIB = /home/violato/enac/programs/install_jsbsim include $(PAPARAZZI_SRC)/conf/autopilot/tiny.makefile @@ -224,11 +226,11 @@ sim.ARCH = sitl sim.TARGET = sim sim.TARGETDIR = sim -sim.CFLAGS += -I$(SIMDIR) -I/home/cocoleon/usr/include/JSBSim -I/usr/include +sim.CFLAGS += -I$(SIMDIR) -I/usr/local/include -I$(MY_JSBSIM_LIB)/include/JSBSim sim.CFLAGS += `pkg-config glib-2.0 --cflags` -I /usr/include/meschach -sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy -L/home/cocoleon/usr/lib/ -lJSBSim +sim.LDFLAGS += `pkg-config glib-2.0 --libs` -lm -lmeschach -lpcre -lglibivy -L$(MY_JSBSIM_LIB)/lib -lJSBSim -sim.CFLAGS += -DJSBSIM_ROOT_DIR=\"/home/cocoleon/dev/paparazzi3/conf/simulator/JSBSim/\" +sim.CFLAGS += -DJSBSIM_ROOT_DIR=\"/home/violato/enac/programs/JSBSim/\" sim.CFLAGS += -DCONFIG=\"tiny.h\" -DAGR_CLIMB -DLOITER_TRIM -DALT_KALMAN @@ -238,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.cpp $(SIMDIR)/sim_ac_fw.cpp +sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index e386e5668f..fd063dbe04 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -87,24 +87,23 @@ clean : # # NPS # -JSBSIM = /home/violato/JSBSim +JSBSIM = /usr/local CC = g++ -CFLAGS = -Wall -I $(JSBSIM)/src -I../include +CFLAGS = -Wall -I $(JSBSIM)/include/JSBSim -I../include LDFLAGS = -L $(JSBSIM)/lib -lJSBSim -CFLAGS += -I /usr/include/meschach -LDFLAGS += -lmeschach +CFLAGS += -I /usr/include/meschach -I /usr/local/include/ +LDFLAGS += -lmeschach -L /usr/lib 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) -test2: nps_test2.c nps_jsbsim.c +test2: nps_test2.c $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS) - # # Dependencies # diff --git a/sw/simulator/nps_fdm.h b/sw/simulator/nps_fdm.h index cb727869ba..d56e7d8d0b 100644 --- a/sw/simulator/nps_fdm.h +++ b/sw/simulator/nps_fdm.h @@ -1,21 +1,26 @@ #ifndef NPS_FDM #define NPS_FDM + #include "std.h" -#include +#include /* Unit Conversion Constants */ #define FT2M 0.3048 -/* Vehicle specific descriptions */ +/********************************************/ +/* Vehicle specific descriptions */ +/********************************************/ +/* Dummy test vehicle (mass with vertical force) */ struct NpsDummy { /* throttle for the control force */ double f_input; }; +/* Quadrotor */ #define NPS_QUAD_MOTOR_FRONT 0 #define NPS_QUAD_MOTOR_BACK 1 #define NPS_QUAD_MOTOR_RIGHT 2 @@ -32,6 +37,7 @@ struct NpsQuad { }; +/* Fixed Wing Airplane */ #define NPS_SFW_ACTUATOR_THROTTLE 0 #define NPS_SFW_ACTUATOR_AILERON_R 1 #define NPS_SFW_ACTUATOR_AILERON_L 2 @@ -48,10 +54,14 @@ struct NpsSFW { }; +/********************************************/ +/* State Structure */ +/********************************************/ struct NpsFdmState { - // generic vehicle state + // part of state + // valid for any kind of vehicle bool_t on_ground; VEC* ecef_pos; @@ -71,13 +81,4 @@ struct NpsFdmState { }; - - - - - - - - - #endif /* NPS_FDM */ diff --git a/sw/simulator/nps_jsbsim.c b/sw/simulator/nps_jsbsim.c index 770808f2e2..0b22f8370c 100644 --- a/sw/simulator/nps_jsbsim.c +++ b/sw/simulator/nps_jsbsim.c @@ -5,12 +5,9 @@ #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); } } @@ -29,7 +26,7 @@ int JSBInit(double sim_dt) { fdmex->DisableOutput(); fdmex->SetDebugLevel(0); - JSBSim::FGState State (fdmex); + JSBSim::FGState State(fdmex); State.Setdt(sim_dt); if ( ! fdmex->LoadModel( RootDir + "aircraft", @@ -37,7 +34,7 @@ int JSBInit(double sim_dt) { RootDir + "systems", AircraftName, 1)) { - cerr << " JSBSim could not be started" << endl << endl; + cerr << "JSBSim could not load model" << endl << endl; delete fdmex; exit(-1); } @@ -45,7 +42,7 @@ int JSBInit(double sim_dt) { JSBSim::FGInitialCondition* IC = fdmex->GetIC(); if ( ! IC->Load(ResetName)) { delete fdmex; - cerr << "Initialization unsuccessful" << endl; + cerr << "JSBSim could not initialize state" << endl; exit(-1); } diff --git a/sw/simulator/nps_test1.c b/sw/simulator/nps_test1.c index e0cdc447f1..a7c4561032 100644 --- a/sw/simulator/nps_test1.c +++ b/sw/simulator/nps_test1.c @@ -11,7 +11,7 @@ //#include "nps_fdm.h" //#include "nps_jsbsim.h" -string RootDir = "/home/violato/JSBSim/"; +string RootDir = "/home/violato/enac/programs/JSBSim/"; string AircraftName = "quad"; string ResetName = "reset00"; @@ -61,7 +61,7 @@ int main(int argc, char** argv) { result = fdmex->RunIC(); - IvyInit ("nps_test2", "nps_test2 READY", NULL, NULL, NULL, NULL); + IvyInit ("nps_test1", "nps_test1 READY", NULL, NULL, NULL, NULL); IvyStart("127.255.255.255"); GMainLoop *ml = g_main_loop_new(NULL, FALSE); diff --git a/sw/simulator/nps_test2.c b/sw/simulator/nps_test2.c index 6ec2939da2..872992fe45 100644 --- a/sw/simulator/nps_test2.c +++ b/sw/simulator/nps_test2.c @@ -4,30 +4,56 @@ #include #include +//#include #include "nps_fdm.h" -#include "nps_jsbsim.h" - -string RootDir = "/home/violato/JSBSim/"; -string AircraftName = "quad"; -string ResetName = "reset00"; -static struct NpsFdmState fdm_state; +//#include "nps_jsbsim.h" +//string RootDir = "/home/violato/enac/programs/JSBSim/"; +//string AircraftName = "quad"; +//string ResetName = "reset00"; +//static struct NpsFdmState fdm_state; /* rate of the host mainloop */ #define HOST_TIMEOUT_PERIOD 4 -struct timeval host_time_start; double host_time_elapsed; double host_time_factor = 1.; +struct timeval host_time_start; -/* 250Hz <-> 4ms */ #define SIM_DT (1./10.) double sim_time; +#define AC_ID 999 + +static void sim_run_one_step(void) { + + static int n=0; + + VEC* a; + a = v_get(3); + n = a->dim; + printf("%d\n",n); + + // run autopilot + // feed inputs + // get new state + + + //for ( int i=0; i<3; i++) { fdm_state.ecef_vel->ve[i] = n + i; } + + // throw msgs in Ivy + //IvySendMsg("%d BOOZ_SIM_SPEED_POS %f, %f, %f", AC_ID, + // fdm_state.ecef_vel->ve[0], + // fdm_state.ecef_vel->ve[1], + // fdm_state.ecef_vel->ve[2]); + n++; + +} static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { struct timeval host_time_now; + gettimeofday (&host_time_now, NULL); host_time_elapsed = host_time_factor * ((host_time_now.tv_sec - host_time_start.tv_sec) + @@ -41,32 +67,11 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { return TRUE; } -static void sim_run_one_step(void) { - - // run autopilot - nps_jsbsim_feed_inputs(fdmex, &fdm_state); - result = fdmex->Run(); - nps_jsbsim_fetch_state(fdmex, &fdm_state); - - // balancer les messages - - // IvySendMsg("%d BOOZ_SIM_SPEED_POS %f %f %f %f %f %f", - // AC_ID, - // (fdm_state.speed_ltp->ve[AXIS_X]), - // (bfm.speed_ltp->ve[AXIS_Y]), - // (bfm.speed_ltp->ve[AXIS_Z]), - // (bfm.pos_ltp->ve[AXIS_X]), - // (bfm.pos_ltp->ve[AXIS_Y]), - // (bfm.pos_ltp->ve[AXIS_Z])); - - -} - static void sim_init(void) { /* Setting JSBSim */ - if (~JSBInit(SIM_DT)) {/* message d'erreur */} + //if (~JSBInit(SIM_DT)) {/* message d'erreur */} /* Setting Ivy */ @@ -82,6 +87,7 @@ int main ( int argc, char** argv) { GMainLoop *ml = g_main_loop_new(NULL, FALSE); + sim_init(); g_timeout_add(HOST_TIMEOUT_PERIOD, sim_periodic, NULL); g_main_loop_run(ml); diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c index fb5d194fc1..350004e47e 100644 --- a/sw/simulator/sim_ac_fw.c +++ b/sw/simulator/sim_ac_fw.c @@ -22,12 +22,12 @@ * */ -#include "sim_ac_jsbsim.hpp" - - +#include "sim_ac_jsbsim.h" #include "main_ap.h" #include "main_fbw.h" +using namespace JSBSim; + //static void sim_gps_feed_data(void); //static void sim_ir_feed_data(void); @@ -47,12 +47,290 @@ void autopilot_event_task(void) { } -void copy_inputs_to_jsbsim(JSBSim::FGFDMExec & FDMExec) { +void copy_inputs_to_jsbsim(JSBSim::FGFDMExec* FDMExec) { + + FGPropagate* Propagate; + + Propagate = FDMExec->GetPropagate(); + + cout << "Input " << Propagate->GetEuler(FGJSBBase::ePhi) << endl; + cout << "Input " << Propagate->GetEuler(FGJSBBase::eTht) << endl; + cout << "Input " << Propagate->GetEuler(FGJSBBase::ePsi) << endl; + } -void copy_outputs_from_jsbsim(JSBSim::FGFDMExec & FDMExec) { +void copy_outputs_from_jsbsim(JSBSim::FGFDMExec* FDMExec) { + + + FGPropagate* Propagate; + + Propagate = FDMExec->GetPropagate(); + + cout << "Output " << Propagate->GetEuler(FGJSBBase::ePhi) << endl; + cout << "Output " << Propagate->GetEuler(FGJSBBase::eTht) << endl; + cout << "Output " << Propagate->GetEuler(FGJSBBase::ePsi) << endl; + } +/* +// Convert from the JSBsim generic_ struct to the FGInterface struct + +bool FGJSBsim::copy_from_JSBsim() +{ + unsigned int i, j; + + _set_Inertias( MassBalance->GetMass(), + MassBalance->GetIxx(), + MassBalance->GetIyy(), + MassBalance->GetIzz(), + MassBalance->GetIxz() ); + + _set_CG_Position( MassBalance->GetXYZcg(1), + MassBalance->GetXYZcg(2), + MassBalance->GetXYZcg(3) ); + + _set_Accels_Body( Aircraft->GetBodyAccel(1), + Aircraft->GetBodyAccel(2), + Aircraft->GetBodyAccel(3) ); + + _set_Accels_CG_Body_N ( Aircraft->GetNcg(1), + Aircraft->GetNcg(2), + Aircraft->GetNcg(3) ); + + _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1), + Auxiliary->GetPilotAccel(2), + Auxiliary->GetPilotAccel(3) ); + + _set_Nlf( Aircraft->GetNlf() ); + + // Velocities + + _set_Velocities_Local( Propagate->GetVel(FGJSBBase::eNorth), + Propagate->GetVel(FGJSBBase::eEast), + Propagate->GetVel(FGJSBBase::eDown) ); + + _set_Velocities_Wind_Body( Propagate->GetUVW(1), + Propagate->GetUVW(2), + Propagate->GetUVW(3) ); + + // Make the HUD work ... + _set_Velocities_Ground( Propagate->GetVel(FGJSBBase::eNorth), + Propagate->GetVel(FGJSBBase::eEast), + -Propagate->GetVel(FGJSBBase::eDown) ); + + _set_V_rel_wind( Auxiliary->GetVt() ); + + _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() ); + + _set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() ); + + _set_V_ground_speed( Auxiliary->GetVground() ); + + _set_Omega_Body( Propagate->GetPQR(FGJSBBase::eP), + Propagate->GetPQR(FGJSBBase::eQ), + Propagate->GetPQR(FGJSBBase::eR) ); + + _set_Euler_Rates( Auxiliary->GetEulerRates(FGJSBBase::ePhi), + Auxiliary->GetEulerRates(FGJSBBase::eTht), + Auxiliary->GetEulerRates(FGJSBBase::ePsi) ); + + _set_Mach_number( Auxiliary->GetMach() ); + + // Positions of Visual Reference Point + FGLocation l = Auxiliary->GetLocationVRP(); + _updateGeocentricPosition( l.GetLatitude(), l.GetLongitude(), + l.GetRadius() - get_Sea_level_radius() ); + + _set_Altitude_AGL( Propagate->GetDistanceAGL() ); + { + double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) }; + double contact[3], d[3], sd, t; + is_valid_m(&t, d, &sd); + get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, d, d, &sd); + double rwrad + = FGColumnVector3( contact[0], contact[1], contact[2] ).Magnitude(); + _set_Runway_altitude( rwrad - get_Sea_level_radius() ); + } + + _set_Euler_Angles( Propagate->GetEuler(FGJSBBase::ePhi), + Propagate->GetEuler(FGJSBBase::eTht), + Propagate->GetEuler(FGJSBBase::ePsi) ); + + _set_Alpha( Auxiliary->Getalpha() ); + _set_Beta( Auxiliary->Getbeta() ); + + + _set_Gamma_vert_rad( Auxiliary->GetGamma() ); + + _set_Earth_position_angle( Inertial->GetEarthPositionAngle() ); + + _set_Climb_Rate( Propagate->Gethdot() ); + + const FGMatrix33& Tl2b = Propagate->GetTl2b(); + for ( i = 1; i <= 3; i++ ) { + for ( j = 1; j <= 3; j++ ) { + _set_T_Local_to_Body( i, j, Tl2b(i,j) ); + } + } + + // Copy the engine values from JSBSim. + for ( i=0; i < Propulsion->GetNumEngines(); i++ ) { + SGPropertyNode * node = fgGetNode("engines/engine", i, true); + char buf[30]; + sprintf(buf, "engines/engine[%d]/thruster", i); + SGPropertyNode * tnode = fgGetNode(buf, true); + FGThruster * thruster = Propulsion->GetEngine(i)->GetThruster(); + + switch (Propulsion->GetEngine(i)->GetType()) { + case FGEngine::etPiston: + { // FGPiston code block + FGPiston* eng = (FGPiston*)Propulsion->GetEngine(i); + node->setDoubleValue("egt-degf", eng->getExhaustGasTemp_degF()); + node->setDoubleValue("oil-temperature-degf", eng->getOilTemp_degF()); + node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); + node->setDoubleValue("mp-osi", eng->getManifoldPressure_inHg()); + // NOTE: mp-osi is not in ounces per square inch. + // This error is left for reasons of backwards compatibility with + // existing FlightGear sound and instrument configurations. + node->setDoubleValue("mp-inhg", eng->getManifoldPressure_inHg()); + node->setDoubleValue("cht-degf", eng->getCylinderHeadTemp_degF()); + node->setDoubleValue("rpm", eng->getRPM()); + } // end FGPiston code block + break; + case FGEngine::etRocket: + { // FGRocket code block + FGRocket* eng = (FGRocket*)Propulsion->GetEngine(i); + } // end FGRocket code block + break; + case FGEngine::etTurbine: + { // FGTurbine code block + FGTurbine* eng = (FGTurbine*)Propulsion->GetEngine(i); + node->setDoubleValue("n1", eng->GetN1()); + node->setDoubleValue("n2", eng->GetN2()); + node->setDoubleValue("egt_degf", 32 + eng->GetEGT()*9/5); + node->setBoolValue("augmentation", eng->GetAugmentation()); + node->setBoolValue("water-injection", eng->GetInjection()); + node->setBoolValue("ignition", eng->GetIgnition()); + node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle()); + node->setDoubleValue("inlet-pos-norm", eng->GetInlet()); + node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); + node->setBoolValue("reversed", eng->GetReversed()); + node->setBoolValue("cutoff", eng->GetCutoff()); + node->setDoubleValue("epr", eng->GetEPR()); + globals->get_controls()->set_reverser(i, eng->GetReversed() ); + globals->get_controls()->set_cutoff(i, eng->GetCutoff() ); + globals->get_controls()->set_water_injection(i, eng->GetInjection() ); + globals->get_controls()->set_augmentation(i, eng->GetAugmentation() ); + } // end FGTurbine code block + break; + case FGEngine::etTurboprop: + { // FGTurboProp code block + FGTurboProp* eng = (FGTurboProp*)Propulsion->GetEngine(i); + node->setDoubleValue("n1", eng->GetN1()); + //node->setDoubleValue("n2", eng->GetN2()); + node->setDoubleValue("itt_degf", 32 + eng->GetITT()*9/5); + node->setBoolValue("ignition", eng->GetIgnition()); + node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle()); + node->setDoubleValue("inlet-pos-norm", eng->GetInlet()); + node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); + node->setBoolValue("reversed", eng->GetReversed()); + node->setBoolValue("cutoff", eng->GetCutoff()); + node->setBoolValue("starting", eng->GetEngStarting()); + node->setBoolValue("generator-power", eng->GetGeneratorPower()); + node->setBoolValue("damaged", eng->GetCondition()); + node->setBoolValue("ielu-intervent", eng->GetIeluIntervent()); + node->setDoubleValue("oil-temperature-degf", eng->getOilTemp_degF()); +// node->setBoolValue("onfire", eng->GetFire()); + globals->get_controls()->set_reverser(i, eng->GetReversed() ); + globals->get_controls()->set_cutoff(i, eng->GetCutoff() ); + } // end FGTurboProp code block + break; + case FGEngine::etElectric: + { // FGElectric code block + FGElectric* eng = (FGElectric*)Propulsion->GetEngine(i); + node->setDoubleValue("rpm", eng->getRPM()); + } // end FGElectric code block + break; + } + + { // FGEngine code block + FGEngine* eng = Propulsion->GetEngine(i); + node->setDoubleValue("fuel-flow-gph", eng->getFuelFlow_gph()); + node->setDoubleValue("thrust_lb", thruster->GetThrust()); + node->setDoubleValue("fuel-flow_pph", eng->getFuelFlow_pph()); + node->setBoolValue("running", eng->GetRunning()); + node->setBoolValue("starter", eng->GetStarter()); + node->setBoolValue("cranking", eng->GetCranking()); + globals->get_controls()->set_starter(i, eng->GetStarter() ); + } // end FGEngine code block + + switch (thruster->GetType()) { + case FGThruster::ttNozzle: + { // FGNozzle code block + FGNozzle* noz = (FGNozzle*)thruster; + } // end FGNozzle code block + break; + case FGThruster::ttPropeller: + { // FGPropeller code block + FGPropeller* prop = (FGPropeller*)thruster; + tnode->setDoubleValue("rpm", thruster->GetRPM()); + tnode->setDoubleValue("pitch", prop->GetPitch()); + tnode->setDoubleValue("torque", prop->GetTorque()); + tnode->setBoolValue("feathered", prop->GetFeather()); + } // end FGPropeller code block + break; + case FGThruster::ttRotor: + { // FGRotor code block + FGRotor* rotor = (FGRotor*)thruster; + } // end FGRotor code block + break; + case FGThruster::ttDirect: + { // Direct code block + } // end Direct code block + break; + } + + } + + // Copy the fuel levels from JSBSim if fuel + // freeze not enabled. + if ( ! Propulsion->GetFuelFreeze() ) { + for (i = 0; i < Propulsion->GetNumTanks(); i++) { + SGPropertyNode * node = fgGetNode("/consumables/fuel/tank", i, true); + FGTank* tank = Propulsion->GetTank(i); + double contents = tank->GetContents(); + double temp = tank->GetTemperature_degC(); + node->setDoubleValue("level-gal_us", contents/6.6); + node->setDoubleValue("level-lbs", contents); + if (temp != -9999.0) node->setDoubleValue("temperature_degC", temp); + } + } + + update_gear(); + + stall_warning->setDoubleValue( Aerodynamics->GetStallWarn() ); + + elevator_pos_pct->setDoubleValue( FCS->GetDePos(ofNorm) ); + left_aileron_pos_pct->setDoubleValue( FCS->GetDaLPos(ofNorm) ); + right_aileron_pos_pct->setDoubleValue( FCS->GetDaRPos(ofNorm) ); + rudder_pos_pct->setDoubleValue( -1*FCS->GetDrPos(ofNorm) ); + flap_pos_pct->setDoubleValue( FCS->GetDfPos(ofNorm) ); + speedbrake_pos_pct->setDoubleValue( FCS->GetDsbPos(ofNorm) ); + spoilers_pos_pct->setDoubleValue( FCS->GetDspPos(ofNorm) ); + tailhook_pos_pct->setDoubleValue( FCS->GetTailhookPos() ); + wing_fold_pos_pct->setDoubleValue( FCS->GetWingFoldPos() ); + + // force a sim crashed if crashed (altitude AGL < 0) + if (get_Altitude_AGL() < -100.0) { + State->SuspendIntegration(); + crashed = true; + } + + return true; + } + + */ + //#include "gps.h" //static void sim_gps_feed_data(void) { //} diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index b29b1d60c2..a98dae319d 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -22,7 +22,7 @@ * */ -#include "sim_ac_jsbsim.hpp" +#include "sim_ac_jsbsim.h" #include #include @@ -58,18 +58,20 @@ static void ivy_transport_init(void); static void sim_init(void) { - //sim_time = 0.; - + double actual_elapsed_time = 0; + double cycle_duration = 0.0; + // *** SET UP JSBSIM *** // FDMExec = new JSBSim::FGFDMExec(); + FDMExec->SetAircraftPath(RootDir + "aircraft"); + FDMExec->SetEnginePath(RootDir + "engine"); + FDMExec->SetSystemsPath(RootDir + "systems"); + FDMExec->GetPropertyManager()->Tie("simulation/frame_start_time", &actual_elapsed_time); + FDMExec->GetPropertyManager()->Tie("simulation/cycle_duration", &cycle_duration); + #ifdef JSBSIM_ROOT_DIR RootDir = JSBSIM_ROOT_DIR; #endif - //FDMExec->SetAircraftPath(RootDir + "aircraft"); - //FDMExec->SetEnginePath(RootDir + "engine"); - //FDMExec->SetSystemsPath(RootDir + "systems"); - //FDMExec->GetPropertyManager()->Tie("simulation/frame_start_time", &actual_elapsed_time); - //FDMExec->GetPropertyManager()->Tie("simulation/cycle_duration", &cycle_duration); if (!AircraftName.empty()) { @@ -79,12 +81,26 @@ static void sim_init(void) { if ( ! FDMExec->LoadModel( RootDir + "aircraft", RootDir + "engine", RootDir + "systems", - AircraftName)) { + AircraftName)){ cerr << " JSBSim could not be started" << endl << endl; delete FDMExec; exit(-1); } + if (1) { + FDMExec->PrintPropertyCatalog(); + delete FDMExec; + return 0; + } + + if ( ! FDMExec->LoadModel("quad")) { + cerr << " JSBSim could not be started" << endl << endl; + delete FDMExec; + exit(-1); + } + + cout << "Quad Loaded." << endl << endl; + // Initial conditions (from flight_plan.h and aircraft.h ???) JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); if ( ! IC->Load(ICName)) { @@ -100,6 +116,9 @@ static void sim_init(void) { exit(-1); } + // DEBUG + cout << "So far so good. Init." << endl; + // init sensors ? or discribe them in jSBSim ivy_transport_init(); @@ -113,15 +132,18 @@ static void sim_init(void) { static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { /* read actuators positions and feed JSBSim inputs */ - copy_inputs_to_jsbsim(*FDMExec); + cout <<"Copy Inputs." << endl; + copy_inputs_to_jsbsim(FDMExec); /* run JSBSim flight model */ + cout << "Run model." << endl; bool result = FDMExec->Run(); //sim_time += DT; /* read outputs from model state (and display ?) */ - copy_outputs_from_jsbsim(*FDMExec); + cout <<"Copy Outputs." << endl; + copy_outputs_from_jsbsim(FDMExec); /* run the airborne code */ @@ -139,10 +161,10 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { int main ( int argc, char** argv) { ICName = ""; - AircraftName = ""; + AircraftName = "quad"; LogOutputName = ""; - sim_parse_options(argc, argv); + //sim_parse_options(argc, argv); sim_init(); @@ -180,6 +202,7 @@ static void sim_parse_options(int argc, char** argv) { } int i; + for (i = 1; i < argc; ++i) { string argument = string(argv[i]); diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index 8c34b1621b..d3712c4690 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -22,10 +22,12 @@ * */ -#ifndef SIM_AC_JSBSIM_HPP -#define SIM_AC_JSBSIM_HPP +#ifndef SIM_AC_JSBSIM_H +#define SIM_AC_JSBSIM_H #include +#include +// #include #include "airframe.h" #include "flight_plan.h" @@ -33,8 +35,8 @@ void autopilot_init(void); void autopilot_periodic_task(void); void autopilot_event_task(void); -void copy_inputs_to_jsbsim(JSBSim::FGFDMExec & FDMExec); -void copy_outputs_from_jsbsim(JSBSim::FGFDMExec & FDMExec); +void copy_inputs_to_jsbsim(JSBSim::FGFDMExec* FDMExec); +void copy_outputs_from_jsbsim(JSBSim::FGFDMExec* FDMExec); -#endif // SIM_AC_JSBSIM_HPP +#endif // SIM_AC_JSBSIM_H