//------------------------------------------------------------------------------
// 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);
}
//------------------------------------------------------------------------------
// 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);
}