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

}
Beispiel #2
0
/**
 * 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;
  }

}
Beispiel #3
0
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;
}
Beispiel #4
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);
    //}

}
Beispiel #5
0
//------------------------------------------------------------------------------
// 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();
}