fgtrimaxis.cpp

6 DOF Missle Simulation

  int center,i,ref;

  // favor an off-center unit so that the same one can be used for both
  // pitch and roll.  An on-center unit is used (for pitch)if that's all
  // that's in contact with the ground.
  i=0; ref=-1; center=-1;
  while( (ref < 0) && (i < fdmex->GetGroundReactions()->GetNumGearUnits()) ) {
    if(fdmex->GetGroundReactions()->GetGearUnit(i)->GetWOW()) {
      if(fabs(fdmex->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation(2)) > 0.01)
        ref=i;
      else
        center=i;
    }
    i++;
  }
  if((ref < 0) && (center >= 0)) {
    ref=center;
  }
  cout << "SetThetaOnGround ref gear: " << ref << endl;
  if(ref >= 0) {
    double sp = fdmex->GetPropagate()->GetSinEuler(ePhi);
    double cp = fdmex->GetPropagate()->GetCosEuler(ePhi);
    double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1);
    double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2);
    double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3);
    double hagl = -1*lx*sin(ff) +
                    ly*sp*cos(ff) +
                    lz*cp*cos(ff);

    fgic->SetAltitudeAGLFtIC(hagl);
    cout << "SetThetaOnGround new alt: " << hagl << endl;
  }
  fgic->SetThetaRadIC(ff);
  cout << "SetThetaOnGround new theta: " << ff << endl;
}

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

bool FGTrimAxis::initTheta(void) {
  int i,N;
  int iForward = 0;
  int iAft = 1;
  double zAft,zForward,zDiff,theta;
  double xAft,xForward,xDiff;
  bool level;
  double saveAlt;

  saveAlt=fgic->GetAltitudeAGLFtIC();
  fgic->SetAltitudeAGLFtIC(100);


  N=fdmex->GetGroundReactions()->GetNumGearUnits();

  //find the first wheel unit forward of the cg
  //the list is short so a simple linear search is fine
  for( i=0; i<N; i++ ) {
    if(fdmex->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation(1) > 0 ) {
        iForward=i;
        break;
    }
  }
  //now find the first wheel unit aft of the cg
  for( i=0; i<N; i++ ) {
    if(fdmex->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation(1) < 0 ) {
        iAft=i;
        break;
    }
  }

  // now adjust theta till the wheels are the same distance from the ground
  xAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetBodyLocation(1);
  xForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetBodyLocation(1);
  xDiff = xForward - xAft;
  zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3);
  zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3);
  zDiff = zForward - zAft;
  level=false;
  theta=fgic->GetThetaDegIC();
  while(!level && (i < 100)) {
    theta+=radtodeg*atan(zDiff/xDiff);
    fgic->SetThetaDegIC(theta);
    fdmex->RunIC();
    zAft=fdmex->GetGroundReactions()->GetGearUnit(iAft)->GetLocalGear(3);
    zForward=fdmex->GetGroundReactions()->GetGearUnit(iForward)->GetLocalGear(3);
    zDiff = zForward - zAft;
    //cout << endl << theta << "  " << zDiff << endl;
    //cout << "0: " << fdmex->GetGroundReactions()->GetGearUnit(0)->GetLocalGear() << endl;
    //cout << "1: " << fdmex->GetGroundReactions()->GetGearUnit(1)->GetLocalGear() << endl;
    if(fabs(zDiff ) < 0.1)
        level=true;
    i++;
  }
  //cout << i << endl;
  if (debug_lvl > 0) {
      cout << "    Initial Theta: " << fdmex->GetPropagate()->GetEuler(eTht)*radtodeg << endl;
      cout << "    Used gear unit " << iAft << " as aft and " << iForward << " as forward" << endl;
  }
  control_min=(theta+5)*degtorad;
  control_max=(theta-5)*degtorad;
  fgic->SetAltitudeAGLFtIC(saveAlt);
  if(i < 100)
    return true;
  else
    return false;
}

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

void FGTrimAxis::SetPhiOnGround(double ff) {
  int i,ref;

  i=0; ref=-1;
  //must have an off-center unit here
  while ( (ref < 0) && (i < fdmex->GetGroundReactions()->GetNumGearUnits()) ) {
    if ( (fdmex->GetGroundReactions()->GetGearUnit(i)->GetWOW()) &&
      (fabs(fdmex->GetGroundReactions()->GetGearUnit(i)->GetBodyLocation(2)) > 0.01))
        ref=i;
    i++;
  }
  if (ref >= 0) {
    double st = fdmex->GetPropagate()->GetSinEuler(eTht);
    double ct = fdmex->GetPropagate()->GetCosEuler(eTht);
    double lx = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(1);
    double ly = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(2);
    double lz = fdmex->GetGroundReactions()->GetGearUnit(ref)->GetBodyLocation(3);
    double hagl = -1*lx*st +
                    ly*sin(ff)*ct +
                    lz*cos(ff)*ct;

    fgic->SetAltitudeAGLFtIC(hagl);
  }
  fgic->SetPhiRadIC(ff);

}

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

void FGTrimAxis::Run(void) {

  double last_state_value;
  int i;
  setControl();
  //cout << "FGTrimAxis::Run: " << control_value << endl;
  i=0;
  bool stable=false;
  while(!stable) {
    i++;
    last_state_value=state_value;
    fdmex->RunIC();
    getState();
    if(i > 1) {
      if((fabs(last_state_value - state_value) < tolerance) || (i >= 100) )
        stable=true;
    }
  }

  its_to_stable_value=i;
  total_stability_iterations+=its_to_stable_value;
  total_iterations++;
}

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

void FGTrimAxis::setThrottlesPct(void) {
  double tMin,tMax;
  for(unsigned i=0;i<fdmex->GetPropulsion()->GetNumEngines();i++) {
      tMin=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMin();
      tMax=fdmex->GetPropulsion()->GetEngine(i)->GetThrottleMax();
      //cout << "setThrottlespct: " << i << ", " << control_min << ", " << control_max << ", " << control_value;
      fdmex->GetFCS()->SetThrottleCmd(i,tMin+control_value*(tMax-tMin));
      //cout << "setThrottlespct: " << fdmex->GetFCS()->GetThrottleCmd(i) << endl;
      fdmex->RunIC(); //apply throttle change
      fdmex->GetPropulsion()->GetSteadyState();
  }
}

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

void FGTrimAxis::AxisReport(void) {

  char out[80];

  sprintf(out,"  %20s: %6.2f %5s: %9.2e Tolerance: %3.0e",
           GetControlName().c_str(), GetControl()*control_convert,
           GetStateName().c_str(), GetState()+state_target, GetTolerance());
  cout << out;

  if( fabs(GetState()+state_target) < fabs(GetTolerance()) )
     cout << "  Passed" << endl;
  else
     cout << "  Failed" << endl;
}

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

double FGTrimAxis::GetAvgStability( void ) {
  if(total_iterations > 0) {
    return double(total_stability_iterations)/double(total_iterations);
  }
  return 0;
}

/*****************************************************************************/
//    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 FGTrimAxis::Debug(int from)
{

  if (debug_lvl <= 0) return;
  if (debug_lvl & 1 ) { // Standard console startup message output
    if (from == 0) { // Constructor

    }
  }
  if (debug_lvl & 2 ) { // Instantiation/Destruction notification
    if (from == 0) cout << "Instantiated: FGTrimAxis" << endl;
    if (from == 1) cout << "Destroyed:    FGTrimAxis" << 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;
    }
  }
}
}