Пример #1
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;
  }

}
Пример #2
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);
    //}

}