LZY\79495
119 lines
3.2 KiB
Matlab
119 lines
3.2 KiB
Matlab
function trim_and_lin
|
|
|
|
proj = slproject.getCurrentProject;
|
|
wd = cd;
|
|
cd([proj.RootFolder '/work']);
|
|
mdl = evalin('base', 'mdl_name');
|
|
fcn = [mdl, '_an'];
|
|
h0 = 50;
|
|
v_trim = 25;
|
|
mass = 39;
|
|
[success,opts1, op_point] =get_trim_Lon(h0, v_trim, mass);
|
|
if success
|
|
opts = opts1;
|
|
x0 = [0 0 opts.h];
|
|
alpha = opts.alpha;
|
|
v = opts.v;
|
|
de0 = opts.de;
|
|
tht0 = opts.tht;
|
|
v0 = [v*cos(alpha) 0 v*sin(alpha)];
|
|
att0 = [0 tht0 0];
|
|
thr0 = opts.throttle;
|
|
[Glon,Glat] = linfdm(opts.fcn,op_point);
|
|
ss_wq = get_ss_wq(Glon);
|
|
q_de = tf(ss_wq('q','de'));
|
|
p_da = tf(Glat('p','da'));
|
|
r_dr = tf(Glat('r','dr'));
|
|
ss_br = get_ss_br(Glat,v);
|
|
|
|
save(['trim_lin_v',num2str(v_trim),'.mat'],'x0','v0','att0','thr0','de0','alpha','Glon','Glat','opts','q_de','p_da','r_dr','ss_wq','ss_br');
|
|
else
|
|
return;
|
|
end
|
|
|
|
|
|
|
|
cd(wd);
|
|
end
|
|
|
|
|
|
function ss_wq = get_ss_wq(Glon)
|
|
|
|
A1 = Glon.A([2 3],[2 3]);
|
|
B1 = Glon.B([2 3],1);
|
|
C1 = Glon.C([2 3],[2 3]);
|
|
D1 = Glon.D([2 3],1);
|
|
ss_wq = ss(A1,B1,C1,D1);
|
|
ss_wq.statename = {'w', 'q'};
|
|
ss_wq.inputname = {'de'};
|
|
ss_wq.outputname = {'alpha', 'q'};
|
|
end
|
|
|
|
function ss_br = get_ss_br(Glat,v)
|
|
|
|
[A,B] = calc_AB(Glat,v,0);
|
|
A1 = A;
|
|
B1 = B(:,2);
|
|
C1 = eye(4);
|
|
D1 = zeros(4,1);
|
|
ss_br = ss(A1,B1,C1,D1);
|
|
ss_br.statename = {'beta','p', 'r','phi'};
|
|
ss_br.inputname = {'dr'};
|
|
ss_br.outputname = {'beta','p', 'r','phi'};
|
|
|
|
end
|
|
|
|
|
|
% function Gsp = getGsp(Glon, v)
|
|
%
|
|
% A = Glon.A([2 3],[2 3]);
|
|
% B = Glon.B([2 3],1);
|
|
% C = Glon.C([2 3],[2 3]);
|
|
% D = Glon.D([2 3],1);
|
|
% V0 = sqrt(v(1).^2+v(2).^2+v(3).^2);
|
|
% Asp = [A(1,1) A(1,2)./V0;A(2,1).*V0 A(2,2)];
|
|
% Bsp = [B(1,1)./V0;B(2,1)];
|
|
% Csp = eye(2);
|
|
% Dsp = zeros(2,1);
|
|
% Gsp = ss(Asp,Bsp,C1,D1);
|
|
% Gsp.statename = {'w', 'q'};
|
|
% Gsp.inputname = {'de'};
|
|
% Gsp.outputname = {'alpha', 'q'};
|
|
% q_de = tf(Gsp('q','de'));
|
|
%
|
|
%
|
|
% end
|
|
|
|
|
|
function [A,B] = calc_AB(mode,V,index)
|
|
|
|
A = mode.A;
|
|
B = mode.B;
|
|
if index==1
|
|
A = [ A(1,1), A(1,2).*V, A(1,3), A(1,4)
|
|
A(2,1)./V, A(2,2), A(2,3)./V, A(2,4)./V
|
|
A(3,1), A(3,2).*V, A(3,3), A(3,4)
|
|
A(4,1), A(4,2).*V, A(4,3), A(4,4)];
|
|
|
|
B = [ B(1,1), B(1,2)
|
|
B(2,1)./V, B(2,2)./V
|
|
B(3,1), B(3,2)
|
|
B(4,1), B(4,2) ];
|
|
|
|
else
|
|
A = [ A(1,1), A(1,2)./V, A(1,3)./V, A(1,4)./V
|
|
A(2,1).*V, A(2,2), A(2,3), A(2,4)
|
|
A(3,1).*V, A(3,2), A(3,3), A(3,4)
|
|
A(4,1).*V, A(4,2), A(4,3), A(4,4)];
|
|
|
|
B = [ B(1,1)./V, B(1,2)./V
|
|
B(2,1), B(2,2)
|
|
B(3,1), B(3,2)
|
|
B(4,1), B(4,2) ];
|
|
|
|
|
|
end
|
|
|
|
end
|
|
|