SITL: add Simulink heli example

libraries/SITL/examples/JSON/MATLAB/Heli/heli_init.m

@@ -0,0 +1,128 @@
+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')

libraries/SITL/examples/JSON/MATLAB/Heli/readme.md

@@ -0,0 +1,3 @@
+This is a example Helicopter Simulink model. This has accurate physics and can be run using the EKF. Blocks from this example could be used to make other vehicles without having to work out the conversions to the ArduPilot reference frames. Note that ground interaction is neglected, once you take off you can't land! heli_init.m must be run before the module will run.
+
+Many thanks to [Bill Geyer](https://github.com/bnsgeyer) for providing this model.