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