function [e,Tl,Tr,Hl,Hr] = PvtolTrim(x,pvtol,Vw,Upsilon) %PVTOLTRIM % [e,T1,T2,H1,H2] = PvtolTrim(x,rotor,pvtol,Vw,Upsilon) % Calculate trim residual for PVTOL birotor % % Inputs % x: trim variables vector [theta, u(1), u(2)] % u(1): mean rotor speed (Omega_l + Omega_r)/2 % u(2): rotor speed difference (Omega_l - Omega_r)/2 % pvtol: vehicle airframe & rotorcoefficients structure (see Coefficients.m) % Vw: horizontal wind (m/s) % Upsilon: rotor tilt angle (rad) % % Outputs % e: residual % Tl: thust trim rotor left % Tr: thust trim rotor right % Hl: drag trim rotor left % Hr: drag trim rotor right % % Trim equilibrium equations - eqs(3.11) - (3.13) % W = (T_l+T_r) \cos\theta\cos\Upsilon - (T_l-T_r) \sin\theta\sin\Upsilon % + (H_l+H_r) \sin \theta\cos\Upsilon +(H_l-H_r) \cos\theta\sin\Upsilon % (T_l+T_r) \sin \theta\cos\Upsilon + (T_l-T_r) \cos\theta\sin\Upsilon = % + (H_l+H_r) \cos\theta\cos\Upsilon -(H_l-H_r) \sin\theta\sin\Upsilon % (T_l - T_r)\ell \cos\Upsilon = - (H_l + H_r)\ell \sin\Upsilon % (c) James Whidborne % Cranfield University, 16 August 2020 %% constants and coefficients g = 9.81; % gravitational constant rotor = pvtol.rotor; % rotor coefficients A = pi*rotor.R^2; % disc area W = pvtol.m*g; % weight rho = pvtol.rho; % air density (kg/m^3) theta=x(1); % vehicle pitch angle Omegal=(x(2)+x(3))/2; % rotor left speed Omegar=(x(2)-x(3))/2; % rotor right speed alphal = (theta+Upsilon); % rotor left angle of attack (steady state) alphar = (theta-Upsilon); % rotor right angle of attack (steady state) %% thrusts and drags - solve using fzero Tl = fzero(@(x)rho*A*(Omegal*rotor.R)^2*RotorCT(x,alphal,Vw,Omegal,rho,rotor)-x, W/2); Hl = rho*A*(Omegal*rotor.R)^2*RotorCH(Tl,alphal,Vw,Omegal,rho,rotor); Tr = fzero(@(x)rho*A*(Omegar*rotor.R)^2*RotorCT(x,alphar,Vw,Omegar,rho,rotor)-x, W/2); Hr = rho*A*(Omegar*rotor.R)^2*RotorCH(Tr,alphar,Vw,Omegar,rho,rotor); %% calculate residual vector from eqs(3.11) - (3.13) F= [W-(Tl+Tr)*cos(theta)*cos(Upsilon)+(Tl-Tr)*sin(theta)*sin(Upsilon)-(Hl+Hr)*sin(theta)*cos(Upsilon)-(Hl-Hr)*cos(theta)*sin(Upsilon); (Tl+Tr)*sin(theta)*cos(Upsilon)+(Tl-Tr)*cos(theta)*sin(Upsilon)-(Hl+Hr)*cos(theta)*cos(Upsilon)+(Hl-Hr)*sin(theta)*sin(Upsilon); (Tl-Tr)*pvtol.l*cos(Upsilon)+(Hl+Hr)*pvtol.l*sin(Upsilon)]; e = F'*F; % square and sum end