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);
}
//------------------------------------------------------------------------------
// setRudderPedalInput(pedal) -- Pedal inputs: normalized
// pedal: -1.0 -> max left; 0.0 -> center; 1.0 -> max right
//------------------------------------------------------------------------------
void JSBSimModel::setRudderPedalInput(const double pedal)
{
if (fdmex == nullptr) return;
JSBSim::FGFCS* FCS = fdmex->GetFCS();
if (FCS == nullptr) 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);
}
//------------------------------------------------------------------------------
// setGearHandleSwitch() -- Set Gear handle switch position
// 0 -> handle up; 1 -> handle down
//------------------------------------------------------------------------------
void JSBSimModel::setGearHandleSwitch(const double sw)
{
if (fdmex == nullptr) return;
JSBSim::FGFCS* FCS = fdmex->GetFCS();
if (FCS == nullptr) return;
FCS->SetGearCmd(sw);
}
//------------------------------------------------------------------------------
// setControlStickRollInput(Roll) -- Control inputs: normalized
// roll: -1.0 -> max left; 0.0 -> center; 1.0 -> max right
//------------------------------------------------------------------------------
void JSBSimModel::setControlStickRollInput(const double roll)
{
if (fdmex == nullptr) return;
JSBSim::FGFCS* FCS = fdmex->GetFCS();
if (FCS == nullptr) return;
FCS->SetDaCmd(roll);
}
//------------------------------------------------------------------------------
// setBrakes() -- Sets brake positions (left & right)
// No brake force -> 0.0
// Max brake force -> 1.0
//------------------------------------------------------------------------------
void JSBSimModel::setBrakes(const double left, const double right)
{
if (fdmex == nullptr) return;
JSBSim::FGFCS* FCS = fdmex->GetFCS();
if (FCS == nullptr) return;
FCS->SetLBrake(left);
FCS->SetRBrake(right);
FCS->SetCBrake(0.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();
}
fdmex->RunIC();
}
//------------------------------------------------------------------------------
// 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);
}