fginitialcondition.cpp

6 DOF Missle Simulation

//******************************************************************************

bool FGInitialCondition::solve(double *y,double x)
{
  double x1,x2,x3,f1,f2,f3,d,d0;
  double eps=1E-5;
  double const relax =0.9;
  int i;
  bool success=false;

  //initializations
  d=1;
  x2 = 0;
  x1=xlo;x3=xhi;
  f1=(this->*sfunc)(x1)-x;
  f3=(this->*sfunc)(x3)-x;
  d0=fabs(x3-x1);

  //iterations
  i=0;
  while ((fabs(d) > eps) && (i < 100)) {
    d=(x3-x1)/d0;
    x2 = x1-d*d0*f1/(f3-f1);

    f2=(this->*sfunc)(x2)-x;
    //cout << "solve x1,x2,x3: " << x1 << "," << x2 << "," << x3 << endl;
    //cout << "                " << f1 << "," << f2 << "," << f3 << endl;

    if(fabs(f2) <= 0.001) {
      x1=x3=x2;
    } else if(f1*f2 <= 0.0) {
      x3=x2;
      f3=f2;
      f1=relax*f1;
    } else if(f2*f3 <= 0) {
      x1=x2;
      f1=f2;
      f3=relax*f3;
    }
    //cout << i << endl;
    i++;
  }//end while
  if(i < 100) {
    success=true;
    *y=x2;
  }

  //cout << "Success= " << success << " Vcas: " << vcas*fpstokts << " Mach: " << x2 << endl;
  return success;
}

//******************************************************************************

double FGInitialCondition::GetWindDirDegIC(void) const {
  if(weast != 0.0)
    return atan2(weast,wnorth)*radtodeg;
  else if(wnorth > 0)
    return 0.0;
  else
    return 180.0;
}

//******************************************************************************

bool FGInitialCondition::Load(string rstfile, bool useStoredPath)
{
  int n;

  string sep = "/";
  if( useStoredPath ) {
    init_file_name = fdmex->GetFullAircraftPath() + sep + rstfile + ".xml";
  } else {
    init_file_name = rstfile;
  }

  document = LoadXMLDocument(init_file_name);

  // Make sure that the document is valid
  if (!document) {
    cerr << "File: " << init_file_name << " could not be read." << endl;
    exit(-1);
  }

  if (document->GetName() != string("initialize")) {
    cerr << "File: " << init_file_name << " is not a reset file." << endl;
    exit(-1);
  }

  if (document->FindElement("latitude"))
    SetLatitudeDegIC(document->FindElementValueAsNumberConvertTo("latitude", "DEG"));
  if (document->FindElement("longitude"))
    SetLongitudeDegIC(document->FindElementValueAsNumberConvertTo("longitude", "DEG"));
  if (document->FindElement("altitude"))
    SetAltitudeFtIC(document->FindElementValueAsNumberConvertTo("altitude", "FT"));
  if (document->FindElement("ubody"))
    SetUBodyFpsIC(document->FindElementValueAsNumberConvertTo("ubody", "FT/SEC"));
  if (document->FindElement("vbody"))
    SetVBodyFpsIC(document->FindElementValueAsNumberConvertTo("vbody", "FT/SEC"));
  if (document->FindElement("wbody"))
    SetWBodyFpsIC(document->FindElementValueAsNumberConvertTo("wbody", "FT/SEC"));
  if (document->FindElement("vnorth"))
    SetVNorthFpsIC(document->FindElementValueAsNumberConvertTo("vnorth", "FT/SEC"));
  if (document->FindElement("veast"))
    SetVEastFpsIC(document->FindElementValueAsNumberConvertTo("veast", "FT/SEC"));
  if (document->FindElement("vdown"))
    SetVDownFpsIC(document->FindElementValueAsNumberConvertTo("vdown", "FT/SEC"));
  if (document->FindElement("winddir"))
    SetWindDirDegIC(document->FindElementValueAsNumberConvertTo("winddir", "DEG"));
  if (document->FindElement("vwind"))
    SetWindMagKtsIC(document->FindElementValueAsNumberConvertTo("vwind", "KTS"));
  if (document->FindElement("hwind"))
    SetHeadWindKtsIC(document->FindElementValueAsNumberConvertTo("hwind", "KTS"));
  if (document->FindElement("xwind"))
    SetCrossWindKtsIC(document->FindElementValueAsNumberConvertTo("xwind", "KTS"));
  if (document->FindElement("vc"))
    SetVcalibratedKtsIC(document->FindElementValueAsNumberConvertTo("vc", "KTS"));
  if (document->FindElement("vt"))
    SetVtrueKtsIC(document->FindElementValueAsNumberConvertTo("vt", "KTS"));
  if (document->FindElement("mach"))
    SetMachIC(document->FindElementValueAsNumber("mach"));
  if (document->FindElement("phi"))
    SetPhiDegIC(document->FindElementValueAsNumberConvertTo("phi", "DEG"));
  if (document->FindElement("theta"))
    SetThetaDegIC(document->FindElementValueAsNumberConvertTo("theta", "DEG"));
  if (document->FindElement("psi"))
    SetPsiDegIC(document->FindElementValueAsNumberConvertTo("psi", "DEG"));
  if (document->FindElement("alpha"))
    SetAlphaDegIC(document->FindElementValueAsNumberConvertTo("alpha", "DEG"));
  if (document->FindElement("beta"))
    SetBetaDegIC(document->FindElementValueAsNumberConvertTo("beta", "DEG"));
  if (document->FindElement("gamma"))
    SetFlightPathAngleDegIC(document->FindElementValueAsNumberConvertTo("gamma", "DEG"));
  if (document->FindElement("roc"))
    SetClimbRateFpsIC(document->FindElementValueAsNumberConvertTo("roc", "FT/SEC"));
  if (document->FindElement("vground"))
    SetVgroundKtsIC(document->FindElementValueAsNumberConvertTo("vground", "KTS"));
  if (document->FindElement("targetNlf"))
  {
    SetTargetNlfIC(document->FindElementValueAsNumber("targetNlf"));
  }

  // Check to see if any engines are specified to be initialized in a running state
  FGPropulsion* propulsion = fdmex->GetPropulsion();
  Element* running_elements = document->FindElement("running");
  while (running_elements) {
    n = int(running_elements->GetDataAsNumber());
    propulsion->InitRunning(n);
    running_elements = document->FindNextElement("running");
  }

  fdmex->RunIC();

  return true;
}

//******************************************************************************

void FGInitialCondition::bind(void){
  PropertyManager->Tie("ic/vc-kts", this,
                       &FGInitialCondition::GetVcalibratedKtsIC,
                       &FGInitialCondition::SetVcalibratedKtsIC,
                       true);
  PropertyManager->Tie("ic/ve-kts", this,
                       &FGInitialCondition::GetVequivalentKtsIC,
                       &FGInitialCondition::SetVequivalentKtsIC,
                       true);
  PropertyManager->Tie("ic/vg-kts", this,
                       &FGInitialCondition::GetVgroundKtsIC,
                       &FGInitialCondition::SetVgroundKtsIC,
                       true);
  PropertyManager->Tie("ic/vt-kts", this,
                       &FGInitialCondition::GetVtrueKtsIC,
                       &FGInitialCondition::SetVtrueKtsIC,
                       true);
  PropertyManager->Tie("ic/mach", this,
                       &FGInitialCondition::GetMachIC,
                       &FGInitialCondition::SetMachIC,
                       true);
  PropertyManager->Tie("ic/roc-fpm", this,
                       &FGInitialCondition::GetClimbRateFpmIC,
                       &FGInitialCondition::SetClimbRateFpmIC,
                       true);
  PropertyManager->Tie("ic/gamma-deg", this,
                       &FGInitialCondition::GetFlightPathAngleDegIC,
                       &FGInitialCondition::SetFlightPathAngleDegIC,
                       true);
  PropertyManager->Tie("ic/alpha-deg", this,
                       &FGInitialCondition::GetAlphaDegIC,
                       &FGInitialCondition::SetAlphaDegIC,
                       true);
  PropertyManager->Tie("ic/beta-deg", this,
                       &FGInitialCondition::GetBetaDegIC,
                       &FGInitialCondition::SetBetaDegIC,
                       true);
  PropertyManager->Tie("ic/theta-deg", this,
                       &FGInitialCondition::GetThetaDegIC,
                       &FGInitialCondition::SetThetaDegIC,
                       true);
  PropertyManager->Tie("ic/phi-deg", this,
                       &FGInitialCondition::GetPhiDegIC,
                       &FGInitialCondition::SetPhiDegIC,
                       true);
  PropertyManager->Tie("ic/psi-true-deg", this,
                       &FGInitialCondition::GetPsiDegIC );
  PropertyManager->Tie("ic/lat-gc-deg", this,
                       &FGInitialCondition::GetLatitudeDegIC,
                       &FGInitialCondition::SetLatitudeDegIC,
                       true);
  PropertyManager->Tie("ic/long-gc-deg", this,
                       &FGInitialCondition::GetLongitudeDegIC,
                       &FGInitialCondition::SetLongitudeDegIC,
                       true);
  PropertyManager->Tie("ic/h-sl-ft", this,
                       &FGInitialCondition::GetAltitudeFtIC,
                       &FGInitialCondition::SetAltitudeFtIC,
                       true);
  PropertyManager->Tie("ic/h-agl-ft", this,
                       &FGInitialCondition::GetAltitudeAGLFtIC,
                       &FGInitialCondition::SetAltitudeAGLFtIC,
                       true);
  PropertyManager->Tie("ic/sea-level-radius-ft", this,
                       &FGInitialCondition::GetSeaLevelRadiusFtIC,
                       &FGInitialCondition::SetSeaLevelRadiusFtIC,
                       true);
  PropertyManager->Tie("ic/terrain-altitude-ft", this,
                       &FGInitialCondition::GetTerrainAltitudeFtIC,
                       &FGInitialCondition::SetTerrainAltitudeFtIC,
                       true);
  PropertyManager->Tie("ic/vg-fps", this,
                       &FGInitialCondition::GetVgroundFpsIC,
                       &FGInitialCondition::SetVgroundFpsIC,
                       true);
  PropertyManager->Tie("ic/vt-fps", this,
                       &FGInitialCondition::GetVtrueFpsIC,
                       &FGInitialCondition::SetVtrueFpsIC,
                       true);
  PropertyManager->Tie("ic/vw-bx-fps", this,
                       &FGInitialCondition::GetWindUFpsIC);
  PropertyManager->Tie("ic/vw-by-fps", this,
                       &FGInitialCondition::GetWindVFpsIC);
  PropertyManager->Tie("ic/vw-bz-fps", this,
                       &FGInitialCondition::GetWindWFpsIC);
  PropertyManager->Tie("ic/vw-north-fps", this,
                       &FGInitialCondition::GetWindNFpsIC);
  PropertyManager->Tie("ic/vw-east-fps", this,
                       &FGInitialCondition::GetWindEFpsIC);
  PropertyManager->Tie("ic/vw-down-fps", this,
                       &FGInitialCondition::GetWindDFpsIC);
  PropertyManager->Tie("ic/vw-mag-fps", this,
                       &FGInitialCondition::GetWindFpsIC);
  PropertyManager->Tie("ic/vw-dir-deg", this,
                       &FGInitialCondition::GetWindDirDegIC,
                       &FGInitialCondition::SetWindDirDegIC,
                       true);

  PropertyManager->Tie("ic/roc-fps", this,
                       &FGInitialCondition::GetClimbRateFpsIC,
                       &FGInitialCondition::SetClimbRateFpsIC,
                       true);
  PropertyManager->Tie("ic/u-fps", this,
                       &FGInitialCondition::GetUBodyFpsIC,
                       &FGInitialCondition::SetUBodyFpsIC,
                       true);
  PropertyManager->Tie("ic/v-fps", this,
                       &FGInitialCondition::GetVBodyFpsIC,
                       &FGInitialCondition::SetVBodyFpsIC,
                       true);
  PropertyManager->Tie("ic/w-fps", this,
                       &FGInitialCondition::GetWBodyFpsIC,
                       &FGInitialCondition::SetWBodyFpsIC,
                       true);
  PropertyManager->Tie("ic/vn-fps", this,
                       &FGInitialCondition::GetVNorthFpsIC,
                       &FGInitialCondition::SetVNorthFpsIC,
                       true);
  PropertyManager->Tie("ic/ve-fps", this,
                       &FGInitialCondition::GetVEastFpsIC,
                       &FGInitialCondition::SetVEastFpsIC,
                       true);
  PropertyManager->Tie("ic/vd-fps", this,
                       &FGInitialCondition::GetVDownFpsIC,
                       &FGInitialCondition::SetVDownFpsIC,
                       true);
  PropertyManager->Tie("ic/gamma-rad", this,
                       &FGInitialCondition::GetFlightPathAngleRadIC,
                       &FGInitialCondition::SetFlightPathAngleRadIC,
                       true);
  PropertyManager->Tie("ic/alpha-rad", this,
                       &FGInitialCondition::GetAlphaRadIC,
                       &FGInitialCondition::SetAlphaRadIC,
                       true);
  PropertyManager->Tie("ic/theta-rad", this,
                       &FGInitialCondition::GetThetaRadIC,
                       &FGInitialCondition::SetThetaRadIC,
                       true);
  PropertyManager->Tie("ic/beta-rad", this,
                       &FGInitialCondition::GetBetaRadIC,
                       &FGInitialCondition::SetBetaRadIC,
                       true);
  PropertyManager->Tie("ic/phi-rad", this,
                       &FGInitialCondition::GetPhiRadIC,
                       &FGInitialCondition::SetPhiRadIC,
                       true);
  PropertyManager->Tie("ic/psi-true-rad", this,
                       &FGInitialCondition::GetPsiRadIC);
  PropertyManager->Tie("ic/lat-gc-rad", this,
                       &FGInitialCondition::GetLatitudeRadIC,
                       &FGInitialCondition::SetLatitudeRadIC,
                       true);
  PropertyManager->Tie("ic/long-gc-rad", this,
                       &FGInitialCondition::GetLongitudeRadIC,
                       &FGInitialCondition::SetLongitudeRadIC,
                       true);
  PropertyManager->Tie("ic/p-rad_sec", this,
                       &FGInitialCondition::GetPRadpsIC,
                       &FGInitialCondition::SetPRadpsIC,
                       true);
  PropertyManager->Tie("ic/q-rad_sec", this,
                       &FGInitialCondition::GetQRadpsIC,
                       &FGInitialCondition::SetQRadpsIC,
                       true);
  PropertyManager->Tie("ic/r-rad_sec", this,
                       &FGInitialCondition::GetRRadpsIC,
                       &FGInitialCondition::SetRRadpsIC,
                       true);

  typedef int (FGInitialCondition::*iPMF)(void) const;
  PropertyManager->Tie("simulation/write-state-file",
                       this,
                       (iPMF)0,
                       &FGInitialCondition::WriteStateFile);

}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//    The bitmasked value choices are as follows:
//    unset: In this case (the default) JSBSim would only print
//       out the normally expected messages, essentially echoing
//       the config files as they are read. If the environment
//       variable is not set, debug_lvl is set to 1 internally
//    0: This requests JSBSim not to output any messages
//       whatsoever.
//    1: This value explicity requests the normal JSBSim
//       startup messages
//    2: This value asks for a message to be printed out when
//       a class is instantiated
//    4: When this value is set, a message is displayed when a
//       FGModel object executes its Run() method
//    8: When this value is set, various runtime state variables
//       are printed out periodically
//    16: When set various parameters are sanity checked and
//       a message is printed out when they go out of bounds

void FGInitialCondition::Debug(int from)
{
  if (debug_lvl <= 0) return;

  if (debug_lvl & 1) { // Standard console startup message output
  }
  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
    if (from == 0) cout << "Instantiated: FGInitialCondition" << endl;
    if (from == 1) cout << "Destroyed:    FGInitialCondition" << endl;
  }
  if (debug_lvl & 4 ) { // Run() method entry print for FGModel-derived objects
  }
  if (debug_lvl & 8 ) { // Runtime state variables
  }
  if (debug_lvl & 16) { // Sanity checking
  }
  if (debug_lvl & 64) {
    if (from == 0) { // Constructor
      cout << IdSrc << endl;
      cout << IdHdr << endl;
    }
  }
}
}