Eigen::MatrixXd cinv(const Eigen::MatrixXd M, const Eigen::VectorXi TS, const Eigen::MatrixXd H, const double tol){ // ****************************************************************************************************** // Function that implements the continuous inverse as in: // Mansard, N., et al.; A Unified Approach to Integrate Unilateral Constraints in the Stack of Tasks // IEEE, Transactions on Robotics, 2009 // // INPUT // - M: matrix from which we want to compute the continuous inverse // - TS: task size vector with 'ts(i)' being the size of task 'i'. The sum of the elements of 'TS' == number of rows of 'M'. // - H: task activation matrix whose diagonal elements \in [0,1] // // OUTPUT // Given B(m) the set of all permutations without repetitions of the first 'm' elements, // that is, B(m==3) = {{1},{2},{3},{1,2},{1,3},{2,3},{1,2,3}} then: // - cinv = sum_{P \in B(m)}( prod_{i \in P}(h_i) * prod_{i !\in P}(1.0 - h_i) * pinv(M_P) ) // with: // - m = TS.size() // - M_P = H0_P * M and H0_P as the diagonal matrix with HO_P(j,j) = 1 if j \in P, otherwise HO_P(j,j) = 0. Eigen::VectorXd vH(H.rows()); for (unsigned int i=0; i<vH.size(); ++i) vH(i) = H(i,i); return cinv(M, TS, vH, tol); }
double FGPropeller::Calculate(double PowerAvailable) { double omega, alpha, beta; double Vel = fdmex->GetAuxiliary()->GetAeroUVW(eU); double rho = fdmex->GetAtmosphere()->GetDensity(); double RPS = RPM/60.0; if (RPS > 0.00) J = Vel / (Diameter * RPS); // Calculate J normally else J = 1000.0; // Set J to a high number if (MaxPitch == MinPitch) ThrustCoeff = cThrust->GetValue(J); else ThrustCoeff = cThrust->GetValue(J, Pitch); ThrustCoeff *= CtFactor; if (P_Factor > 0.0001) { alpha = fdmex->GetAuxiliary()->Getalpha(); beta = fdmex->GetAuxiliary()->Getbeta(); SetActingLocationY( GetLocationY() + P_Factor*alpha*Sense); SetActingLocationZ( GetLocationZ() + P_Factor*beta*Sense); } Thrust = ThrustCoeff*RPS*RPS*D4*rho; omega = RPS*2.0*M_PI; vFn(1) = Thrust; // The Ixx value and rotation speed given below are for rotation about the // natural axis of the engine. The transform takes place in the base class // FGForce::GetBodyForces() function. vH(eX) = Ixx*omega*Sense; vH(eY) = 0.0; vH(eZ) = 0.0; if (omega > 0.0) ExcessTorque = GearRatio * PowerAvailable / omega; else ExcessTorque = GearRatio * PowerAvailable / 1.0; RPM = (RPS + ((ExcessTorque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0; if (RPM < 1.0) RPM = 0; // Engine friction stops rotation arbitrarily at 1 RPM. vMn = fdmex->GetPropagate()->GetPQR()*vH + vTorque; return Thrust; // return thrust in pounds }
double FGPropeller::Calculate(double EnginePower) { FGColumnVector3 localAeroVel = Transform().Transposed() * in.AeroUVW; double omega, PowerAvailable; double Vel = localAeroVel(eU); double rho = in.Density; double RPS = RPM/60.0; // Calculate helical tip Mach double Area = 0.25*Diameter*Diameter*M_PI; double Vtip = RPS * Diameter * M_PI; HelicalTipMach = sqrt(Vtip*Vtip + Vel*Vel) / in.Soundspeed; PowerAvailable = EnginePower - GetPowerRequired(); if (RPS > 0.0) J = Vel / (Diameter * RPS); // Calculate J normally else J = Vel / Diameter; if (MaxPitch == MinPitch) { // Fixed pitch prop ThrustCoeff = cThrust->GetValue(J); } else { // Variable pitch prop ThrustCoeff = cThrust->GetValue(J, Pitch); } // Apply optional scaling factor to Ct (default value = 1) ThrustCoeff *= CtFactor; // Apply optional Mach effects from CT_MACH table if (CtMach) ThrustCoeff *= CtMach->GetValue(HelicalTipMach); Thrust = ThrustCoeff*RPS*RPS*D4*rho; // Induced velocity in the propeller disk area. This formula is obtained // from momentum theory - see B. W. McCormick, "Aerodynamics, Aeronautics, // and Flight Mechanics" 1st edition, eqn. 6.15 (propeller analysis chapter). // Since Thrust and Vel can both be negative we need to adjust this formula // To handle sign (direction) separately from magnitude. double Vel2sum = Vel*abs(Vel) + 2.0*Thrust/(rho*Area); if( Vel2sum > 0.0) Vinduced = 0.5 * (-Vel + sqrt(Vel2sum)); else Vinduced = 0.5 * (-Vel - sqrt(-Vel2sum)); // We need to drop the case where the downstream velocity is opposite in // direction to the aircraft velocity. For example, in such a case, the // direction of the airflow on the tail would be opposite to the airflow on // the wing tips. When such complicated airflows occur, the momentum theory // breaks down and the formulas above are no longer applicable // (see H. Glauert, "The Elements of Airfoil and Airscrew Theory", // 2nd edition, §16.3, pp. 219-221) if ((Vel+2.0*Vinduced)*Vel < 0.0) { // The momentum theory is no longer applicable so let's assume the induced // saturates to -0.5*Vel so that the total velocity Vel+2*Vinduced equals 0. Vinduced = -0.5*Vel; } // P-factor is simulated by a shift of the acting location of the thrust. // The shift is a multiple of the angle between the propeller shaft axis // and the relative wind that goes through the propeller disk. if (P_Factor > 0.0001) { double tangentialVel = localAeroVel.Magnitude(eV, eW); if (tangentialVel > 0.0001) { double angle = atan2(tangentialVel, localAeroVel(eU)); double factor = Sense * P_Factor * angle / tangentialVel; SetActingLocationY( GetLocationY() + factor * localAeroVel(eW)); SetActingLocationZ( GetLocationZ() + factor * localAeroVel(eV)); } } omega = RPS*2.0*M_PI; vFn(eX) = Thrust; // The Ixx value and rotation speed given below are for rotation about the // natural axis of the engine. The transform takes place in the base class // FGForce::GetBodyForces() function. vH(eX) = Ixx*omega*Sense; vH(eY) = 0.0; vH(eZ) = 0.0; if (omega > 0.0) ExcessTorque = PowerAvailable / omega; else ExcessTorque = PowerAvailable / 1.0; RPM = (RPS + ((ExcessTorque / Ixx) / (2.0 * M_PI)) * deltaT) * 60.0; if (RPM < 0.0) RPM = 0.0; // Engine won't turn backwards // Transform Torque and momentum first, as PQR is used in this // equation and cannot be transformed itself. vMn = in.PQR*(Transform()*vH) + Transform()*vTorque; return Thrust; // return thrust in pounds }
double FGPropeller::Calculate(double EnginePower) { FGColumnVector3 localAeroVel = Transform().Transposed() * in.AeroUVW; double omega, PowerAvailable; double Vel = localAeroVel(eU); double rho = in.Density; double RPS = RPM/60.0; // Calculate helical tip Mach double Area = 0.25*Diameter*Diameter*M_PI; double Vtip = RPS * Diameter * M_PI; HelicalTipMach = sqrt(Vtip*Vtip + Vel*Vel) / in.Soundspeed; if (RPS > 0.0) J = Vel / (Diameter * RPS); // Calculate J normally else J = Vel / Diameter; PowerAvailable = EnginePower - GetPowerRequired(); if (MaxPitch == MinPitch) { // Fixed pitch prop ThrustCoeff = cThrust->GetValue(J); } else { // Variable pitch prop ThrustCoeff = cThrust->GetValue(J, Pitch); } // Apply optional scaling factor to Ct (default value = 1) ThrustCoeff *= CtFactor; // Apply optional Mach effects from CT_MACH table if (CtMach) ThrustCoeff *= CtMach->GetValue(HelicalTipMach); Thrust = ThrustCoeff*RPS*RPS*D4*rho; // Induced velocity in the propeller disk area. This formula is obtained // from momentum theory - see B. W. McCormick, "Aerodynamics, Aeronautics, // and Flight Mechanics" 1st edition, eqn. 6.15 (propeller analysis chapter). // Since Thrust and Vel can both be negative we need to adjust this formula // To handle sign (direction) separately from magnitude. double Vel2sum = Vel*abs(Vel) + 2.0*Thrust/(rho*Area); if( Vel2sum > 0.0) Vinduced = 0.5 * (-Vel + sqrt(Vel2sum)); else Vinduced = 0.5 * (-Vel - sqrt(-Vel2sum)); // P-factor is simulated by a shift of the acting location of the thrust. // The shift is a multiple of the angle between the propeller shaft axis // and the relative wind that goes through the propeller disk. if (P_Factor > 0.0001) { double tangentialVel = localAeroVel.Magnitude(eV, eW); if (tangentialVel > 0.0001) { // The angle made locally by the air flow with respect to the propeller // axis is influenced by the induced velocity. This attenuates the // influence of a string cross wind and gives a more realistic behavior. double angle = atan2(tangentialVel, Vel+Vinduced); double factor = Sense * P_Factor * angle / tangentialVel; SetActingLocationY( GetLocationY() + factor * localAeroVel(eW)); SetActingLocationZ( GetLocationZ() + factor * localAeroVel(eV)); } } omega = RPS*2.0*M_PI; vFn(eX) = Thrust; // The Ixx value and rotation speed given below are for rotation about the // natural axis of the engine. The transform takes place in the base class // FGForce::GetBodyForces() function. FGColumnVector3 vH(Ixx*omega*Sense*Sense_multiplier, 0.0, 0.0); if (omega > 0.0) ExcessTorque = PowerAvailable / omega; else ExcessTorque = PowerAvailable / 1.0; RPM = (RPS + ((ExcessTorque / Ixx) / (2.0 * M_PI)) * in.TotalDeltaT) * 60.0; if (RPM < 0.0) RPM = 0.0; // Engine won't turn backwards // Transform Torque and momentum first, as PQR is used in this // equation and cannot be transformed itself. vMn = in.PQRi*(Transform()*vH) + Transform()*vTorque; return Thrust; // return thrust in pounds }