diff --git a/functions/miscellaneous/getXCP.m b/functions/miscellaneous/getXCP.m new file mode 100644 index 0000000000000000000000000000000000000000..6498711c0219891f38a4ae3247180d6f300e265f --- /dev/null +++ b/functions/miscellaneous/getXCP.m @@ -0,0 +1,117 @@ +function [XCPvect, qualityIndex, stabilityCoeffs] = getXCP(coeff, iAlpha, iBeta, input, vars) +% OUTPUT: +% XCPvect, double [1, 7]: +% (1) XCPlon +% (2) XCPlat +% (3) XCPtot - std +% (4) XCP tot - MOD +% (5) XCPlon - aTot +% (6) XCPlat - aTot +% (7) XCPor +% qualityIndex, double[1, 5]: +% (1) Angle between Maero and Faero [rad] +% (2) Angle between Fyz and Myz [rad] +% (3) Angle between Fyz and vRyz [rad] +% (4) Error index T +% (5) Magnitude of Fpar +% stabilityCoeff, double[1, 10]: +% (1) Cma (2) CNA (3) Cm (4) CN (5) Cnb +% (6) CYB (7) Cn (8) CY (9) Cl (10) CA +% (11) CYrot (12) CNrot (13) CmRot (14) CnRot + +XCPvect = nan(7, 1); +qualityIndex = zeros(4, 1); + +%% COEFFICIENTS +if input.flagXCP + Cma = coeff( 6, iAlpha, 1, iBeta); % My D + CNA = -coeff( 5, iAlpha, 1, iBeta); % Fz D + Cm = coeff( 2, iAlpha, 1, iBeta); % My + CN = -coeff( 1, iAlpha, 1, iBeta); % Fz + Cnb = nan; % Mz D + CYB = nan; % Fy D + Cn = coeff( 4, iAlpha, 1, iBeta); % Mz + CY = coeff( 3, iAlpha, 1, iBeta); % Fy + Cl = nan; + CA = nan; + +else + Cma = coeff( 8, iAlpha, 1, iBeta); % My D + CNA = -coeff( 4, iAlpha, 1, iBeta); % Fz D + Cm = coeff( 9, iAlpha, 1, iBeta); % My + CN = -coeff( 5, iAlpha, 1, iBeta); % Fz + Cnb = coeff(12, iAlpha, 1, iBeta); % Mz D + CYB = coeff( 2, iAlpha, 1, iBeta); % Fy D + Cn = coeff(13, iAlpha, 1, iBeta); % Mz + CY = coeff( 3, iAlpha, 1, iBeta); % Fy + CA = -coeff(1, iAlpha, 1, iBeta); + Cl = coeff(6, iAlpha, 1, iBeta); +end + +Fyz = [0, CY, CN]; +Myz = [0, Cm, Cn]; + +Faero = [CA, CY, CN]; +Maero = [Cl, Cm, Cn]; +vRyz = -1.*[0, tand(input.fltcon.BETA(iBeta)), tand(input.fltcon.ALPHA(iAlpha))]; + +%% COMPUTE XCPs + +%%% LONGITUDINAL +if abs(input.fltcon.ALPHA(iAlpha)) < 1 + XCPvect(1) = Cma/CNA; +else + XCPvect(1) = Cm/CN; +end + +%%% LATERAL +if input.fltcon.BETA(iBeta) == 0 && not(vars.useAlphaTot) + XCPvect(2) = -Cnb/CYB; +else + XCPvect(2) = -Cn/CY; +end + +%%% TOTAL FORMULATION + +Mdir = Myz./norm(Myz); + +Fpar = dot(Fyz, Mdir).*Mdir; +Fnorm = Fyz - Fpar; + +%%% TOTAL - STD +sgn = sign(dot(cross(Myz, Fyz), [1 0 0])); +XCPvect(3) = sgn * norm(Myz)/norm(Fyz); + +%%% TOTAL - MOD +sgn = sign(dot(cross(Myz, Fnorm), [1 0 0])); +XCPvect(4) = sgn * norm(Myz)/norm(Fnorm); + +%%% ALPHA TOT - REFERENCE FRAME +[~, phi] = getAlphaPhi(input.fltcon.ALPHA(iAlpha)*pi/180, input.fltcon.BETA(iBeta)*pi/180); +R = [cos(phi) -sin(phi); sin(phi) cos(phi)]; +Frot = R*Fyz(2:3)'; +Mrot = R*Myz(2:3)'; + +if norm(Frot) == 0 + XCPvect(5) = XCPvect(1); % Longitudinal + XCPvect(6) = XCPvect(2); % Lateral +else + XCPvect(5) = Mrot(1)/Frot(2); % Longitudinal + XCPvect(6) = -Mrot(2)/Frot(1); % Lateral +end + +%%% OPEN ROCKET +if vars.checkOpenRocket + angle = getAlphaPhi(input.fltcon.ALPHA(iAlpha)*pi/180, input.fltcon.BETA(iBeta)*pi/180); + XCPvect(7) = aerodynamics(input, angle, vars.modHaack); +end +%% SAVE COEFFICIENTS +stabilityCoeffs = [Cma CNA Cm CN Cnb CYB Cn CY Cl CA Frot(1) Frot(2) Mrot(1) Mrot(2)]'; + +%% COMPUTE QUALITY INDEXES +qualityIndex(1) = acos( dot(Faero, Maero)/( norm(Faero)*norm(Maero) ) ); +qualityIndex(2) = acos( dot(Fyz, Myz)/ (norm(Fyz) * norm(Myz)) ); +qualityIndex(3) = acos( dot(vRyz, Fyz)/(norm(vRyz) * norm(Fyz)) ); +qualityIndex(4) = 100 - abs( (90 - acosd(dot(Myz, vRyz)/(norm(Myz) * norm(vRyz))) )/90 ); +qualityIndex(5) = norm(Fpar); +end \ No newline at end of file