C++ jsbsim::FGFCS类代码示例

int JSBSimModel::getEngPLA(LCreal* const pla, const int max) const
{
    if (fdmex == 0) return 0;
    JSBSim::FGPropulsion* Propulsion = fdmex->GetPropulsion();
    if (Propulsion == 0) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return 0;

    // return throttle PLA (percent)
    if (pla == 0 || max <= 0) {
        return 0;
    }
    int num = getNumberOfEngines();
    if (max < num) {
        num = max;
    }
    for (int i = 0; i < num; i++) {
        JSBSim::FGEngine* eng = Propulsion->GetEngine(i);
        double t = FCS->GetThrottlePos(i);
        double tmin = eng->GetThrottleMin();
        double tmax = eng->GetThrottleMax();
        double throttle = (t - tmin) / (tmax - tmin) * 100.0;
        pla[i] = (LCreal)throttle;
    }
    return num;
}

//------------------------------------------------------------------------------
// getSpeedBrakesSwitch() --   Returns the speed brake position (percent)
//      0-> Fully Retracted;  100.0 -> Fully Extended 
//------------------------------------------------------------------------------
LCreal JSBSimModel::getSpeedBrakePosition() const
{
    if (fdmex == 0) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return 0;

    return (LCreal)(FCS->GetDsbPos(JSBSim::ofNorm) * 100.0);
}

//------------------------------------------------------------------------------
// setGearHandleSwitch() -- Set Gear handle switch position
//                          0 -> Handle up;  1 -> hande down
//------------------------------------------------------------------------------
void JSBSimModel::setGearHandleSwitch(const LCreal sw)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    FCS->SetGearCmd(sw);
}

//------------------------------------------------------------------------------
// getLandingGearPosition() --   Returns the landing gear position (percent)
//      0-> Fully Retracted;  100.0 -> Fully Extended 
//------------------------------------------------------------------------------
LCreal JSBSimModel::getLandingGearPosition() const
{
    if (fdmex == 0) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return 0;

    return (LCreal)(FCS->GetGearPos() * 100.0);
}

//------------------------------------------------------------------------------
// setRudderPedalInput(pedal) -- Pedal inputs: normalized
//          pedal:  -1.0 -> max left;  0.0 -> center;  1.0 -> max right
//------------------------------------------------------------------------------
void JSBSimModel::setRudderPedalInput(const LCreal pedal)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    FCS->SetDrCmd(-pedal);
}

//------------------------------------------------------------------------------
// setControlStickPitchInput(Pitch) --  Control inputs: normalized
//  pitch:  -1.0 -> max forward (nose down); 0.0 -> center;  1.0 -> max back (nose up)
//------------------------------------------------------------------------------
void JSBSimModel::setControlStickPitchInput(const LCreal pitch)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    FCS->SetDeCmd(-pitch);
}

//------------------------------------------------------------------------------
// setControlStickRollInput(Roll) --  Control inputs: normalized
//   roll:  -1.0 -> max left;  0.0 -> center;  1.0 -> max right
//------------------------------------------------------------------------------
void JSBSimModel::setControlStickRollInput(const LCreal roll)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    FCS->SetDaCmd(roll);
}

//------------------------------------------------------------------------------
// getSpeedBrakesSwitch() --   Returns the speed brake position (percent)
//      0-> Fully Retracted;  100.0 -> Fully Extended
//------------------------------------------------------------------------------
LCreal JSBSimModel::getSpeedBrakePosition() const
{
    if (fdmex == nullptr) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == nullptr) return 0;

    return static_cast<LCreal>(FCS->GetDsbPos(JSBSim::ofNorm) * 100.0);
}

//------------------------------------------------------------------------------
// getLandingGearPosition() --   Returns the landing gear position (percent)
//      0-> Fully Retracted;  100.0 -> Fully Extended
//------------------------------------------------------------------------------
LCreal JSBSimModel::getLandingGearPosition() const
{
    if (fdmex == nullptr) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == nullptr) return 0;

    return static_cast<LCreal>(FCS->GetGearPos() * 100.0);
}

//------------------------------------------------------------------------------
// setBrakes() -- Sets brake positions (left & right)
//               No brake force  -> 0.0
//               Max brake force -> 1.0
//------------------------------------------------------------------------------
void JSBSimModel::setBrakes(const LCreal left, const LCreal right)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    FCS->SetLBrake(left);
    FCS->SetRBrake(right);
    FCS->SetCBrake(0.0);
}

//------------------------------------------------------------------------------
// setSpeedBrakesSwitch() --   Sets the speed brake switch position:
//      -1.0 -> Retract;  0.0 -> Hold;  1.0 -> Extend 
//------------------------------------------------------------------------------
void JSBSimModel::setSpeedBrakesSwitch(const LCreal sw)
{
    if (fdmex == 0) return;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return;

    if (sw > 0.0) {
        FCS->SetDsbCmd(100.0);
    }
    else if (sw < 0.0) {
        FCS->SetDsbCmd(0.0);
    }
}

//------------------------------------------------------------------------------
// int setThrottles(positions,num) -- Set throttle positions
//
//    positions -> Array of throttle positions
//      (for each throttle)
//              < 0.0       -> Cutoff
//              0.0         -> Idle
//              1.0         -> MIL
//              2.0         -> A/B
//    num -> number of throttle positions to get/set
//    returns the actual number of throttle positions
//------------------------------------------------------------------------------
int JSBSimModel::setThrottles(const LCreal* const positions, const int num)
{
    if (fdmex == 0) return 0;
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (FCS == 0) return 0;

    if (positions == 0 || num <= 0) {
        return 0;
    }
    int n = getNumberOfEngines();
    if (num < n) {
        n = num;
    }
    for (int i = 0; i < n; i++) {
        double pos = positions[i] * 0.5; // CGB * 100.0;
        if (pos > 1.0) {
            pos = 1.0;
        }
        FCS->SetThrottleCmd(i, pos);
    }
    return n;
}

//------------------------------------------------------------------------------
// reset() -- 
//------------------------------------------------------------------------------
void JSBSimModel::reset()
{
    BaseClass::reset();

    pitchTrimPos  = (LCreal)0.0;
    pitchTrimRate = (LCreal)0.1;
    pitchTrimSw   = (LCreal)0.0;
    rollTrimPos   = (LCreal)0.0;
    rollTrimRate  = (LCreal)0.1;
    rollTrimSw    = (LCreal)0.0;

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

    // must have strings set
    if (rootDir == 0 || model == 0) return;

    // Must also have the JSBSim object
    if (fdmex == 0) {
        // must have a JSBSim property manager
        if (propMgr == 0) {
            propMgr = new JSBSim::FGPropertyManager();
        }
        fdmex = new JSBSim::FGFDMExec(propMgr);

        std::string RootDir(rootDir->getString());
        fdmex->SetAircraftPath(RootDir + "aircraft");
        fdmex->SetEnginePath(RootDir + "engine");
        fdmex->SetSystemsPath(RootDir + "systems"); // JSBSim-1.0 or after only

        fdmex->LoadModel(model->getString());
        JSBSim::FGPropertyManager* propMgr = fdmex->GetPropertyManager();
        if (propMgr != 0) {
            hasHeadingHold = propMgr->HasNode("ap/heading_hold") && propMgr->HasNode("ap/heading_setpoint");
            hasVelocityHold = propMgr->HasNode("ap/airspeed_hold") && propMgr->HasNode("ap/airspeed_setpoint");
            hasAltitudeHold = propMgr->HasNode("ap/altitude_hold") && propMgr->HasNode("ap/altitude_setpoint");
#if 0
            // CGB this isn't working for some reason. I set the values directly in "dynamics" for now.
            if (hasHeadingHold) {
                propMgr->Tie("ap/heading_hold", this, &JSBSimModel::isHeadingHoldOn);
                propMgr->Tie("ap/heading_setpoint", this, &JSBSimModel::getCommandedHeadingD);
            }
            if (hasVelocityHold) {
                propMgr->Tie("ap/airspeed_hold", this, &JSBSimModel::isVelocityHoldOn);
                propMgr->Tie("ap/airspeed_setpoint", this, &JSBSimModel::getCommandedVelocityKts);
            }
            if (hasAltitudeHold) {
                propMgr->Tie("ap/altitude_hold", this, &JSBSimModel::isAltitudeHoldOn);
                propMgr->Tie("ap/altitude_setpoint", this, &JSBSimModel::getCommandedAltitude * Basic::Distance::M2FT);
            }
#endif
        }
    }
    
#if 0
    // CGB TBD
    reset = 0;
    freeze = 0;
#endif
    JSBSim::FGInitialCondition* fgic = fdmex->GetIC();
    if (fgic == 0) return;

    fgic->SetAltitudeASLFtIC(Basic::Distance::M2FT * p->getAltitude());

#if 0
    fgic->SetTrueHeadingDegIC(Basic::Angle::R2DCC * p->getHeading());
    fgic->SetRollAngleDegIC(Basic::Angle::R2DCC * p->getRoll());
    fgic->SetPitchAngleDegIC(Basic::Angle::R2DCC * p->getPitch());
#else
    fgic->SetPsiDegIC(Basic::Angle::R2DCC * p->getHeading());
    fgic->SetPhiDegIC(Basic::Angle::R2DCC * p->getRoll());
    fgic->SetThetaDegIC(Basic::Angle::R2DCC * p->getPitch());
#endif
    fgic->SetVtrueKtsIC(Basic::Distance::M2NM * p->getTotalVelocity() * 3600.0f);
    fgic->SetLatitudeDegIC(p->getInitLatitude());
    fgic->SetLongitudeDegIC(p->getInitLongitude());

    JSBSim::FGPropulsion* Propulsion = fdmex->GetPropulsion();
    JSBSim::FGFCS* FCS = fdmex->GetFCS();
    if (Propulsion != 0 && FCS != 0) {
        Propulsion->SetMagnetos(3);
        for (unsigned int i=0; i < Propulsion->GetNumEngines(); i++) {
            FCS->SetMixtureCmd(i, 1.0);
            FCS->SetThrottleCmd(i, 1.0);
            FCS->SetPropAdvanceCmd(i, 1.0);
            FCS->SetMixturePos(i, 1.0);
            FCS->SetThrottlePos(i, 1.0);
            FCS->SetPropAdvance(i, 1.0);
            JSBSim::FGEngine* eng = Propulsion->GetEngine(i);
            eng->SetRunning(true);
            JSBSim::FGThruster* thruster = eng->GetThruster();
            thruster->SetRPM(1000.0);
        }
        Propulsion->SetFuelFreeze(p->isFuelFrozen());
        Propulsion->InitRunning(-1);     // -1 refers to "All Engines"
        Propulsion->GetSteadyState();
//.........这里部分代码省略.........

//------------------------------------------------------------------------------
// 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();
//.........这里部分代码省略.........