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