Пример #1
0
// ----------------------------------------------------------------------------
// updateData() - update background data
// ----------------------------------------------------------------------------
void MapDisplay::updateData(const double dt)
{
   BaseClass::updateData(dt);

   // get pointer to MapPage data
   int cmdRange {};
   const auto page = static_cast<MapPage*>(subpage());
   if (page != nullptr) {
      cmdRange = static_cast<int>(page->getRange());
   }

   double cmdAirspeed {}, cmdAltitude {}, cmdHeading {};
   bool apButtonsVis {};
   // pilot max
   double maxAccel {}, maxTurn {}, maxBank {}, maxClimb {};
   // default to autopilot mode off
   int apMode {1};
   const auto pA = static_cast<oe::models::Aircraft*>(getOwnship());
   if (pA != nullptr) {
      const auto ap = static_cast<oe::models::Autopilot*>(pA->getPilot());
      if (ap != nullptr) {
         // button visibility is based on autopilot being in NO modes
         apButtonsVis = (ap->isNavModeOn() || ap->isLoiterModeOn() || ap->isFollowTheLeadModeOn());
         cmdAirspeed = ap->getCommandedVelocityKts();
         cmdAltitude = ap->getCommandedAltitudeFt();
         cmdHeading = ap->getCommandedHeadingD();
         if (ap->isNavModeOn()) apMode = 2;
         else if (ap->isLoiterModeOn()) apMode = 3;
         else if (ap->isFollowTheLeadModeOn()) apMode = 4;
         maxAccel = ap->getMaxVelAcc();
         maxTurn = ap->getMaxTurnRate();
         maxBank = ap->getMaxBankAngle();
         maxClimb = ap->getMaxClimbRate();
      }
   }

   // get pointer to LaeroModel data
   send("cmdRange",     UPDATE_VALUE, cmdRange,     cmdRangeSD);
   send("cmdAirspeed",  UPDATE_VALUE, cmdAirspeed,  cmdAirspeedSD);
   send("cmdAltitude",  UPDATE_VALUE, cmdAltitude,  cmdAltitudeSD);
   send("cmdHeading",   UPDATE_VALUE, cmdHeading,   cmdHeadingSD);
   // Autopilot mode
   send("apMode", SELECT, apMode, apModeSD);
   // button visibility
   send("apReqButtons", SET_VISIBILITY, !apButtonsVis, apReqButtonVisSD);
   // maximums
   send("cmdAccel",   UPDATE_VALUE, maxAccel,   maxAccelSD);
   send("cmdClimb",   UPDATE_VALUE, maxClimb,   maxClimbSD);
   send("cmdRot",   UPDATE_VALUE, maxTurn,   maxTurnSD);
   send("cmdBank", UPDATE_VALUE, maxBank, maxBankSD);
}
Пример #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);
}