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(); }