You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
128 lines
3.2 KiB
128 lines
3.2 KiB
addpath(genpath('../../MATLAB')) |
|
|
|
% init the globalr varables required by the heli SITL example |
|
|
|
global A |
|
global B |
|
|
|
A=zeros(6,6); %#ok<*PREALL> |
|
B=zeros(6,7); |
|
|
|
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
|
% initialize higher order model variables |
|
DCM_0 = zeros(3,3); |
|
ic_vel = zeros(3,1); |
|
ic_angrate = zeros(3,1); |
|
ic_controls = zeros(4,1); |
|
ic_euler_ang = zeros(3,1); |
|
|
|
% direction IC's |
|
h_0=0; h=0; |
|
North_0=0;% |
|
East_0=0; |
|
Psi = 353*pi/180.0; |
|
|
|
%Derivatives accountfor mass |
|
M=1; |
|
|
|
Xu=-0.02; % barn door |
|
Zw=-2.0; |
|
Mq=-2.0; |
|
Mu=0.001; |
|
Yv=-0.4; |
|
Lp=-6.0; |
|
Nr=-8.0; |
|
Lv=-0.02; |
|
|
|
|
|
|
|
% [u v w p q r]' |
|
|
|
A = [ Xu 0 0 0 0 0;... |
|
0 Yv 0 0 0 0;... |
|
0 0 Zw 0 0 0;... |
|
0 Lv 0 Lp 0 0;... |
|
Mu 0 0 0 Mq 0;... |
|
0 0 0 0 0 Nr]; |
|
|
|
|
|
% lat long coll ped ' |
|
|
|
Zcoll=-60; |
|
Mlong = 1; |
|
Llat=1; |
|
Nped=2; |
|
B = [ 0 0 0 0;... |
|
0 0 0 0;... |
|
0 0 Zcoll 0 ;... |
|
Llat 0 0 0 ;... |
|
0 Mlong 0 0 ;... |
|
0 0 0 Nped ]; |
|
|
|
max_saved=300000; % limits no. of data points to the last max_saved |
|
|
|
%% Re-order rows of A for aerospace blk based sim - columns to be ordered in u. |
|
% Pare A down for Forces only |
|
A_Fxyz_uvw_pqr=A([1 2 3],[1 2 3 4 5 6]); |
|
% Pare A down for Moments only |
|
A_Mxyz_uvw_pqr=A([4 5 6],[1 2 3 4 5 6]); |
|
|
|
%% B rows = X,Y,Z,L,M,N, B cols = u=[lat long coll ped] |
|
% Pare B down for Forces only |
|
B_Fxyz_u_cont=B([1 2 3],[1 2 3 4]); |
|
% Pare B down for Moments only |
|
B_Mxyz_u_cont=B([4 5 6],[1 2 3 4]); |
|
|
|
ic_u=0.0000; ic_v=0.0000; ic_w=0.0000; zPsi=306*pi/180; % Points down RWY32 |
|
|
|
%ic_u=0.00001; ic_v=0.00001; ic_w=0.00001; zPsi=306*pi/180; % Points down RWY32 |
|
ic_U=sqrt(ic_u^2+ic_v^2+ic_w^2); |
|
|
|
|
|
%% WIND MODEL - NOTE ; SET Vw = 0 @ h_ic, Vw units in braces are KTS |
|
h_deck=21; h_island=h_deck+130; |
|
h_contact=h_deck; % h_contact is used in landing gear model (same for fixed and rotary model) |
|
|
|
%% TURBULENCE |
|
W_GUST=.8; % BW in rad/sec |
|
K_gust=0 ; % 80; % Gust Gain Factor |
|
Th_wind_limit=40; % Limits on wind swing in deg |
|
hw_gust=[0 1 h_island h_island+1 5000]; % 5 break points |
|
K_gust_factor=[1.1 1 .5 0.25 0]; |
|
%% WIND |
|
% hw=[0, h_deck+1, h_deck+1, 150 151 20000]; % FOR SHIP.. 6 BREAK POINTS |
|
hw=[0 200 5000 10000 15000 20000]; % 6 BREAK POINTS, MONO INCREASING |
|
Vw=15*[1 1 1 1 1 1]; % wind magnitude in fps |
|
Dir_w=0*[1 1 1 1 1 1]; % wind direction in deg |
|
|
|
Ftrim_grav=-32.0757; |
|
Roll_ic=0; Pit_ic=0; |
|
|
|
%Make the trim direction cosine matrix |
|
phi_0 = 3; |
|
tht_0 = Pit_ic; |
|
psi_0 = Psi*180/pi; |
|
|
|
cphi = cosd(phi_0); |
|
sphi = sind(phi_0); |
|
ctht = cosd(tht_0); |
|
stht = sind(tht_0); |
|
cpsi = cosd(psi_0); |
|
spsi = sind(psi_0); |
|
|
|
DCM_0 = [1 0 0; |
|
0 cphi sphi; |
|
0 -sphi cphi] * ... |
|
[ctht 0 -stht; |
|
0 1 0; |
|
stht 0 ctht]*... |
|
[cpsi spsi 0; |
|
-spsi cpsi 0 |
|
0 0 1]; |
|
|
|
%% A short run of the sim to sample cockpit the current control positions |
|
U_CONT_O=[0 0 0 0]; |
|
|
|
|
|
Ts=2000; % set Ts for real time sim run |
|
disp('Init Loaded') |