// Dynamics model interface
bool LaeroModel::setCommandedHeadingD(const double h, const double hDps, const double maxBank)
{
//-------------------------------------------------------
// get data pointers
//-------------------------------------------------------
Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
bool ok = (pPlr != nullptr);
if (ok) {
//----------------------------------------------------
// define local constants
//----------------------------------------------------
const double MAX_BANK_RAD = maxBank * Basic::Angle::D2RCC;
//const double TAU = 2.0; // time constant [sec]
const double TAU = 1.0; // time constant [sec]
//-------------------------------------------------------
// get current data
//-------------------------------------------------------
double velMps = pPlr->getTotalVelocity();
double hdgDeg = pPlr->getHeadingD();
double hdgErrDeg = Basic::Angle::aepcdDeg(h - hdgDeg);
double hdgErrAbsDeg = std::fabs(hdgErrDeg);
//-------------------------------------------------------
// get absolute heading rate of change (hdgDotAbsDps)
//-------------------------------------------------------
double hdgDotMaxAbsRps = Eaagles::ETHGM * std::tan(MAX_BANK_RAD) / velMps;
double hdgDotMaxAbsDps = hdgDotMaxAbsRps * Basic::Angle::R2DCC;
double hdgDotAbsDps = hDps;
if (hdgDotAbsDps > hdgDotMaxAbsDps) {
hdgDotAbsDps = hdgDotMaxAbsDps;
}
double hdgErrBrkAbsDeg = TAU * hdgDotAbsDps;
if (hdgErrAbsDeg < hdgErrBrkAbsDeg) {
hdgDotAbsDps = hdgErrAbsDeg / TAU;
}
//-------------------------------------------------------
// define direction of heading rate of change (hdgDotDps)
//-------------------------------------------------------
double hdgDotDps = sign(hdgErrDeg) * hdgDotAbsDps;
psiDot = hdgDotDps * Basic::Angle::D2RCC;
//-------------------------------------------------------
// define bank angle as a function of turn rate
//-------------------------------------------------------
double phiCmdDeg = std::atan2(psiDot * velMps, Eaagles::ETHGM) * Basic::Angle::R2DCC;
ok = flyPhi(phiCmdDeg);
}
return ok;
}
//------------------------------------------------------------------------------
// 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();
}
//------------------------------------------------------------------------------
// 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);
}