C++ Player::getRollR方法代码示例

bool LaeroModel::flyPhi(const double phiCmdDeg, const double phiDotCmdDps)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   bool ok = (pPlr != nullptr);
   if (ok) {

      //-------------------------------------------------------
      // convert argument units (deg -> rad)
      //-------------------------------------------------------
      double phiCmdRad    = phiCmdDeg * Basic::Angle::D2RCC;
      double phiDotCmdRps = phiDotCmdDps * Basic::Angle::D2RCC;

      //-------------------------------------------------------
      // current phi error (rad)
      //-------------------------------------------------------
      double phiRad    = pPlr->getRollR();
      double phiErrRad = phiCmdRad - phiRad;

      //-------------------------------------------------------
      // phi error break point (rad)
      //-------------------------------------------------------
      const double TAU = 1.0;  // time constant [sec]
      double phiErrBrkRad = phiDotCmdRps * TAU;

      //-------------------------------------------------------
      // control signal for commanded phi (rps)
      //-------------------------------------------------------
      double phiDotRps = sign(phiErrRad) * phiDotCmdRps;
      if (std::abs(phiErrRad) < phiErrBrkRad) {
         phiDotRps = (phiErrRad / phiErrBrkRad) * phiDotCmdRps;
      }

      //-------------------------------------------------------
      // assign result to roll control
      //-------------------------------------------------------
      phiDot = phiDotRps;
   }

   return ok;
}

//------------------------------------------------------------------------------
// updateRAC -- update Robot Aircraft
//------------------------------------------------------------------------------
void RacModel::updateRAC(const LCreal dt)
{
   // Get our Player (must have one!)
   Simulation::Player* pp = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
   if (pp == nullptr) return;

   // Acceleration of Gravity (M/S)
   LCreal g = ETHG * Basic::Distance::FT2M;

   // Set default commanded values
   if (cmdAltitude < -9000.0)
       cmdAltitude = static_cast<LCreal>(pp->getAltitudeM());
   if (cmdHeading < -9000.0)
       cmdHeading = static_cast<LCreal>(pp->getHeadingD());
   if (cmdVelocity < -9000.0)
       cmdVelocity = pp->getTotalVelocityKts();

   // ---
   // Compute delta altitude; commanded vertical velocity and
   // commanded flight path angle
   // ---

   // Max altitude rate 6000 ft /min converted to M/S
   double maxAltRate = (3000.0 / 60.0) * Basic::Distance::FT2M;

   // commanded vertical velocity is delta altitude limited to max rate
   double cmdAltRate = (cmdAltitude - pp->getAltitudeM());
   if (cmdAltRate > maxAltRate) cmdAltRate = maxAltRate;
   else if (cmdAltRate < -maxAltRate) cmdAltRate = -maxAltRate;

   // Compute commanded flight path angle (gamma)
   double cmdPitch = 0;
   LCreal vt = pp->getTotalVelocity();
   if (vt > 0) {
      cmdPitch = std::asin( cmdAltRate/vt );
   }

   // ---
   // Compute Max G
   // ---
   LCreal gmax = gMax;                     // Max g,s
   if(pp->getTotalVelocity() < vpMaxG) {
      gmax = 1.0f + (gMax - 1.0f) * (pp->getTotalVelocity() - vpMin) / (vpMaxG - vpMin);
   }

   // ---
   // Computer max turn rate, max/min pitch rates
   // ---
   LCreal ra_max = gmax * g / pp->getTotalVelocity();    // Turn rate base on vp and g,s (rad/sec)
   LCreal qa_max = ra_max;                           // Max pull up pitch rate (rad/sec)
   LCreal qa_min = -qa_max;                          // Max pushover pitch rate (rad/sec)
   if(gmax > 2.0) {
      // Max yaw rate (rad/sec)
      qa_min = -( 2.0f * g / pp->getTotalVelocity());
   }

   // ---
   // Get old angular values
   // ---
   const osg::Vec3 oldRates = pp->getAngularVelocities();
   //LCreal pa1 = oldRates[Simulation::Player::IROLL];
   LCreal qa1 = oldRates[Simulation::Player::IPITCH];
   LCreal ra1 = oldRates[Simulation::Player::IYAW];

   // ---
   // Find pitch rate and update pitch
   // ---
   double qa = Basic::Angle::aepcdRad(cmdPitch - pp->getPitchR()) * 0.1;
   if(qa > qa_max) qa = qa_max;
   if(qa < qa_min) qa = qa_min;

   // Find turn rate
   double ra = Basic::Angle::aepcdRad((cmdHeading  * Basic::Angle::D2RCC) - pp->getHeadingR()) * 0.1;
   if(ra > ra_max) ra = ra_max;
   if(ra < -ra_max) ra = -ra_max;

   // Damage
   double dd = pp->getDamage();
   if (dd > 0.5) {
      ra += (dd - 0.5) * ra_max;
      qa -= (dd - 0.5) * qa_max;
   }

   // Using Pitch rate, integrate pitch
   double newTheta = static_cast<LCreal>(pp->getPitch() + (qa + qa1) * dt / 2.0);

   // Use turn rate integrate heading
   double newPsi = static_cast<LCreal>(pp->getHeading() + (ra + ra1) * dt / 2.0);
   if(newPsi > 2.0*PI) newPsi -= 2.0*PI;
   if(newPsi < 0.0) newPsi += 2.0*PI;

   // Roll angle is proportional to max turn rate - filtered
   double pa = 0.0;
   double newPhi = 0.98 * pp->getRollR() + 0.02 * (ra / ra_max * (Basic::Angle::D2RCC * 60.0));

   // Find Acceleration
   double cmdVelMPS = cmdVelocity * (Basic::Distance::NM2M / 3600.0);
//.........这里部分代码省略.........