From dcae9dc4851c6b88c087caef8b590760b1e4083a Mon Sep 17 00:00:00 2001
From: Mauco03 <marco.gaibotti@skywarder.eu>
Date: Thu, 30 May 2024 12:15:12 +0200
Subject: [PATCH] [refactoring-ode][functions] Added getXCP to common

---
 functions/miscellaneous/getXCP.m | 117 +++++++++++++++++++++++++++++++
 1 file changed, 117 insertions(+)
 create mode 100644 functions/miscellaneous/getXCP.m

diff --git a/functions/miscellaneous/getXCP.m b/functions/miscellaneous/getXCP.m
new file mode 100644
index 0000000..6498711
--- /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
-- 
GitLab