static void init_jsbsim(double dt) {
char buf[1024];
string rootdir;
sprintf(buf,"%s/conf/simulator/jsbsim/",getenv("PAPARAZZI_HOME"));
rootdir = string(buf);
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",
AIRFRAME_NAME,
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 ( ! IC->Load(NPS_INITIAL_CONDITITONS)) {
#ifdef DEBUG
cerr << "Initialization unsuccessful" << endl;
#endif
delete FDMExec;
exit(-1);
}
}
/**
* 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;
}
}
int real_main(int argc, char* argv[])
{
// *** INITIALIZATIONS *** //
ScriptName = "";
AircraftName = "";
ResetName = "";
LogOutputName = "";
LogDirectiveName.clear();
bool result = false, success;
bool was_paused = false;
double frame_duration;
double new_five_second_value = 0.0;
double actual_elapsed_time = 0;
double initial_seconds = 0;
double current_seconds = 0.0;
double paused_seconds = 0.0;
double sim_lag_time = 0;
double cycle_duration = 0.0;
double override_sim_rate_value = 0.0;
long sleep_nseconds = 0;
realtime = false;
play_nice = false;
suspend = false;
catalog = false;
nohighlight = false;
// *** PARSE OPTIONS PASSED INTO THIS SPECIFIC APPLICATION: JSBSim *** //
success = options(argc, argv);
if (!success) {
PrintHelp();
exit(-1);
}
// *** SET UP JSBSIM *** //
FDMExec = new JSBSim::FGFDMExec();
FDMExec->SetRootDir(RootDir);
FDMExec->SetAircraftPath("aircraft");
FDMExec->SetEnginePath("engine");
FDMExec->SetSystemsPath("systems");
FDMExec->GetPropertyManager()->Tie("simulation/frame_start_time", &actual_elapsed_time);
FDMExec->GetPropertyManager()->Tie("simulation/cycle_duration", &cycle_duration);
if (nohighlight) FDMExec->disableHighLighting();
if (simulation_rate < 1.0 )
FDMExec->Setdt(simulation_rate);
else
FDMExec->Setdt(1.0/simulation_rate);
if (override_sim_rate) override_sim_rate_value = FDMExec->GetDeltaT();
// *** OPTION A: LOAD A SCRIPT, WHICH LOADS EVERYTHING ELSE *** //
if (!ScriptName.empty()) {
result = FDMExec->LoadScript(ScriptName, override_sim_rate_value);
if (!result) {
cerr << "Script file " << ScriptName << " was not successfully loaded" << endl;
delete FDMExec;
exit(-1);
}
// *** OPTION B: LOAD AN AIRCRAFT AND A SET OF INITIAL CONDITIONS *** //
} else if (!AircraftName.empty() || !ResetName.empty()) {
if (catalog) FDMExec->SetDebugLevel(0);
if ( ! FDMExec->LoadModel( "aircraft",
"engine",
"systems",
AircraftName)) {
cerr << " JSBSim could not be started" << endl << endl;
delete FDMExec;
exit(-1);
}
if (catalog) {
FDMExec->PrintPropertyCatalog();
delete FDMExec;
return 0;
}
JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
if ( ! IC->Load(ResetName)) {
delete FDMExec;
cerr << "Initialization unsuccessful" << endl;
exit(-1);
}
} else {
cout << " No Aircraft, Script, or Reset information given" << endl << endl;
delete FDMExec;
exit(-1);
}
// Load output directives file[s], if given
for (unsigned int i=0; i<LogDirectiveName.size(); i++) {
if (!LogDirectiveName[i].empty()) {
if (!FDMExec->SetOutputDirectives(LogDirectiveName[i])) {
cout << "Output directives not properly set in file " << LogDirectiveName[i] << endl;
delete FDMExec;
exit(-1);
}
}
}
// OVERRIDE OUTPUT FILE NAME. THIS IS USEFUL FOR CASES WHERE MULTIPLE
// RUNS ARE BEING MADE (SUCH AS IN A MONTE CARLO STUDY) AND THE OUTPUT FILE
// NAME MUST BE SET EACH TIME TO AVOID THE PREVIOUS RUN DATA FROM BEING OVER-
// WRITTEN. THIS OVERRIDES ONLY THE FILENAME FOR THE FIRST FILE.
if (!LogOutputName.empty()) {
string old_filename = FDMExec->GetOutputFileName();
if (!FDMExec->SetOutputFileName(LogOutputName)) {
cout << "Output filename could not be set" << endl;
} else {
cout << "Output filename change from " << old_filename << " from aircraft"
" configuration file to " << LogOutputName << " specified on"
" command line" << endl;
}
}
// SET PROPERTY VALUES THAT ARE GIVEN ON THE COMMAND LINE
for (unsigned int i=0; i<CommandLineProperties.size(); i++) {
if (!FDMExec->GetPropertyManager()->GetNode(CommandLineProperties[i])) {
cerr << endl << " No property by the name " << CommandLineProperties[i] << endl;
goto quit;
} else {
FDMExec->SetPropertyValue(CommandLineProperties[i], CommandLinePropertyValues[i]);
}
}
FDMExec->RunIC();
FDMExec->GetPropagate()->DumpState();
cout << endl << JSBSim::FGFDMExec::fggreen << JSBSim::FGFDMExec::highint
<< "---- JSBSim Execution beginning ... --------------------------------------------"
<< JSBSim::FGFDMExec::reset << endl << endl;
result = FDMExec->Run(); // MAKE AN INITIAL RUN
if (suspend) FDMExec->Hold();
// Print actual time at start
char s[100];
time_t tod;
time(&tod);
strftime(s, 99, "%A %B %d %Y %X", localtime(&tod));
cout << "Start: " << s << " (HH:MM:SS)" << endl;
frame_duration = FDMExec->GetDeltaT();
if (realtime) sleep_nseconds = (long)(frame_duration*1e9);
else sleep_nseconds = (10000000); // 0.01 seconds
tzset();
current_seconds = initial_seconds = getcurrentseconds();
// *** CYCLIC EXECUTION LOOP, AND MESSAGE READING *** //
while (result && FDMExec->GetSimTime() <= end_time) {
FDMExec->ProcessMessage(); // Process messages, if any.
// Check if increment then hold is on and take appropriate actions if it is
// Iterate is not supported in realtime - only in batch and playnice modes
FDMExec->CheckIncrementalHold();
// if running realtime, throttle the execution, else just run flat-out fast
// unless "playing nice", in which case sleep for a while (0.01 seconds) each frame.
// If suspended, then don't increment cumulative realtime "stopwatch".
if ( ! FDMExec->Holding()) {
if ( ! realtime ) { // ------------ RUNNING IN BATCH MODE
result = FDMExec->Run();
if (play_nice) sim_nsleep(sleep_nseconds);
} else { // ------------ RUNNING IN REALTIME MODE
// "was_paused" will be true if entering this "run" loop from a paused state.
if (was_paused) {
initial_seconds += paused_seconds;
was_paused = false;
}
current_seconds = getcurrentseconds(); // Seconds since 1 Jan 1970
actual_elapsed_time = current_seconds - initial_seconds; // Real world elapsed seconds since start
sim_lag_time = actual_elapsed_time - FDMExec->GetSimTime(); // How far behind sim-time is from actual
// elapsed time.
for (int i=0; i<(int)(sim_lag_time/frame_duration); i++) { // catch up sim time to actual elapsed time.
result = FDMExec->Run();
cycle_duration = getcurrentseconds() - current_seconds; // Calculate cycle duration
current_seconds = getcurrentseconds(); // Get new current_seconds
if (FDMExec->Holding()) break;
}
if (play_nice) sim_nsleep(sleep_nseconds);
if (FDMExec->GetSimTime() >= new_five_second_value) { // Print out elapsed time every five seconds.
cout << "Simulation elapsed time: " << FDMExec->GetSimTime() << endl;
new_five_second_value += 5.0;
}
}
} else { // Suspended
was_paused = true;
paused_seconds = getcurrentseconds() - current_seconds;
sim_nsleep(sleep_nseconds);
result = FDMExec->Run();
}
}
quit:
// PRINT ENDING CLOCK TIME
time(&tod);
strftime(s, 99, "%A %B %d %Y %X", localtime(&tod));
cout << "End: " << s << " (HH:MM:SS)" << endl;
// CLEAN UP
delete FDMExec;
return 0;
}
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);
//}
}
//------------------------------------------------------------------------------
// reset() --
//------------------------------------------------------------------------------
void JSBSimModel::reset()
{
BaseClass::reset();
pitchTrimPos = (LCreal)0.0;
pitchTrimRate = (LCreal)0.1;
pitchTrimSw = (LCreal)0.0;
rollTrimPos = (LCreal)0.0;
rollTrimRate = (LCreal)0.1;
rollTrimSw = (LCreal)0.0;
// Get our Player (must have one!)
Simulation::Player* p = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
if (p == 0) return;
// must have strings set
if (rootDir == 0 || model == 0) return;
// Must also have the JSBSim object
if (fdmex == 0) {
// must have a JSBSim property manager
if (propMgr == 0) {
propMgr = new JSBSim::FGPropertyManager();
}
fdmex = new JSBSim::FGFDMExec(propMgr);
std::string RootDir(rootDir->getString());
fdmex->SetAircraftPath(RootDir + "aircraft");
fdmex->SetEnginePath(RootDir + "engine");
fdmex->SetSystemsPath(RootDir + "systems"); // JSBSim-1.0 or after only
fdmex->LoadModel(model->getString());
JSBSim::FGPropertyManager* propMgr = fdmex->GetPropertyManager();
if (propMgr != 0) {
hasHeadingHold = propMgr->HasNode("ap/heading_hold") && propMgr->HasNode("ap/heading_setpoint");
hasVelocityHold = propMgr->HasNode("ap/airspeed_hold") && propMgr->HasNode("ap/airspeed_setpoint");
hasAltitudeHold = propMgr->HasNode("ap/altitude_hold") && propMgr->HasNode("ap/altitude_setpoint");
#if 0
// CGB this isn't working for some reason. I set the values directly in "dynamics" for now.
if (hasHeadingHold) {
propMgr->Tie("ap/heading_hold", this, &JSBSimModel::isHeadingHoldOn);
propMgr->Tie("ap/heading_setpoint", this, &JSBSimModel::getCommandedHeadingD);
}
if (hasVelocityHold) {
propMgr->Tie("ap/airspeed_hold", this, &JSBSimModel::isVelocityHoldOn);
propMgr->Tie("ap/airspeed_setpoint", this, &JSBSimModel::getCommandedVelocityKts);
}
if (hasAltitudeHold) {
propMgr->Tie("ap/altitude_hold", this, &JSBSimModel::isAltitudeHoldOn);
propMgr->Tie("ap/altitude_setpoint", this, &JSBSimModel::getCommandedAltitude * Basic::Distance::M2FT);
}
#endif
}
}
#if 0
// CGB TBD
reset = 0;
freeze = 0;
#endif
JSBSim::FGInitialCondition* fgic = fdmex->GetIC();
if (fgic == 0) return;
fgic->SetAltitudeASLFtIC(Basic::Distance::M2FT * p->getAltitude());
#if 0
fgic->SetTrueHeadingDegIC(Basic::Angle::R2DCC * p->getHeading());
fgic->SetRollAngleDegIC(Basic::Angle::R2DCC * p->getRoll());
fgic->SetPitchAngleDegIC(Basic::Angle::R2DCC * p->getPitch());
#else
fgic->SetPsiDegIC(Basic::Angle::R2DCC * p->getHeading());
fgic->SetPhiDegIC(Basic::Angle::R2DCC * p->getRoll());
fgic->SetThetaDegIC(Basic::Angle::R2DCC * p->getPitch());
#endif
fgic->SetVtrueKtsIC(Basic::Distance::M2NM * p->getTotalVelocity() * 3600.0f);
fgic->SetLatitudeDegIC(p->getInitLatitude());
fgic->SetLongitudeDegIC(p->getInitLongitude());
JSBSim::FGPropulsion* Propulsion = fdmex->GetPropulsion();
JSBSim::FGFCS* FCS = fdmex->GetFCS();
if (Propulsion != 0 && FCS != 0) {
Propulsion->SetMagnetos(3);
for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) {
FCS->SetMixtureCmd(i, 1.0);
FCS->SetThrottleCmd(i, 1.0);
FCS->SetPropAdvanceCmd(i, 1.0);
FCS->SetMixturePos(i, 1.0);
FCS->SetThrottlePos(i, 1.0);
FCS->SetPropAdvance(i, 1.0);
JSBSim::FGEngine* eng = Propulsion->GetEngine(i);
eng->SetRunning(true);
JSBSim::FGThruster* thruster = eng->GetThruster();
thruster->SetRPM(1000.0);
}
Propulsion->SetFuelFreeze(p->isFuelFrozen());
Propulsion->InitRunning(-1); // -1 refers to "All Engines"
Propulsion->GetSteadyState();
}
fdmex->RunIC();
}
