%  ---------------------------------------------------------------------  %
%   V   V  EEEEE  H   H  III   CCCC  L      EEEEE
%   V   V  E      H   H   I   C      L      E
%   V   V  EEE    HHHHH   I   C      L      EEE
%    V V   E      H   H   I   C      L      E
%     V    EEEEE  H   H  III   CCCC  LLLLL  EEEEE
%
%   PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
%   P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
%   PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
%   P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
%   P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
%  ---------------------------------------------------------------------  %
%% PARAMETERS OF THE N-ROTOR VEHICLE

%% STUDENTS TO SPECIFY LAYOUT AND PARAMETERS OF THEIR N-ROTOR DESIGN HERE:

% Gravitational constant:
g = 9.81;
g_vec = [0; 0; -g];





% The TRUE mass of the N-rotor vehicle, used for simulating the equations
% of motion [kg]
nrotor_vehicle_mass_true = 0.200;

% The "measured" mass of the N-rotor vehicle, USED FOR computing the
% FEED-FORWARD THRUST and CONTROLLER GAINS [kg]
nrotor_vehicle_mass_for_controller = 0.200 * 1.00;





% The TRUE mass moment of inertia of the N-rotor vehicle, used for
% simulating the equations of motion [kg m^2]
nrotor_vehicle_inertia_true = [...
        11.0 ,   0.0 ,   0.0 ;...
         0.0 ,  11.0 ,   0.0 ;...
         0.0 ,   0.0 ,  22.0  ...
    ] * 1e-4;

% Inverse of the moment of inertia for convenience
nrotor_vehicle_inertia_true_inverse = inv(nrotor_vehicle_inertia_true);

% NOTE: that an "nrotor_vehicle_inertia_for_controller" is not specified
% because it is not needed for the controller design used by the template.
% However, if the controller design is changed to use the mass moment of
% inertia, then a "for controller" variable should be added to allow for
% the possibility of model mis-match
%nrotor_vehicle_inertia_for_controller = ...





% The the layout of the N-rotor vehicle:
% NOTE: the layout is specified as a matrix with the following format:
% nrotor_vehicle_layout = [...
%       x_1  ,  x_2  ,  ...  ,  x_{N-1}  ,  x_{N}  ;...
%       y_1  ,  y_2  ,  ...  ,  y_{N-1}  ,  y_{N}  ;...
%       c_1  ,  c_2  ,  ...  ,  c_{N-1}  ,  c_{N}   ...
%   ];
% where:
%   > x_i   is the x coordinate of propeller "i" relative to the CoG
%   > y_i   is the y coordinate of propeller "i" relative to the CoG
%   > c_i   is the thurst to torque coefficient of the propeller, with the
%           sign used to indicate the propeller's direction of rotation,
%           +ve indicates clock-wise propeller rotation.


% Specify a "baseline" value for the x and y coordinates of the propellers
% in the layout [meters]
x_baseline = 0.300;
y_baseline = 0.300;

% Specify a "baseline" value for the "torque-to-thrust" ratio to make
% constructing the "nrotor_vehicle_layout" matrix cleaner
c_baseline = 0.1;


% The true N-rotor vehicle layout, used for simulating the equations of
% motion:

% Symmetric 4-rotor Example:
nrotor_vehicle_layout_true = [...
        [ -1 , -1 ,  1 ,  1 ] * x_baseline  ;...
        [ -1 ,  1 , -1 ,  1 ] * y_baseline  ;...
        [ -1 ,  1 ,  1 , -1 ] * c_baseline   ...
    ];

% Symmetric 8-rotor Example:
% nrotor_vehicle_layout_true = [...
%         [ -1 , -1 ,  1 ,  1 , -1.414 ,  0      ,  1.414 ,  0.0   ] * x_baseline  ;...
%         [ -1 ,  1 , -1 ,  1 ,  0     ,  1.414  ,  0     , -1.414 ] * y_baseline  ;...
%         [  1 ,  1 ,  1 ,  1 , -1     , -1      , -1     , -1     ] * c_baseline   ...
%     ];


% The "measured" symmetric Quad-rotor layout in "X" formation, USED FOR
% computing the FEED-FORWARD THRUST and CONTROLLER GAINS

% Specify an offset in the X and Y body frame directions, in [meters]
x_offset = x_baseline * 0.00;
y_offset = y_baseline * 0.00;

% Make the layout used for controller design the true layout shifted by the
% offsets specified
nrotor_vehicle_layout_for_controller = nrotor_vehicle_layout_true + repmat( [x_offset;y_offset;0.000] , 1 , size(nrotor_vehicle_layout_true,2) );





% SPECIFY THE MAXIMUM AND MINIMUM THRUST THAT CAN BE PRODUCED BY EACH
% PROPELLER:
% > This should be in unit of [Newtons]
% > For the minimum thrust: (this should always be zero)
nrotor_vehicle_thrust_min =   zeros( size(nrotor_vehicle_layout_true,2) , 1 );
% > For the maximum thrust:
nrotor_vehicle_thrust_max = 10*ones( size(nrotor_vehicle_layout_true,2) , 1 );





% Clear the variable that are not needed
clear c_baseline;
clear x_offset;
clear y_offset;





%  ---------------------------------------------------------------------  %
%   III  N   N  III  TTTTT  III    A    L
%    I   NN  N   I     T     I    A A   L
%    I   N N N   I     T     I   A   A  L
%    I   N  NN   I     T     I   AAAAA  L
%   III  N   N  III    T    III  A   A  LLLL
%
%    CCCC   OOO   N   N  DDDD   III  TTTTT  III   OOO   N   N   SSSS
%   C      O   O  NN  N  D   D   I     T     I   O   O  NN  N  S
%   C      O   O  N N N  D   D   I     T     I   O   O  N N N   SSS
%   C      O   O  N  NN  D   D   I     T     I   O   O  N  NN      S
%    CCCC   OOO   N   N  DDDD   III    T    III   OOO   N   N  SSSS
%  ---------------------------------------------------------------------  %
%% INITIAL CONDITIONS FOR THE INTEGRATORS IN THE SIMULINK MODEL

% NOTE:
% It is important to set the initial condition of any integrator block in
% the Simulnk mode because Simulink use the size of the initial conidition
% to determine the size the vector signals.

% Initial conditions for the full state
nrotor_initial_condition_p       =    [0; 0; 0];
nrotor_initial_condition_p_dot   =    [0; 0; 0];
nrotor_initial_condition_psi     =    [0; 0; 0];
nrotor_initial_condition_psi_dot =    [0; 0; 0];
nrotor_initial_condition = [...
        nrotor_initial_condition_p          ;...
        nrotor_initial_condition_p_dot      ;...
        nrotor_initial_condition_psi        ;...
        nrotor_initial_condition_psi_dot    ;...
    ];

% Initial conditions for the body rates
nrotor_initial_condition_body_rates = [0; 0; 0];


