Exemple #1
0
//------------------------------------------------------------------------------
// 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);
   double vpdot = (cmdVelMPS - pp->getTotalVelocity()) * 0.05;
   if(vpdot > maxAccel)  vpdot = maxAccel;
   if(vpdot < -maxAccel) vpdot = -maxAccel;

   // Compute new velocity (body coordinates)
   double newVP = pp->getTotalVelocity() + vpdot * dt;

   // Set our angular values
   pp->setEulerAngles(newPhi, newTheta, newPsi);
   pp->setAngularVelocities(pa, qa, ra);

   // Set our velocity vector (body coordinates)
   pp->setVelocityBody( static_cast<LCreal>(newVP), 0.0, 0.0);

   // Set our acceleration vector (body coordinates)
   pp->setAccelerationBody( static_cast<LCreal>(vpdot), 0.0, 0.0);
}
Exemple #2
0
//------------------------------------------------------------------------------
// dynamics() -- update player's vehicle dynamics
//------------------------------------------------------------------------------
void JSBSimModel::dynamics(const LCreal dt)
{

    // Get our Player (must have one!)
    Simulation::Player* p = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
    if (p == 0) return;

    if (fdmex == 0) return;

    JSBSim::FGPropagate* Propagate = fdmex->GetPropagate();
    if (Propagate == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;
    JSBSim::FGAccelerations* Accelerations = fdmex->GetAccelerations();
    if (Accelerations == 0) return;

    pitchTrimPos += pitchTrimRate * pitchTrimSw * dt;
    if (pitchTrimPos > 1.0) {
        pitchTrimPos = 1.0;
    } else if (pitchTrimPos < -1.0) {
        pitchTrimPos = -1.0;
    }
    FCS->SetPitchTrimCmd(pitchTrimPos);

    rollTrimPos += rollTrimRate * rollTrimSw * dt;
    if (rollTrimPos > 1.0) {
        rollTrimPos = 1.0;
    } else if (rollTrimPos < -1.0) {
        rollTrimPos = -1.0;
    }
    FCS->SetRollTrimCmd(rollTrimPos);

    fdmex->Setdt(dt);
    // ---
    // Pass flags & Data
    // ---
#if 0
    // CGB TBD
    if (isFrozen() || dt == 0) freeze = -1;
    else freeze = 0;

    if (isPositionFrozen()) posFrz = -1;
    else posFrz = 0;

    if (isAltitudeFrozen()) altFrz = -1;
    else altFrz = 0;

    if (isFuelFrozen()) fuelFrz = -1;
    else fuelFrz = 0;

    rw_elev = getTerrainElevationFt();
#endif

    // ---
    // Run the model
    // ---
    fdmex->Run();     //loop JSBSim once w/o integrating

    // ---
    // Set values for Player & AirVehicle interfaces
    //    (Note: Player::dynamics() computes the new position)
    // ---
    p->setAltitude(Basic::Distance::FT2M * Propagate->GetAltitudeASL(), true);
    p->setVelocity((LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eNorth)), (LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eEast)), (LCreal)(Basic::Distance::FT2M * Propagate->GetVel(JSBSim::FGJSBBase::eDown)));
    p->setVelocityBody((LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(1)), (LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(2)), (LCreal)(Basic::Distance::FT2M * Propagate->GetUVW(3)));
//    LCreal accX = Basic::Distance::FT2M * Propagate->GetUVWdot(1);
//    LCreal accY = Basic::Distance::FT2M * Propagate->GetUVWdot(2);
//    LCreal accZ = Basic::Distance::FT2M * Propagate->GetUVWdot(3);
    const JSBSim::FGMatrix33& Tb2l = Propagate->GetTb2l();
    const JSBSim::FGColumnVector3& vUVWdot = Accelerations->GetUVWdot();

    p->setEulerAngles((LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::ePhi)), (LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::eTht)), (LCreal)(Propagate->GetEuler(JSBSim::FGJSBBase::ePsi)));
    p->setAngularVelocities((LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eP)), (LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eQ)), (LCreal)(Propagate->GetPQR(JSBSim::FGJSBBase::eR)));

    JSBSim::FGColumnVector3 vVeldot = Tb2l * vUVWdot;
    p->setAcceleration((LCreal)(Basic::Distance::FT2M * vVeldot(1)), (LCreal)(Basic::Distance::FT2M * vVeldot(2)), (LCreal)(Basic::Distance::FT2M * vVeldot(3)));

    //std::printf("(%6.1f, %6.1f, %6.1f)   vel=%8.1f   alt=%8.1f alt2=%8.1f\n", acData->phi, acData->theta, acData->psi, acData->vp, acData->hp, (M2FT*getAltitude()) );
    //std::printf("f=%6.1f p=%6.1f, qa=%6.1f, a=%6.1f, g=%6.1f\n", hotasIO->pitchForce, acData->theta, acData->qa, acData->alpha, acData->gamma );

        //{
        // std::cout << "JSBSim: ---------------------------------" << std::endl;
        // osg::Vec4 fq;
        // fq.set(acData->e1, acData->e2, acData->e4, acData->e4);
        // osg::Matrix m2;
        // m2.set(
        //    acData->l1, acData->l2, acData->l3, 0,
        //    acData->m1, acData->m2, acData->m3, 0,
        //    acData->n1, acData->n2, acData->n3, 0,
        //    0,          0,          0,          1
        //    );
        // std::printf("Eaagles*EA: (%6.1f, %6.1f, %6.1f)\n", getRollD(), getPitchD(), getHeadingD());
        // osg::Matrix m0 = getRotationalMatrix();
        // osg::Quat q0 = getQuaternions();
        // setRotationalMatrix(m2);
        // //setQuaternions(fq);
        // osg::Quat eq = getQuaternions();
        // osg::Matrix m1 = getRotationalMatrix();
        // std::printf("Eaagles EA: (%6.1f, %6.1f, %6.1f)\n", getRollD(), getPitchD(), getHeadingD());
        // std::printf("JSBSim    EA: (%6.1f, %6.1f, %6.1f)\n", acData->phi, acData->theta, acData->psi);
        // std::printf("Eaagles* Q: (%6.3f, %6.3f, %6.3f, %6.3f)\n", q0[0], q0[1], q0[2], q0[3]);
        // std::printf("Eaagles  Q: (%6.3f, %6.3f, %6.3f, %6.3f)\n", eq[0], eq[1], eq[2], eq[3]);
        // std::printf("JSBSim     Q: (%6.3f, %6.3f, %6.3f, %6.3f)\n", fq[0], fq[1], fq[2], fq[3]);
        // std::printf("Eaagles*mm: (%6.3f, %6.3f, %6.3f, %6.3f)\n", m0(0,0), m0(0,1), m0(0,2), m0(0,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m0(1,0), m0(1,1), m0(1,2), m0(1,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m0(2,0), m0(2,1), m0(2,2), m0(2,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m0(3,0), m0(3,1), m0(3,2), m0(3,3));
        // std::printf("Eaagles mm: (%6.3f, %6.3f, %6.3f, %6.3f)\n", m1(0,0), m1(0,1), m1(0,2), m1(0,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m1(1,0), m1(1,1), m1(1,2), m1(1,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m1(2,0), m1(2,1), m1(2,2), m1(2,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m1(3,0), m1(3,1), m1(3,2), m1(3,3));
        // std::printf("JSBSim    mm: (%6.3f, %6.3f, %6.3f, %6.3f)\n", m2(0,0), m2(0,1), m2(0,2), m2(0,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m2(1,0), m2(1,1), m2(1,2), m2(1,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m2(2,0), m2(2,1), m2(2,2), m2(2,3));
        // std::printf("            (%6.3f, %6.3f, %6.3f, %6.3f)\n", m2(3,0), m2(3,1), m2(3,2), m2(3,3));
        //}

    // CGB Set Autopilot Properties directly for now
    JSBSim::FGPropertyManager* propMgr = fdmex->GetPropertyManager();
    if (propMgr != 0) {
        JSBSim::FGPropertyNode* propNode = propMgr->GetNode();
        if (propNode != 0) {
            if (hasHeadingHold) {
                propNode->SetBool("ap/heading_hold", isHeadingHoldOn());
                propNode->SetDouble("ap/heading_setpoint", getCommandedHeadingD());
            }
            if (hasVelocityHold) {
                propNode->SetBool("ap/airspeed_hold", isVelocityHoldOn());
                propNode->SetDouble("ap/airspeed_setpoint", getCommandedVelocityKts());
            }
            if (hasAltitudeHold) {
                propNode->SetBool("ap/altitude_hold", isAltitudeHoldOn());
                propNode->SetDouble("ap/altitude_setpoint", (getCommandedAltitude() * Basic::Distance::M2FT) );
            }
        }
    }

    BaseClass::dynamics(dt);
}
//----------------------------------------------------------
// update4DofModel -- update equations of motion
//----------------------------------------------------------
void LaeroModel::update4DofModel(const LCreal dt)
{
   //-------------------------------------------------------
   // get data pointers
   //-------------------------------------------------------
   Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );

   if (pPlr != nullptr) {

      //==============================================================
      // ROTATIONAL EOM
      //==============================================================

      //----------------------------------------------------
      // integrate Euler angles using Adams-Bashforth
      //----------------------------------------------------
      phi += 0.5 * (3.0 * phiDot - phiDot1) * dT;
      if (phi >  PI) phi = -PI;
      if (phi < -PI) phi =  PI;

      tht += 0.5 * (3.0 * thtDot - thtDot1) * dT;
      if (tht >=  HALF_PI) tht =  (HALF_PI - EPSILON);
      if (tht <= -HALF_PI) tht = -(HALF_PI - EPSILON);

      psi += 0.5 * (3.0 * psiDot - psiDot1) * dT;
      if (psi >  PI) psi = -PI;
      if (psi < -PI) psi =  PI;

      //----------------------------------------------------
      // update Euler angles
      //----------------------------------------------------
      pPlr->setEulerAngles(phi, tht, psi);

      //----------------------------------------------------
      // hold current rotational control values for next iteration
      //----------------------------------------------------
      phiDot1 = phiDot;
      thtDot1 = thtDot;
      psiDot1 = psiDot;

      //----------------------------------------------------
      // compute sin, cos of Euler angles
      //----------------------------------------------------
      double sinPhi = std::sin(phi);
      double cosPhi = std::cos(phi);
      double sinTht = std::sin(tht);
      double cosTht = std::cos(tht);
      double sinPsi = std::sin(psi);
      double cosPsi = std::cos(psi);

      //----------------------------------------------------
      // compute local to body axes matrix
      //----------------------------------------------------
      double l1 =  cosTht * cosPsi;
      double l2 =  cosTht * sinPsi;
      double l3 = -sinTht;
      double m1 =  sinPhi * sinTht * cosPsi - cosPhi * sinPsi;
      double m2 =  sinPhi * sinTht * sinPsi + cosPhi * cosPsi;
      double m3 =  sinPhi * cosTht;
      double n1 =  cosPhi * sinTht * cosPsi + sinPhi * sinPsi;
      double n2 =  cosPhi * sinTht * sinPsi - sinPhi * cosPsi;
      double n3 =  cosPhi * cosTht;

      //----------------------------------------------------
      // update p,q,r angular velocity components (body)
      //----------------------------------------------------
      p = phiDot                    +       (-sinTht)*psiDot;
      q =           (cosPhi)*thtDot + (cosTht*sinPhi)*psiDot;
      r =          (-sinPhi)*thtDot + (cosTht*cosPhi)*psiDot;

      //----------------------------------------------------
      // update angular velocities
      //----------------------------------------------------
      pPlr->setAngularVelocities(p, q, r);


      //==============================================================
      // TRANSLATIONAL EOM
      //==============================================================

      //----------------------------------------------------
      // integrate u,v,w velocity components (body)
      //----------------------------------------------------
      u  += 0.5*(3.0*uDot - uDot1)*dT;
      v  += 0.5*(3.0*vDot - vDot1)*dT;
      w  += 0.5*(3.0*wDot - wDot1)*dT;

      //----------------------------------------------------
      // hold current translational control values for next iteration
      //----------------------------------------------------
      uDot1   = uDot;
      vDot1   = vDot;
      wDot1   = wDot;

      //----------------------------------------------------
      // update velocity in NED system
      //----------------------------------------------------
      velN  = l1*u + m1*v + n1*w;
      velE  = l2*u + m2*v + n2*w;
      velD  = l3*u + m3*v + n3*w;

      pPlr->setVelocity(velN, velE, velD);

      //----------------------------------------------------
      // update acceleration in NED system
      //----------------------------------------------------
      //double A = uDot + w*q - v*r;
      //double B = vDot + u*r - w*p;
      //double C = wDot + v*p - u*q;

      //accN  = l1*A + m1*B + n1*C;
      //accE  = l2*A + m2*B + n2*C;
      //accD  = l3*A + m3*B + n3*C;

      //pPlr->setAcceleration(accN, accE, accD);
   }
}