//------------------------------------------------------------------------------
// stepOwnshipPlayer() -- Step to the next local player
//------------------------------------------------------------------------------
void SimStation::stepOwnshipPlayer()
{
Basic::PairStream* pl = getSimulation()->getPlayers();
if (pl != nullptr) {
Simulation::Player* f = nullptr;
Simulation::Player* n = nullptr;
bool found = false;
// Find the next player
Basic::List::Item* item = pl->getFirstItem();
while (item != nullptr) {
Basic::Pair* pair = static_cast<Basic::Pair*>(item->getValue());
if (pair != nullptr) {
Simulation::Player* ip = static_cast<Simulation::Player*>(pair->object());
if ( ip->isMode(Simulation::Player::ACTIVE) &&
ip->isLocalPlayer() &&
ip->isClassType(typeid(Simulation::AirVehicle))
) {
if (f == nullptr) { f = ip; } // Remember the first
if (found) { n = ip; ; break; }
if (ip == getOwnship()) found = true;
}
}
item = item->getNext();
}
if (found && n == nullptr) n = f;
if (n != nullptr) setOwnshipPlayer(n);
pl->unref();
}
}
// 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() -- sets up our initial flying values
//----------------------------------------------------------
void LaeroModel::reset()
{
BaseClass::reset();
Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
if (pPlr != nullptr) {
LCreal initVel = pPlr->getInitVelocity();
u = initVel * Basic::Distance::NM2M / Basic::Time::H2S;
}
}
// setCommandedVelocityKts() - also can limit velocity rate of acceleration
bool LaeroModel::setCommandedVelocityKts(const double v, const double vNps)
{
//-------------------------------------------------------
// 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 KTS2MPS = Basic::Distance::NM2M / Basic::Time::H2S;
//-------------------------------------------------------
// convert argument units (deg -> rad)
//-------------------------------------------------------
double velCmdMps = v * KTS2MPS;
double velDotCmdMps2 = vNps * KTS2MPS;
//-------------------------------------------------------
// current vel error (rad)
//-------------------------------------------------------
double velMps = pPlr->getTotalVelocityKts() * KTS2MPS;
double velErrMps = velCmdMps - velMps;
//-------------------------------------------------------
// vel error break point (rad)
//-------------------------------------------------------
const double TAU = 1.0; // time constant [sec]
double velErrBrkMps = velDotCmdMps2 * TAU;
//-------------------------------------------------------
// control signal for commanded vel (rps)
//-------------------------------------------------------
double velDotMps2 = sign(velErrMps) * velDotCmdMps2;
if (std::abs(velErrMps) < velErrBrkMps) {
velDotMps2 = (velErrMps / velErrBrkMps) * velDotCmdMps2;
}
//-------------------------------------------------------
// assign result to velocity control
//-------------------------------------------------------
uDot = velDotMps2;
}
return true;
}
// Dynamics model interface - all input values in meters
bool LaeroModel::setCommandedAltitude(const double a, const double aMps, const double maxPitch)
{
//-------------------------------------------------------
// 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 TAU = 4.0; // time constant [sec]
//-------------------------------------------------------
// get current alt error (mtr)
//-------------------------------------------------------
double altMtr = pPlr->getAltitude();
double altErrMtr = a - altMtr;
//-------------------------------------------------------
// get alt error break point (mtr)
//-------------------------------------------------------
double altDotCmdMps = aMps;
double altErrBrkMtr = altDotCmdMps * TAU;
//-------------------------------------------------------
// get commanded altDot (mps)
//-------------------------------------------------------
double altDotMps = sign(altErrMtr) * altDotCmdMps;
if (std::abs(altErrMtr) < altErrBrkMtr) {
altDotMps = altErrMtr * (altDotCmdMps / altErrBrkMtr);
}
//-------------------------------------------------------
// assign result to altitude control
//-------------------------------------------------------
double thtCmdDeg = (altDotMps / u) * Basic::Angle::R2DCC;
// SLS - TO DO: Limit commanded pitch to max pitch angle as well.
ok = flyTht(thtCmdDeg);
}
return ok;
}
bool LaeroModel::flyPsi(const double psiCmdDeg, const double psiDotCmdDps)
{
//-------------------------------------------------------
// get data pointers
//-------------------------------------------------------
Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
bool ok = (pPlr != nullptr);
if (ok) {
//-------------------------------------------------------
// convert argument units (deg -> rad)
//-------------------------------------------------------
double psiCmdRad = psiCmdDeg * Basic::Angle::D2RCC;
double psiDotCmdRps = psiDotCmdDps * Basic::Angle::D2RCC;
//-------------------------------------------------------
// current psi error (rad)
//-------------------------------------------------------
double psiRad = pPlr->getHeadingR();
double psiErrRad = psiCmdRad - psiRad;
//-------------------------------------------------------
// psi error break point (rad)
//-------------------------------------------------------
const double TAU = 1.0; // time constant [sec]
double psiErrBrkRad = psiDotCmdRps * TAU;
//-------------------------------------------------------
// control signal for commanded psi (rps)
//-------------------------------------------------------
double psiDotRps = sign(psiErrRad) * psiDotCmdRps;
if (std::abs(psiErrRad) < psiErrBrkRad) {
psiDotRps = (psiErrRad / psiErrBrkRad) * psiDotCmdRps;
}
//-------------------------------------------------------
// assign result to azimuth control
//-------------------------------------------------------
psiDot = psiDotRps;
}
return ok;
}
bool LaeroModel::flyTht(const double thtCmdDeg, const double thtDotCmdDps)
{
//-------------------------------------------------------
// get data pointers
//-------------------------------------------------------
Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
bool ok = (pPlr != nullptr);
if (ok) {
//-------------------------------------------------------
// convert argument units (deg -> rad)
//-------------------------------------------------------
double thtCmdRad = thtCmdDeg * Basic::Angle::D2RCC;
double thtDotCmdRps = thtDotCmdDps * Basic::Angle::D2RCC;
//-------------------------------------------------------
// current tht error (rad)
//-------------------------------------------------------
double thtRad = pPlr->getPitchR();
double thtErrRad = thtCmdRad - thtRad;
//-------------------------------------------------------
// tht error break point (rad)
//-------------------------------------------------------
const double TAU = 1.0; // time constant [sec]
double thtErrBrkRad = thtDotCmdRps * TAU;
//-------------------------------------------------------
// control signal for commanded tht (rps)
//-------------------------------------------------------
double thtDotRps = sign(thtErrRad) * thtDotCmdRps;
if (std::abs(thtErrRad) < thtErrBrkRad) {
thtDotRps = (thtErrRad / thtErrBrkRad) * thtDotCmdRps;
}
//-------------------------------------------------------
// assign result to pitch control
//-------------------------------------------------------
thtDot = thtDotRps;
}
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();
}
//------------------------------------------------------------------------------
// 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);
}
//------------------------------------------------------------------------------
// weaponFireMsgFactory() -- (Output support) Weapon fire message factory
//------------------------------------------------------------------------------
bool Nib::weaponFireMsgFactory(const LCreal)
{
bool ok = true;
//std::cout << "NetIO::weaponFireMsgFactory() HERE!!" << std::endl;
// Get our NetIO
NetIO* disIO = (NetIO*)(getNetIO());
//Simulation* sim = disIO->getSimulation();
// Set the NIB mode so that we don't do this again.
setMode(Simulation::Player::ACTIVE);
// Our NIB's player is a weapon that just became active
Simulation::Weapon* mPlayer = (Simulation::Weapon*)(getPlayer());
// Ok, we have the weapon, now get the firing and target players
Simulation::Player* tPlayer = mPlayer->getTargetPlayer();
Simulation::Player* fPlayer = mPlayer->getLaunchVehicle();
if (fPlayer == 0) return false;
// ---
// PDU header
// ---
FirePDU pdu;
pdu.header.protocolVersion = disIO->getVersion();
pdu.header.exerciseIdentifier = disIO->getExerciseID();
pdu.header.PDUType = NetIO::PDU_FIRE;
pdu.header.protocolFamily = NetIO::PDU_FAMILY_WARFARE;
pdu.header.timeStamp = disIO->timeStamp();
pdu.header.length = sizeof(FirePDU);
// ---
// Set the PDU data with the firing (launcher) player's id
// ---
pdu.firingEntityID.ID = fPlayer->getID();
pdu.firingEntityID.simulationID.siteIdentification = disIO->getSiteID();
pdu.firingEntityID.simulationID.applicationIdentification = disIO->getApplicationID();
// ---
// Set the PDU data with the munition's ID
// ---
pdu.munitionID.ID = mPlayer->getID();
pdu.munitionID.simulationID.siteIdentification = disIO->getSiteID();
pdu.munitionID.simulationID.applicationIdentification = disIO->getApplicationID();
// ---
// Set the PDU data with the target's ID
// ---
{
bool tOk = false;
if (tPlayer != 0) {
pdu.targetEntityID.ID = tPlayer->getID();
if (tPlayer->isLocalPlayer()) {
// Local player, use our site/app/exerc IDs
pdu.targetEntityID.simulationID.siteIdentification = disIO->getSiteID();
pdu.targetEntityID.simulationID.applicationIdentification = disIO->getApplicationID();
tOk = true;
}
else {
const Nib* fNIB = dynamic_cast<const Nib*>( tPlayer->getNib() );
if (fNIB != 0) {
// Networked player, use it's NIB's IDs
pdu.targetEntityID.simulationID.siteIdentification = fNIB->getSiteID();
pdu.targetEntityID.simulationID.applicationIdentification = fNIB->getApplicationID();
tOk = true;
}
}
}
if (!tOk) {
// Networked player, use it's NIB's IDs
pdu.targetEntityID.ID = 0;
pdu.targetEntityID.simulationID.siteIdentification = 0;
pdu.targetEntityID.simulationID.applicationIdentification = 0;
}
}
// ---
// Event ID
// ---
pdu.eventID.simulationID.siteIdentification = disIO->getSiteID();
pdu.eventID.simulationID.applicationIdentification = disIO->getApplicationID();
pdu.eventID.eventNumber = mPlayer->getReleaseEventID();
// ---
// Location & Velociy
// ---
// World Coordinates
osg::Vec3d geocPos = mPlayer->getGeocPosition();
pdu.location.X_coord = geocPos[Basic::Nav::IX];
pdu.location.Y_coord = geocPos[Basic::Nav::IY];
pdu.location.Z_coord = geocPos[Basic::Nav::IZ];
// Velocity
osg::Vec3d geocVel = mPlayer->getGeocVelocity();
pdu.velocity.component[0] = (float)geocVel[Basic::Nav::IX];
pdu.velocity.component[1] = (float)geocVel[Basic::Nav::IY];
pdu.velocity.component[2] = (float)geocVel[Basic::Nav::IZ];
// ---
// Burst
// ---
pdu.burst.munision.kind = getEntityKind();
pdu.burst.munision.domain = getEntityDomain();
pdu.burst.munision.country = getEntityCountry();
pdu.burst.munision.category = getEntityCategory();
pdu.burst.munision.subcategory = getEntitySubcategory();
pdu.burst.munision.specific = getEntitySpecific();
pdu.burst.munision.extra = getEntityExtra();
pdu.burst.warhead = 0;
pdu.burst.fuse = 0;;
pdu.burst.quantity = 1;
pdu.burst.rate = 0;
// ---
// Range and fire index
// ---
pdu.fireMissionIndex = 0;
pdu.range = 0.0;
//pdu.dumpData();
//std::cout << "NetIO::weaponFireMsgFactory() fired:";
//std::cout << "(" << pdu.firingEntityID.ID;
//std::cout << "," << pdu.firingEntityID.simulationID.applicationIdentification ;
//std::cout << "," << pdu.firingEntityID.simulationID.siteIdentification;
//std::cout << ") munision:";
//std::cout << "(" << pdu.munitionID.ID;
//std::cout << "," << pdu.munitionID.simulationID.applicationIdentification ;
//std::cout << "," << pdu.munitionID.simulationID.siteIdentification;
//std::cout << ")" << std::endl;
if (Basic::NetHandler::isNotNetworkByteOrder()) pdu.swapBytes();
ok = disIO->sendData((char*)&pdu,sizeof(pdu));
return ok;
}
// ----------------------------------------------------------------------------
// buttonEvent() - called when a button is pressed
// ----------------------------------------------------------------------------
void MapDisplay::buttonEvent(const int b)
{
// cmdRange up, down
MapPage* page = static_cast<MapPage*>(subpage());
// cmdAirspeed, cmdAltitude, cmdHeading up, down
Simulation::Player* pA = getOwnship();
Simulation::Autopilot* ap = 0;
if (pA != 0) {
ap = static_cast<Simulation::Autopilot*>(pA->getPilot());
}
if (page != 0 && ap != 0) {
if (b == DEC_RANGE) {
if (page->getRange() > 5) {
page->setRange(page->getRange() - 5);
}
}
else if (b == INC_RANGE) {
if (page->getRange() < 320) {
page->setRange(page->getRange() + 5);
}
}
else if (b == DEC_CMD_AS) {
double cmdAirspeed = ap->getCommandedVelocityKts();
if (cmdAirspeed > 100) {
cmdAirspeed -= 10;
ap->setCommandedVelocityKts(cmdAirspeed);
}
}
else if (b == INC_CMD_AS) {
double cmdAirspeed = ap->getCommandedVelocityKts();
if (cmdAirspeed < 400) {
cmdAirspeed += 10;
ap->setCommandedVelocityKts(cmdAirspeed);
}
}
else if (b == DEC_CMD_ALT) {
double cmdAltitude = ap->getCommandedAltitudeFt();
if (cmdAltitude > 1000) {
cmdAltitude -= 500;
ap->setCommandedAltitudeFt(cmdAltitude);
}
}
else if (b == INC_CMD_ALT) {
double cmdAltitude = ap->getCommandedAltitudeFt();
if (cmdAltitude < 40000) {
cmdAltitude += 500;
ap->setCommandedAltitudeFt(cmdAltitude);
}
}
else if (b == DEC_CMD_HDG) {
double cmdHeading = ap->getCommandedHeadingD();
cmdHeading -= 10;
if (cmdHeading < -180.0) cmdHeading += 360;
ap->setCommandedHeadingD(cmdHeading);
}
else if (b == INC_CMD_HDG) {
double cmdHeading = ap->getCommandedHeadingD();
cmdHeading += 10;
if (cmdHeading > 180.0) cmdHeading -= 360;
ap->setCommandedHeadingD(cmdHeading);
}
else if (b == INC_CMD_AS_NPS) {
double maxAccel = ap->getMaxVelAcc();
if (maxAccel < 20) maxAccel++;
ap->setMaxVelAccNps(maxAccel);
}
else if (b == DEC_CMD_AS_NPS) {
double maxAccel = ap->getMaxVelAcc();
if (maxAccel > 1) maxAccel--;
ap->setMaxVelAccNps(maxAccel);
}
// Climb rate in meters per second
else if (b == INC_CMD_ALT_MPS) {
double maxClimb = ap->getMaxClimbRate();
if (maxClimb < 100) maxClimb += 5;
ap->setMaxClimbRateMps(maxClimb);
}
else if (b == DEC_CMD_ALT_MPS) {
double maxClimb = ap->getMaxClimbRate();
if (maxClimb > 5) maxClimb -= 5;
ap->setMaxClimbRateMps(maxClimb);
}
// Turn rate in degrees per second
else if (b == INC_CMD_HDG_ROT) {
double maxTR = ap->getMaxTurnRate();
if (maxTR < 25) maxTR++;
ap->setMaxTurnRateDps(maxTR);
}
else if (b == DEC_CMD_HDG_ROT) {
double maxTR = ap->getMaxTurnRate();
if (maxTR > 0) maxTR--;
ap->setMaxTurnRateDps(maxTR);
}
// Max bank (degrees)
else if (b == INC_CMD_HDG_BNK) {
double maxBank = ap->getMaxBankAngle();
if (maxBank < 90) maxBank++;
ap->setMaxBankAngleDeg(maxBank);
}
else if (b == DEC_CMD_HDG_BNK) {
double maxBank = ap->getMaxBankAngle();
if (maxBank > 0) maxBank--;
ap->setMaxBankAngleDeg(maxBank);
}
else if (b == PASSIVE_ENABLE) {
passiveEnable = true;
}
else if (b == PASSIVE_DISABLE) {
passiveEnable = false;
}
// get our autopilot mode and change it
else if (b == CHANGE_AP_MODE) {
// if off, go to nav
bool navMode = ap->isNavModeOn();
bool loiterMode = ap->isLoiterModeOn();
bool flMode = ap->isFollowTheLeadModeOn();
// if in nav mode, go to loiter mode
if (navMode) {
ap->setNavMode(false);
ap->setLoiterMode(true);
}
// if in loiter mode, go to follow the lead mode
else if (loiterMode) {
ap->setLoiterMode(false);
ap->setFollowTheLeadMode(true);
}
// if in follow the lead mode, turn off all modes
else if (flMode) {
ap->setFollowTheLeadMode(false);
}
// all modes off, go back to nav
else ap->setNavMode(true);
}
}
}
//------------------------------------------------------------------------------
// drawFunc()
//------------------------------------------------------------------------------
void Display::drawFunc()
{
Simulation::Player* own = getOwnship();
const Basic::Pair* pair = nullptr;
if (own != nullptr) pair = own->getSensorByType(typeid(RealBeamRadar));
const RealBeamRadar* rdr = nullptr;
if (pair != nullptr) rdr = static_cast<const RealBeamRadar*>( pair->object() );
const GLubyte* image = nullptr; // The image pixels
if (rdr != nullptr) image = rdr->getImage();
if (image != nullptr) {
// Image width (number of columns)
GLsizei imgWidth = rdr->getImageWidth();
// Image height (number of rows)
GLsizei imgHeight = rdr->getImageHeight();
GLint pixelSize = rdr->getPixelSize();
GLenum format = GL_RGB;
if (pixelSize == 4) format = GL_RGBA;
if (testTexture) {
// ---
// Draw using texture map
// ---
glEnable(GL_TEXTURE_2D);
glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_DECAL);
if (texture == 0) {
glGenTextures(1, &texture);
}
glBindTexture(GL_TEXTURE_2D, texture);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
// double start = getComputerTime();
glTexImage2D(GL_TEXTURE_2D, 0, pixelSize, imgWidth, imgHeight, 0, format, GL_UNSIGNED_BYTE, image);
// B SCAN
glBegin(GL_POLYGON);
glTexCoord2f( 1.0f, 1.0f );
glVertex2f( 1.0f, 1.0f );
glTexCoord2f( 0.0f, 1.0f );
glVertex2f( 0.0f, 1.0f );
glTexCoord2f( 0.0f, 0.0f );
glVertex2f( 0.0f, 0.0f );
glTexCoord2f( 1.0f, 0.0f );
glVertex2f( 1.0f, 0.0f );
glEnd();
glDisable(GL_TEXTURE_2D);
// double end = getComputerTime();
// double dtime = (end - start);
//std::cout << "glTexImage2D() dtime = " << dtime << std::endl;
}
else {
// ---
// Draw using glDrawPixels()
// ---
glRasterPos2f(0.0, 0.0);
//double start = getComputerTime();
glDrawPixels(imgWidth, imgHeight, format, GL_UNSIGNED_BYTE, image);
//double end = getComputerTime();
//double dtime = (end - start);
//std::cout << "glDrawPixels() dtime = " << dtime << std::endl;
}
}
}
//------------------------------------------------------------------------------
// 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);
}
//------------------------------------------------------------------------------
// munitionDetonationMsgFactory() -- (Output) Munition detonation message factory
//------------------------------------------------------------------------------
bool Nib::munitionDetonationMsgFactory(const LCreal)
{
// Dummy weapon?
const Simulation::Weapon* ww = dynamic_cast<const Simulation::Weapon*>( getPlayer() );
if (ww != 0) {
if (ww->isDummy()) return true;
}
bool ok = true;
//std::cout << "NetIO::munitionDetonationMsgFactory() HERE!!" << std::endl;
// Get our NetIO
NetIO* disIO = (NetIO*)(getNetIO());
// If our NIB's player just detonated, then it must be a weapon!
Simulation::Weapon* mPlayer = dynamic_cast<Simulation::Weapon*>(getPlayer());
if (mPlayer == 0) return false;
// Ok, we have the weapon, now get the firing and target players
Simulation::Player* tPlayer = mPlayer->getTargetPlayer();
Simulation::Player* fPlayer = mPlayer->getLaunchVehicle();
if (fPlayer == 0) return false;
// ---
// PDU header
// ---
DetonationPDU pdu;
pdu.header.protocolVersion = disIO->getVersion();
pdu.header.PDUType = NetIO::PDU_DETONATION;
pdu.header.protocolFamily = NetIO::PDU_FAMILY_WARFARE;
pdu.header.length = sizeof(DetonationPDU);
pdu.header.exerciseIdentifier = disIO->getExerciseID();
pdu.header.timeStamp = disIO->timeStamp();
pdu.header.status = 0;
pdu.header.padding = 0;
// ---
// Set the PDU data with the firing (launcher) player's id
// ---
pdu.firingEntityID.ID = fPlayer->getID();
pdu.firingEntityID.simulationID.siteIdentification = getSiteID();
pdu.firingEntityID.simulationID.applicationIdentification = getApplicationID();
// ---
// Set the PDU data with the munition's ID
// ---
pdu.munitionID.ID = mPlayer->getID();
pdu.munitionID.simulationID.siteIdentification = getSiteID();
pdu.munitionID.simulationID.applicationIdentification = getApplicationID();
// ---
// Set the PDU data with the target's ID
// ---
{
bool tOk = false;
if (tPlayer != 0) {
pdu.targetEntityID.ID = tPlayer->getID();
if (tPlayer->isLocalPlayer()) {
// Local player, use our site/app/exerc IDs
pdu.targetEntityID.simulationID.siteIdentification = getSiteID();
pdu.targetEntityID.simulationID.applicationIdentification = getApplicationID();
tOk = true;
}
else {
// Networked player, use its NIB's IDs
const Nib* fNIB = dynamic_cast<const Nib*>( tPlayer->getNib() );
if (fNIB != 0) {
pdu.targetEntityID.simulationID.siteIdentification = fNIB->getSiteID();
pdu.targetEntityID.simulationID.applicationIdentification = fNIB->getApplicationID();
tOk = true;
}
}
}
if (!tOk) {
pdu.targetEntityID.ID = 0;
pdu.targetEntityID.simulationID.siteIdentification = 0;
pdu.targetEntityID.simulationID.applicationIdentification = 0;
}
}
// ---
// Event ID
// ---
pdu.eventID.simulationID.siteIdentification = getSiteID();
pdu.eventID.simulationID.applicationIdentification = getApplicationID();
pdu.eventID.eventNumber = mPlayer->getReleaseEventID();
// ---
// Location & Velocity
// ---
// World Coordinates
osg::Vec3d geocPos = mPlayer->getGeocPosition();
pdu.location.X_coord = geocPos[Basic::Nav::IX];
pdu.location.Y_coord = geocPos[Basic::Nav::IY];
pdu.location.Z_coord = geocPos[Basic::Nav::IZ];
// Velocity
osg::Vec3d geocVel = mPlayer->getGeocVelocity();
pdu.velocity.component[0] = (float)geocVel[Basic::Nav::IX];
pdu.velocity.component[1] = (float)geocVel[Basic::Nav::IY];
pdu.velocity.component[2] = (float)geocVel[Basic::Nav::IZ];
// ---
// Burst
// ---
pdu.burst.munition.kind = getEntityKind();
pdu.burst.munition.domain = getEntityDomain();
pdu.burst.munition.country = getEntityCountry();
pdu.burst.munition.category = getEntityCategory();
pdu.burst.munition.subcategory = getEntitySubcategory();
pdu.burst.munition.specific = getEntitySpecific();
pdu.burst.munition.extra = getEntityExtra();
pdu.burst.warhead = 0;
pdu.burst.fuse = 0;;
pdu.burst.quantity = 1;
pdu.burst.rate = 0;
// ---
// Location
// ---
osg::Vec3 lpos = mPlayer->getDetonationLocation();
pdu.locationInEntityCoordinates.component[0] = (float) lpos[0];
pdu.locationInEntityCoordinates.component[1] = (float) lpos[1];
pdu.locationInEntityCoordinates.component[2] = (float) lpos[2];
// ---
// Results
// ---
pdu.detonationResult = (unsigned char)( mPlayer->getDetonationResults() );
pdu.numberOfArticulationParameters = 0;
//std::cout << "NetIO::munitionDetonationMsgFactory() results: " << int(pdu.detonationResult) << std::endl;
//pdu.dumpData();
// ---
// Send the PDU
// ---
if (Basic::NetHandler::isNotNetworkByteOrder()) pdu.swapBytes();
ok = disIO->sendData((char*)&pdu,sizeof(pdu));
// Set the detonation message sent flag so that we don't do this again.
setDetonationMessageSent(true);
return ok;
}
//----------------------------------------------------------
// update4DofModel -- update equations of motion
//----------------------------------------------------------
void LaeroModel::update4DofModel(const LCreal dt)
{
//-------------------------------------------------------
// get data pointers
//-------------------------------------------------------
Simulation::Player* pPlr = static_cast<Simulation::Player*>( findContainerByType(typeid(Simulation::Player)) );
if (pPlr != nullptr) {
//==============================================================
// ROTATIONAL EOM
//==============================================================
//----------------------------------------------------
// integrate Euler angles using Adams-Bashforth
//----------------------------------------------------
phi += 0.5 * (3.0 * phiDot - phiDot1) * dT;
if (phi > PI) phi = -PI;
if (phi < -PI) phi = PI;
tht += 0.5 * (3.0 * thtDot - thtDot1) * dT;
if (tht >= HALF_PI) tht = (HALF_PI - EPSILON);
if (tht <= -HALF_PI) tht = -(HALF_PI - EPSILON);
psi += 0.5 * (3.0 * psiDot - psiDot1) * dT;
if (psi > PI) psi = -PI;
if (psi < -PI) psi = PI;
//----------------------------------------------------
// update Euler angles
//----------------------------------------------------
pPlr->setEulerAngles(phi, tht, psi);
//----------------------------------------------------
// hold current rotational control values for next iteration
//----------------------------------------------------
phiDot1 = phiDot;
thtDot1 = thtDot;
psiDot1 = psiDot;
//----------------------------------------------------
// compute sin, cos of Euler angles
//----------------------------------------------------
double sinPhi = std::sin(phi);
double cosPhi = std::cos(phi);
double sinTht = std::sin(tht);
double cosTht = std::cos(tht);
double sinPsi = std::sin(psi);
double cosPsi = std::cos(psi);
//----------------------------------------------------
// compute local to body axes matrix
//----------------------------------------------------
double l1 = cosTht * cosPsi;
double l2 = cosTht * sinPsi;
double l3 = -sinTht;
double m1 = sinPhi * sinTht * cosPsi - cosPhi * sinPsi;
double m2 = sinPhi * sinTht * sinPsi + cosPhi * cosPsi;
double m3 = sinPhi * cosTht;
double n1 = cosPhi * sinTht * cosPsi + sinPhi * sinPsi;
double n2 = cosPhi * sinTht * sinPsi - sinPhi * cosPsi;
double n3 = cosPhi * cosTht;
//----------------------------------------------------
// update p,q,r angular velocity components (body)
//----------------------------------------------------
p = phiDot + (-sinTht)*psiDot;
q = (cosPhi)*thtDot + (cosTht*sinPhi)*psiDot;
r = (-sinPhi)*thtDot + (cosTht*cosPhi)*psiDot;
//----------------------------------------------------
// update angular velocities
//----------------------------------------------------
pPlr->setAngularVelocities(p, q, r);
//==============================================================
// TRANSLATIONAL EOM
//==============================================================
//----------------------------------------------------
// integrate u,v,w velocity components (body)
//----------------------------------------------------
u += 0.5*(3.0*uDot - uDot1)*dT;
v += 0.5*(3.0*vDot - vDot1)*dT;
w += 0.5*(3.0*wDot - wDot1)*dT;
//----------------------------------------------------
// hold current translational control values for next iteration
//----------------------------------------------------
uDot1 = uDot;
vDot1 = vDot;
wDot1 = wDot;
//----------------------------------------------------
// update velocity in NED system
//----------------------------------------------------
velN = l1*u + m1*v + n1*w;
velE = l2*u + m2*v + n2*w;
velD = l3*u + m3*v + n3*w;
pPlr->setVelocity(velN, velE, velD);
//----------------------------------------------------
// update acceleration in NED system
//----------------------------------------------------
//double A = uDot + w*q - v*r;
//double B = vDot + u*r - w*p;
//double C = wDot + v*p - u*q;
//accN = l1*A + m1*B + n1*C;
//accE = l2*A + m2*B + n2*C;
//accD = l3*A + m3*B + n3*C;
//pPlr->setAcceleration(accN, accE, accD);
}
}