LZY\79495
97 lines
3.4 KiB
Matlab
97 lines
3.4 KiB
Matlab
function [success, opts, op_point,op_report,opsec] = trimLonAlphaDf( opts, h0, alp0, gamma0, v0)
|
|
% 无侧滑的定直飞行
|
|
opsec = update_opsec(opts.opsec, opts, h0, alp0, gamma0, v0);
|
|
%% Trim model
|
|
[op_point,op_report] = findop(opts.fcn,opsec,opts.TrimOptions);
|
|
if ~isempty(strfind(op_report.TerminationString,'successfully'))
|
|
success = true;
|
|
opts = update_opts(opts,op_report);
|
|
else
|
|
success = false;
|
|
end
|
|
end
|
|
|
|
function opsec = update_opsec(opsec, opts, h0, alp0, gamma0, v0)
|
|
n = length(opsec.Inputs);
|
|
for i=1:n
|
|
inputs = opsec.Inputs(i);
|
|
if strcmp(inputs.Block(end-1:end),'de')
|
|
inputs.u = opts.de;
|
|
inputs.Known = false; %unkown de
|
|
inputs.max = opts.max_de;
|
|
inputs.min = opts.min_de;
|
|
elseif strcmp(inputs.Block(end-7:end),'throttle')
|
|
inputs.u = opts.throttle;
|
|
inputs.Known = false; %unkown throttle
|
|
inputs.max = opts.max_throttle;
|
|
inputs.min = opts.min_throttle;
|
|
elseif strcmp(inputs.Block(end-1:end),'df')
|
|
inputs.u = 0;
|
|
inputs.Known = false; %unkown de
|
|
inputs.max = opts.max_df;
|
|
inputs.min = opts.min_df;
|
|
else
|
|
opsec.Inputs(i).Known = true;
|
|
end
|
|
end
|
|
n = length(opsec.States);
|
|
for i=1:n
|
|
states = opsec.States(i);
|
|
if strcmp(states.Block(end-2:end),'psi')
|
|
states.x = [0;opts.tht;opts.psi];
|
|
states.steadystate = [false;false;false];
|
|
states.Known = [true;false;true]; % set phi psi to zero, unknow tht
|
|
elseif strcmp(states.Block(end-1:end),'r ')
|
|
states.x = [0;0;0];
|
|
states.steadystate = [false;true;false]; % qdot = 0
|
|
states.Known = [true;true;true]; % set pqr to zero
|
|
elseif strcmp(states.Block(end-1:end),'wb')
|
|
if opts.v0_type == uint8(2)
|
|
v_t = 340*v0;
|
|
else
|
|
v_t = v0;
|
|
end
|
|
states.x = [v_t*cos(alp0);0;v_t*sin(alp0)];
|
|
states.steadystate = [true;false;true]; % wdot = 0, udot = 0
|
|
states.Known = [false;true;false]; % set vb to zero, unkown ub,wb
|
|
elseif strcmp(states.Block(end-1:end),'ze')
|
|
states.x = [0;0;-h0];
|
|
states.steadystate = [false;false;false];
|
|
states.Known = [true;true;true]; % set x,y to zero, z to h0
|
|
else
|
|
% nx = states.Nx;
|
|
% states.steadystate = zeros(nx,1,'logical'); % Nonequilibrium state for others
|
|
% states.Known = ones(nx,1,'logical'); % do not trim other states
|
|
end
|
|
end
|
|
n = length(opsec.Outputs);
|
|
for i=1:n
|
|
outputs = opsec.Outputs(i);
|
|
if opts.v0_type == uint8(0) && strcmp(outputs.Block(end-2:end),'TAS')
|
|
outputs.y = v0;
|
|
outputs.Known = true; % TAS = v0
|
|
elseif opts.v0_type == uint8(1) && strcmp(outputs.Block(end-2:end),'CAS')
|
|
outputs.y = v0;
|
|
outputs.Known = true; % CAS = v0
|
|
elseif opts.v0_type == uint8(2) && strcmp(outputs.Block(end-2:end),'ach')
|
|
outputs.y = v0;
|
|
outputs.Known = true; % Mach = v0
|
|
elseif strcmp(outputs.Block(end-4:end),'gamma')
|
|
outputs.y = gamma0;
|
|
outputs.Known = true;%true; % gamma = gamma0
|
|
elseif strcmp(outputs.Block(end-3:end),'lpha')
|
|
outputs.y = alp0;
|
|
outputs.Known = true;
|
|
outputs.max = opts.max_alpha;
|
|
outputs.min = opts.min_alpha;
|
|
elseif strcmp(outputs.Block(end-2:end),'eta')
|
|
outputs.y = 0;
|
|
outputs.Known = false;
|
|
outputs.max = opts.max_beta;
|
|
outputs.min = opts.min_beta;
|
|
else
|
|
outputs.Known = zeros(size(outputs.Known),'like',outputs.Known);
|
|
end
|
|
end
|
|
end
|