/**
* Initializes JSBSim.
*
* Sets up the JSBSim executive and loads initial conditions
* Exits NPS with -1 if models or ICs fail to load
*
* @param dt The desired simulation timestep
*
* @warning Needs PAPARAZZI_HOME defined to find the config files
*/
static void init_jsbsim(double dt) {
char buf[1024];
string rootdir;
string jsbsim_ic_name;
sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME"));
rootdir = string(buf);
/* if jsbsim initial conditions are defined, use them
* otherwise use flightplan location
*/
#ifdef NPS_JSBSIM_INIT
jsbsim_ic_name = NPS_JSBSIM_INIT;
#endif
FDMExec = new FGFDMExec();
FDMExec->Setsim_time(0.);
FDMExec->Setdt(dt);
FDMExec->DisableOutput();
FDMExec->SetDebugLevel(0); // No DEBUG messages
if ( ! FDMExec->LoadModel( rootdir + "aircraft",
rootdir + "engine",
rootdir + "systems",
NPS_JSBSIM_MODEL,
false)){
#ifdef DEBUG
cerr << " JSBSim could not be started" << endl << endl;
#endif
delete FDMExec;
exit(-1);
}
//initRunning for all engines
FDMExec->GetPropulsion()->InitRunning(-1);
JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
if(!jsbsim_ic_name.empty()) {
if ( ! IC->Load(jsbsim_ic_name)) {
#ifdef DEBUG
cerr << "Initialization unsuccessful" << endl;
#endif
delete FDMExec;
exit(-1);
}
}
else {
// FGInitialCondition::SetAltitudeASLFtIC
// requires this function to be called
// before itself
IC->SetVgroundFpsIC(0.);
// Use flight plan initial conditions
// convert geodetic lat from flight plan to geocentric
double gd_lat = RadOfDeg(NAV_LAT0 / 1e7);
double gc_lat = gc_of_gd_lat_d(gd_lat, GROUND_ALT);
IC->SetLatitudeDegIC(DegOfRad(gc_lat));
IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
IC->SetAltitudeASLFtIC(FeetOfMeters(GROUND_ALT + 2.0));
IC->SetTerrainElevationFtIC(FeetOfMeters(GROUND_ALT));
IC->SetPsiDegIC(QFU);
IC->SetVgroundFpsIC(0.);
//initRunning for all engines
FDMExec->GetPropulsion()->InitRunning(-1);
if (!FDMExec->RunIC()) {
cerr << "Initialization from flight plan unsuccessful" << endl;
exit(-1);
}
// compute offset between geocentric and geodetic ecef
struct LlaCoor_d lla0 = { RadOfDeg(NAV_LON0 / 1e7), gd_lat, (double)(NAV_ALT0+NAV_MSL0)/1000. };
ecef_of_lla_d(&offset, &lla0);
struct EcefCoor_d ecef0 = {
MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(1)),
MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(2)),
MetersOfFeet(FDMExec->GetPropagate()->GetLocation().Entry(3))
};
VECT3_DIFF(offset, offset, ecef0);
}
// calculate vehicle max radius in m
vehicle_radius_max = 0.01; // specify not 0.0 in case no gear
int num_gear = FDMExec->GetGroundReactions()->GetNumGearUnits();
int i;
for(i = 0; i < num_gear; i++) {
FGColumnVector3 gear_location = FDMExec->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation();
double radius = MetersOfFeet(gear_location.Magnitude());
if (radius > vehicle_radius_max) vehicle_radius_max = radius;
}
}
void jsbsim_init(void) {
// *** SET UP JSBSIM *** //
char* root = getenv("PAPARAZZI_HOME");
if (root == NULL) {
cerr << "PAPARAZZI_HOME is not defined" << endl;
exit(0);
}
string pprzRoot = string(root);
#ifdef JSBSIM_MODEL
AircraftName = JSBSIM_MODEL;
#endif
#ifdef JSBSIM_INIT
ICName = JSBSIM_INIT;
#endif
FDMExec = new JSBSim::FGFDMExec();
/* Set simulation time step */
FDMExec->Setsim_time(0.);
FDMExec->Setdt(DT);
cout << "Simulation delta " << FDMExec->GetDeltaT() << endl;
FDMExec->DisableOutput();
FDMExec->SetDebugLevel(0); // No DEBUG messages
if (!AircraftName.empty()) {
if ( ! FDMExec->LoadModel( pprzRoot + "/conf/simulator",
pprzRoot + "/conf/simulator",
pprzRoot + "/conf/simulator",
AircraftName)) {
cerr << " JSBSim could not be started" << endl << endl;
delete FDMExec;
exit(-1);
}
JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
if(!ICName.empty()) {
if (!IC->Load(ICName)) {
delete FDMExec;
cerr << "Initialization from file unsuccessful" << endl;
exit(-1);
}
}
else {
// FGInitialCondition::SetAltitudeASLFtIC
// requires this function to be called
// before itself
IC->SetVgroundFpsIC(0.);
// Use flight plan initial conditions
IC->SetLatitudeDegIC(NAV_LAT0 / 1e7);
IC->SetLongitudeDegIC(NAV_LON0 / 1e7);
IC->SetAltitudeASLFtIC((GROUND_ALT + 2.0) / FT2M);
IC->SetTerrainElevationFtIC(GROUND_ALT / FT2M);
IC->SetPsiDegIC(QFU);
IC->SetVgroundFpsIC(0.);
//initRunning for all engines
FDMExec->GetPropulsion()->InitRunning(-1);
if (!FDMExec->RunIC()) {
cerr << "Initialization from flight plan unsuccessful" << endl;
exit(-1);
}
}
//FDMExec->GetGroundReactions()->InitModel();
} else {
cerr << " No Aircraft given" << endl << endl;
delete FDMExec;
exit(-1);
}
//if (FDMExec->Run()) cout << "Made Initial Run" << endl;
//else {
// cerr << "Initial run failed " << endl;
// exit(-1);
//}
}
