mirror of https://github.com/ArduPilot/ardupilot
128 lines
3.2 KiB
Mathematica
128 lines
3.2 KiB
Mathematica
|
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')
|