fgatmosphere.cpp

来自「6 DOF Missle Simulation」· C++ 代码 · 共 665 行 · 第 1/2 页

    vWindNED(eNorth) = speed;
  } else {
    vWindNED(eNorth) = speed * cos(psiw);
    vWindNED(eEast) = speed * sin(psiw);
    vWindNED(eDown) = 0.0;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

double FGAtmosphere::GetWindspeed(void) const
{
  return vWindNED.Magnitude();
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::SetWindPsi(double dir)
{
  double mag = GetWindspeed();
  psiw = dir;
  SetWindspeed(mag);  
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::Turbulence(void)
{
  switch (turbType) {
  case ttStandard: {
    TurbGain = TurbGain * TurbGain * 100.0;

    vDirectiondAccelDt(eX) = 1 - 2.0*(double(rand())/double(RAND_MAX));
    vDirectiondAccelDt(eY) = 1 - 2.0*(double(rand())/double(RAND_MAX));
    vDirectiondAccelDt(eZ) = 1 - 2.0*(double(rand())/double(RAND_MAX));

    MagnitudedAccelDt = 1 - 2.0*(double(rand())/double(RAND_MAX)) - Magnitude;
                                // Scale the magnitude so that it moves
                                // away from the peaks
    MagnitudedAccelDt = ((MagnitudedAccelDt - Magnitude) /
                         (1 + fabs(Magnitude)));
    MagnitudeAccel    += MagnitudedAccelDt*rate*TurbRate*State->Getdt();
    Magnitude         += MagnitudeAccel*rate*State->Getdt();
    Magnitude          = fabs(Magnitude);

    vDirectiondAccelDt.Normalize();

                                // deemphasise non-vertical forces
    vDirectiondAccelDt(eX) = square_signed(vDirectiondAccelDt(eX));
    vDirectiondAccelDt(eY) = square_signed(vDirectiondAccelDt(eY));

    vDirectionAccel += vDirectiondAccelDt*rate*TurbRate*State->Getdt();
    vDirectionAccel.Normalize();
    vDirection      += vDirectionAccel*rate*State->Getdt();

    vDirection.Normalize();

                                // Diminish turbulence within three wingspans
                                // of the ground
    vTurbulenceNED = TurbGain * Magnitude * vDirection;
    double HOverBMAC = Auxiliary->GetHOverBMAC();
    if (HOverBMAC < 3.0)
        vTurbulenceNED *= (HOverBMAC / 3.0) * (HOverBMAC / 3.0);

    // I don't believe these next two statements calculate the proper gradient over
    // the aircraft body. One reason is because this has no relationship with the
    // orientation or velocity of the aircraft, which it must have. What is vTurbulenceGrad
    // supposed to represent? And the direction and magnitude of the turbulence can change,
    // so both accelerations need to be accounted for, no?

    // Need to determine the turbulence change in body axes between two time points.

    vTurbulenceGrad = TurbGain*MagnitudeAccel * vDirection;
    vBodyTurbGrad = Propagate->GetTl2b()*vTurbulenceGrad;

    if (Aircraft->GetWingSpan() > 0) {
      vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan();
    } else {
      vTurbPQR(eP) = vBodyTurbGrad(eY)/30.0;
    }
//     if (Aircraft->GetHTailArm() != 0.0)
//       vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm();
//     else
//       vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0;

    if (Aircraft->GetVTailArm() > 0)
      vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm();
    else
      vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0;

                                // Clear the horizontal forces
                                // actually felt by the plane, now
                                // that we've used them to calculate
                                // moments.
                                // Why? (JSB)
//    vTurbulenceNED(eX) = 0.0;
//    vTurbulenceNED(eY) = 0.0;

    break;
  }
  case ttBerndt: { // This is very experimental and incomplete at the moment.

    TurbGain = TurbGain * TurbGain * 100.0;
  
    vDirectiondAccelDt(eX) = 1 - 2.0*(double(rand())/double(RAND_MAX));
    vDirectiondAccelDt(eY) = 1 - 2.0*(double(rand())/double(RAND_MAX));
    vDirectiondAccelDt(eZ) = 1 - 2.0*(double(rand())/double(RAND_MAX));


    MagnitudedAccelDt = 1 - 2.0*(double(rand())/double(RAND_MAX)) - Magnitude;
    MagnitudeAccel    += MagnitudedAccelDt*rate*State->Getdt();
    Magnitude         += MagnitudeAccel*rate*State->Getdt();

    vDirectiondAccelDt.Normalize();
    vDirectionAccel += vDirectiondAccelDt*rate*State->Getdt();
    vDirectionAccel.Normalize();
    vDirection      += vDirectionAccel*rate*State->Getdt();

                                // Diminish z-vector within two wingspans
                                // of the ground
    double HOverBMAC = Auxiliary->GetHOverBMAC();
    if (HOverBMAC < 2.0)
        vDirection(eZ) *= HOverBMAC / 2.0;

    vDirection.Normalize();

    vTurbulenceNED = TurbGain*Magnitude * vDirection;
    vTurbulenceGrad = TurbGain*MagnitudeAccel * vDirection;

    vBodyTurbGrad = Propagate->GetTl2b()*vTurbulenceGrad;
    vTurbPQR(eP) = vBodyTurbGrad(eY)/Aircraft->GetWingSpan();
    if (Aircraft->GetHTailArm() > 0)
      vTurbPQR(eQ) = vBodyTurbGrad(eZ)/Aircraft->GetHTailArm();
    else
      vTurbPQR(eQ) = vBodyTurbGrad(eZ)/10.0;

    if (Aircraft->GetVTailArm() > 0)
      vTurbPQR(eR) = vBodyTurbGrad(eX)/Aircraft->GetVTailArm();
    else
      vTurbPQR(eR) = vBodyTurbGrad(eX)/10.0;

    break;
  }
  case ttCulp: { 

    vTurbPQR(eP) = wind_from_clockwise;
    if (TurbGain == 0.0) return;
  
    // keep the inputs within allowable limts for this model
    if (TurbGain < 0.0) TurbGain = 0.0;
    if (TurbGain > 1.0) TurbGain = 1.0;
    if (TurbRate < 0.0) TurbRate = 0.0;
    if (TurbRate > 30.0) TurbRate = 30.0;
    if (Rhythmicity < 0.0) Rhythmicity = 0.0;
    if (Rhythmicity > 1.0) Rhythmicity = 1.0;

    // generate a sine wave corresponding to turbulence rate in hertz
    double time = FDMExec->GetSimTime();
    double sinewave = sin( time * TurbRate * 6.283185307 );

    double random = 0.0;
    if (target_time == 0.0) {
      strength = random = 1 - 2.0*(double(rand())/double(RAND_MAX));
      target_time = time + 0.71 + (random * 0.5);
    }
    if (time > target_time) {
      spike = 1.0;
      target_time = 0.0;
    }    

    // max vertical wind speed in fps, corresponds to TurbGain = 1.0
    double max_vs = 40;

    vTurbulenceNED(1) = vTurbulenceNED(2) = vTurbulenceNED(3) = 0.0;
    double delta = strength * max_vs * TurbGain * (1-Rhythmicity) * spike;

    // Vertical component of turbulence.
    vTurbulenceNED(3) = sinewave * max_vs * TurbGain * Rhythmicity;
    vTurbulenceNED(3)+= delta;
    double HOverBMAC = Auxiliary->GetHOverBMAC();
    if (HOverBMAC < 3.0)
        vTurbulenceNED(3) *= HOverBMAC * 0.3333;
 
    // Yaw component of turbulence.
    vTurbulenceNED(1) = sin( delta * 3.0 );
    vTurbulenceNED(2) = cos( delta * 3.0 );

    // Roll component of turbulence. Clockwise vortex causes left roll.
    vTurbPQR(eP) += delta * 0.04;

    spike = spike * 0.9;
    break;
  }
  default:
    break;
  }
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::UseExternal(void)
{
  temperature=&exTemperature;
  pressure=&exPressure;
  density=&exDensity;
  useExternal=true;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::UseInternal(void)
{
  temperature=&intTemperature;
  pressure=&intPressure;
  density=&intDensity;
  useExternal=false;
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void FGAtmosphere::bind(void)
{
  typedef double (FGAtmosphere::*PMF)(int) const;
  typedef double (FGAtmosphere::*PMFv)(void) const;
  typedef void   (FGAtmosphere::*PMFd)(int,double);
  PropertyManager->Tie("atmosphere/T-R", this, (PMFv)&FGAtmosphere::GetTemperature);
  PropertyManager->Tie("atmosphere/rho-slugs_ft3", this, (PMFv)&FGAtmosphere::GetDensity);
  PropertyManager->Tie("atmosphere/P-psf", this, (PMFv)&FGAtmosphere::GetPressure);
  PropertyManager->Tie("atmosphere/a-fps", this, &FGAtmosphere::GetSoundSpeed);
  PropertyManager->Tie("atmosphere/T-sl-R", this, &FGAtmosphere::GetTemperatureSL);
  PropertyManager->Tie("atmosphere/rho-sl-slugs_ft3", this, &FGAtmosphere::GetDensitySL);
  PropertyManager->Tie("atmosphere/P-sl-psf", this, &FGAtmosphere::GetPressureSL);
  PropertyManager->Tie("atmosphere/a-sl-fps", this, &FGAtmosphere::GetSoundSpeedSL);
  PropertyManager->Tie("atmosphere/theta", this, &FGAtmosphere::GetTemperatureRatio);
  PropertyManager->Tie("atmosphere/sigma", this, &FGAtmosphere::GetDensityRatio);
  PropertyManager->Tie("atmosphere/delta", this, &FGAtmosphere::GetPressureRatio);
  PropertyManager->Tie("atmosphere/a-ratio", this, &FGAtmosphere::GetSoundSpeedRatio);
  PropertyManager->Tie("atmosphere/psiw-rad", this, &FGAtmosphere::GetWindPsi);
  PropertyManager->Tie("atmosphere/delta-T", this, &FGAtmosphere::GetDeltaT, &FGAtmosphere::SetDeltaT);
  PropertyManager->Tie("atmosphere/T-sl-dev-F", this, &FGAtmosphere::GetSLTempDev, &FGAtmosphere::SetSLTempDev);
  PropertyManager->Tie("atmosphere/density-altitude", this, &FGAtmosphere::GetDensityAltitude);

  PropertyManager->Tie("atmosphere/wind-north-fps", this, eNorth, (PMF)&FGAtmosphere::GetWindNED,
                                                          (PMFd)&FGAtmosphere::SetWindNED);
  PropertyManager->Tie("atmosphere/wind-east-fps",  this, eEast, (PMF)&FGAtmosphere::GetWindNED,
                                                          (PMFd)&FGAtmosphere::SetWindNED);
  PropertyManager->Tie("atmosphere/wind-down-fps",  this, eDown, (PMF)&FGAtmosphere::GetWindNED,
                                                          (PMFd)&FGAtmosphere::SetWindNED);
  PropertyManager->Tie("atmosphere/wind-from-cw", this, &FGAtmosphere::GetWindFromClockwise,
                                                        &FGAtmosphere::SetWindFromClockwise);
  PropertyManager->Tie("atmosphere/wind-mag-fps", this, &FGAtmosphere::GetWindspeed,
                                                        &FGAtmosphere::SetWindspeed);
  PropertyManager->Tie("atmosphere/total-wind-north-fps", this, eNorth, (PMF)&FGAtmosphere::GetTotalWindNED);
  PropertyManager->Tie("atmosphere/total-wind-east-fps",  this, eEast,  (PMF)&FGAtmosphere::GetTotalWindNED);
  PropertyManager->Tie("atmosphere/total-wind-down-fps",  this, eDown,  (PMF)&FGAtmosphere::GetTotalWindNED);

  PropertyManager->Tie("atmosphere/gust-north-fps", this, eNorth, (PMF)&FGAtmosphere::GetGustNED,
                                                          (PMFd)&FGAtmosphere::SetGustNED);
  PropertyManager->Tie("atmosphere/gust-east-fps",  this, eEast, (PMF)&FGAtmosphere::GetGustNED,
                                                          (PMFd)&FGAtmosphere::SetGustNED);
  PropertyManager->Tie("atmosphere/gust-down-fps",  this, eDown, (PMF)&FGAtmosphere::GetGustNED,
                                                          (PMFd)&FGAtmosphere::SetGustNED);

  PropertyManager->Tie("atmosphere/p-turb-rad_sec", this,1, (PMF)&FGAtmosphere::GetTurbPQR);
  PropertyManager->Tie("atmosphere/q-turb-rad_sec", this,2, (PMF)&FGAtmosphere::GetTurbPQR);
  PropertyManager->Tie("atmosphere/r-turb-rad_sec", this,3, (PMF)&FGAtmosphere::GetTurbPQR);
  PropertyManager->Tie("atmosphere/turb-rate", this, &FGAtmosphere::GetTurbRate, &FGAtmosphere::SetTurbRate);
  PropertyManager->Tie("atmosphere/turb-gain", this, &FGAtmosphere::GetTurbGain, &FGAtmosphere::SetTurbGain);
  PropertyManager->Tie("atmosphere/turb-rhythmicity", this, &FGAtmosphere::GetRhythmicity,
                                                            &FGAtmosphere::SetRhythmicity);
}

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//    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 FGAtmosphere::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: FGAtmosphere" << endl;
    if (from == 1) cout << "Destroyed:    FGAtmosphere" << 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 & 128) { // Turbulence
    if (first_pass && from == 2) {
      first_pass = false;
      cout << "vTurbulenceNED(X), vTurbulenceNED(Y), vTurbulenceNED(Z), "
           << "vTurbulenceGrad(X), vTurbulenceGrad(Y), vTurbulenceGrad(Z), "
           << "vDirection(X), vDirection(Y), vDirection(Z), "
           << "Magnitude, "
           << "vTurbPQR(P), vTurbPQR(Q), vTurbPQR(R), " << endl;
    } 
    if (from == 2) {
      cout << vTurbulenceNED << ", " << vTurbulenceGrad << ", " << vDirection << ", " << Magnitude << ", " << vTurbPQR << endl;
    }
  }
  if (debug_lvl & 64) {
    if (from == 0) { // Constructor
      cout << IdSrc << endl;
      cout << IdHdr << endl;
    }
  }
}

} // namespace JSBSim