%% --------------------------------------------------------------------- %%
%  Modelling, Control, and Estimation of N-rotor vehicles
%
%% CONTROL ARCHITECTURE AND TUNING OF PID AND LQR CONTROLLERS
%  
%  Note:
%  PID stands for Proportional Integral Derivative
%  LQR stands for Linear Quadratic Regulator
%
%% --------------------------------------------------------------------- %%



%% ASSUMPTIONS:
%  1) The origin of the body frame is at the center of mass of the N-rotor
%     vehicle.
%  2) The thrusts from the propellers are all aligned with the positive
%     z-axis of the body frame.
%  3) Gravity is aligned with the negative z-axis of the inertial frame



%  ---------------------------------------------------------------------  %
%    CCCC  L      EEEEE    A    RRRR
%   C      L      E       A A   R   R
%   C      L      EEE    A   A  RRRR
%   C      L      E      AAAAA  R  R
%    CCCC  LLLLL  EEEEE  A   A  R   R
%  ---------------------------------------------------------------------  %
%% CLEAR AND CLOSE EVERYTHING
% > Clear all variables:
clear;
% > Close all open figures
close all;
% > Clear any text from the Command Window
clc;





%  ---------------------------------------------------------------------  %
%   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

% INFO:
% The student should open the file "get_vehicle_paramters.m" and specify
% the N-rotor vehicle design as instructed in that file

exercise02_get_crazyflie_vehicle_paramters;

% The "get_vehicle_paramters_for_exercise02" script puts the following
% varibles into the workspace so that they can be used in the Simulink
% model:
% >> g
% >> g_vec
% >> nrotor_vehicle_mass_true
% >> nrotor_vehicle_mass_for_controller
% >> nrotor_vehicle_inertia_true
% >> nrotor_vehicle_inertia_true_inverse
% >> nrotor_vehicle_layout_true
% >> nrotor_vehicle_layout_for_controller
% >> nrotor_vehicle_thrust_min
% >> nrotor_vehicle_thrust_max
% >> nrotor_initial_condition



% INFO:
% The following function "visualise_nrotor_vehicle" makes a nice plot
% showing the top view layout of the N-rotor vehicle. Note that the rotor
% size is scaled automatically so that the circles representing each rotor
% "just touch", but this may sometimes lead to a strange appearance.
%if exist( 'visualise_nrotor_vehicle' , 'file' )
%    visualise_nrotor_vehicle( nrotor_vehicle_layout_true );
%end





%% --------------------------------------------------------------------- %%
%   FFFFF  RRRR   EEEEE   QQQ   U   U  EEEEE  N   N   CCCC  Y   Y      /
%   F      R   R  E      Q   Q  U   U  E      NN  N  C       Y Y      /
%   FFF    RRRR   EEE    Q   Q  U   U  EEE    N N N  C        Y      /
%   F      R  R   E      Q  Q   U   U  E      N  NN  C        Y     /
%   F      R   R  EEEEE   QQ Q   UUU   EEEEE  N   N   CCCC    Y    /
%
%   SSSS    A    M   M  PPPP   L      EEEEE       TTTTT  III  M   M  EEEEE
%  S       A A   MM MM  P   P  L      E             T     I   MM MM  E
%   SSS   A   A  M M M  PPPP   L      EEE           T     I   M M M  EEE
%      S  AAAAA  M   M  P      L      E             T     I   M   M  E
%  SSSS   A   A  M   M  P      LLLLL  EEEEE         T    III  M   M  EEEEE
%  ---------------------------------------------------------------------  %
%% SPECIFY THE FREQUENCY OF THE MEASUREMENTS AND CONTROLLER

% For convenience of Simulink, the specifications here are in terms of the
% sample time, i.e., the recipricol of the frequency

% Sample time for the different types of measurements:
% > for the full state measurement
sample_time_measurements_full_state = 1/200;
% > for the body rates measurement taken by the on-board gyroscope
sample_time_measurements_body_rates = 1/500;
% > for the body accelerations measurement taken by the on-board
%   accelerometer
sample_time_measurements_body_accelerations = 1/500;

% Sample time for the OUTER loop controller
sample_time_controller_outer = 1/200;
% Sample time for the INNER loop controller
sample_time_controller_inner = 1/500;





%  ---------------------------------------------------------------------  %
%   DDDD   EEEEE  FFFFF    A    U   U  L      TTTTT
%   D   D  E      F       A A   U   U  L        T
%   D   D  EEE    FFF    A   A  U   U  L        T
%   D   D  E      F      AAAAA  U   U  L        T
%   DDDD   EEEEE  F      A   A   UUU   LLLLL    T
%
%    OOO   U   U  TTTTT  EEEEE  RRRR
%   O   O  U   U    T    E      R   R
%   O   O  U   U    T    EEE    RRRR
%   O   O  U   U    T    E      R   R
%    OOO    UUU     T    EEEEE  R   R
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% LOAD THE HARD-CODED "DEFAULT" LINEAR QUADRATIC REGULATOR (LQR) CONTROLLER

% INFO:
% > For the purposes of the template, an LQR controller feedback matrix is
%   hard-coded and should stabilise most N-rotor vehicle layouts.

exercise02_get_default_outer_controller;

% The "get_vehicle_paramters_for_exercise02" script puts the following
% varibles into the workspace so that they can be used here:
% >> K_lqr_outer_loop_continous_time
% >> K_lqr_outer_loop_discrete_time_1Hz
% >> K_lqr_outer_loop_discrete_time_100Hz
% >> K_lqr_outer_loop_discrete_time_200Hz


% SELECT HERE THE CONTROLLER TO USE BY UN-COMMENTING THE DESIRED LINE
%K_lqr_outer_loop = K_lqr_outer_loop_continous_time;
%K_lqr_outer_loop = K_lqr_outer_loop_discrete_time_1Hz;
%K_lqr_outer_loop = K_lqr_outer_loop_discrete_time_100Hz;
K_lqr_outer_loop = K_lqr_outer_loop_discrete_time_200Hz;





%  ---------------------------------------------------------------------  %
%    SSSS  TTTTT  U   U  DDDD   EEEEE  N   N  TTTTT
%   S        T    U   U  D   D  E      NN  N    T
%    SSS     T    U   U  D   D  EEE    N N N    T
%       S    T    U   U  D   D  E      N  NN    T
%   SSSS     T     UUU   DDDD   EEEEE  N   N    T
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% THIS EXERCISE DOES NOT REQUIRE THE STUDENT TO DESIGN A CONTROLLER

% > For students implementing their own LQR controller, they should add
%   the necessary code to define the linearisation of the N-rotor vehicle
%   and solve the Riccati equation to get their own LQR controller feedback
%   matrix.









%  ---------------------------------------------------------------------  %
%   EEEEE   QQQ   U   U  III  L      III  BBBB   RRRRI  III  U   U  M   M
%   E      Q   Q  U   U   I   L       I   B   B  R   R   I   U   U  MM MM
%   EEE    Q   Q  U   U   I   L       I   BBBB   RRRR    I   U   U  M M M
%   E      Q  Q   U   U   I   L       I   B   B  R  R    I   U   U  M   M
%   EEEEE   QQ Q   UUU   III  LLLLL  III  BBBB   R   R  III   UUU   M   M
%
%   TTTTT  H   H  RRRR   U   U   SSSS  TTTTT   SSSS
%     T    H   H  R   R  U   U  S        T    S
%     T    HHHHH  RRRR   U   U   SSS     T     SSS
%     T    H   H  R  R   U   U      S    T        S
%     T    H   H  R   R   UUU   SSSS     T    SSSS
%  ---------------------------------------------------------------------  %
%% COMPUTE THE EQUILIBRIUM THRUSTS

% INFO:
% The "exercise02_compute_equilibrium_thrusts" function computes and
% returns the equilibrium thrusts as a column vector of length N. Thus the
% variable "nrotor_vehicle_equilibrium_thrust" is in the workspace and can
% be used in the Simulink model.

nrotor_vehicle_equilibrium_thrust = exercise02_compute_equilibrium_thrusts(nrotor_vehicle_layout_for_controller,nrotor_vehicle_mass_for_controller,g);






%  ---------------------------------------------------------------------  %
%    CCCC  RRRR     A    ZZZZZ  Y   Y  FFFFF  L      III  EEEEE
%   C      R   R   A A      Z    Y Y   F      L       I   E
%   C      RRRR   A   A    Z      Y    FFF    L       I   EEE
%   C      R   R  AAAAA   Z       Y    F      L       I   E
%    CCCC  R   R  A   A  ZZZZZ    Y    F      LLLLL  III  EEEEE
%
%   III  N   N  N   N  EEEEE  RRRR
%    I   NN  N  NN  N  E      R   R
%    I   N N N  N N N  EEE    RRRR
%    I   N  NN  N  NN  E      R   R
%   III  N   N  N   N  EEEEE  R   R
%
%    CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
%   C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
%   C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
%   C      O   O  N  NN    T    R  R   O   O  L      L      E      R  R
%    CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
%  ---------------------------------------------------------------------  %
%% LOAD THE PID PARAMTERS OF THE RATE CONTROLLER THAT RUN ON-BOARD THE CRAZYFLIE

% INFO:
%  The PID Parameters are taken directly from the crazyflie firmware,

exercise02_get_crazyflie_inner_controller;





%  ---------------------------------------------------------------------  %
%   M   M  EEEE    A     SSSS  U  U  RRRR   EEEE  M   M  EEEE  N   N  TTTTT
%   MM MM  E      A A   S      U  U  R   R  E     MM MM  E     NN  N    T
%   M M M  EEE   A   A   SSS   U  U  RRRR   EEE   M M M  EEE   N N N    T
%   M   M  E     AAAAA      S  U  U  R  R   E     M   M  E     N  NN    T
%   M   M  EEEE  A   A  SSSS    UU   R   R  EEEE  M   M  EEEE  N   N    T
%
%   N   N   OOO   III   SSSS  EEEEE
%   NN  N  O   O   I   S      E
%   N N N  O   O   I    SSS   EEE
%   N  NN  O   O   I       S  E
%   N   N   OOO   III  SSSS   EEEEE
%  ---------------------------------------------------------------------  %
%% SPECIFY THE MEASUREMENT NOISE PARAMETERS

exercise02_get_measurement_noise_details;





%  ---------------------------------------------------------------------  %
%   RRRR   EEEEE  FFFFF  EEEEE  RRRR   EEEEE  N   N   CCCC  EEEEE
%   R   R  E      F      E      R   R  E      NN  N  C      E
%   RRRR   EEE    FFFF   EEE    RRRR   EEE    N N N  C      EEE
%   R  R   E      F      E      R  R   E      N  NN  C      E
%   R   R  EEEEE  F      EEEEE  R   R  EEEEE  N   N   CCCC  EEEEE
%  ---------------------------------------------------------------------  %
%% SPECIFY THE REFERENCE SIGNAL FOR (x,y,z,yaw)

% INFO:
% The student can open the file
% "get_reference_specification_for_exercise02.m" and follow the comments
% there to adjust the reference signal
exercise02_get_reference_specification;


% The "get_reference_specification_for_exercise02" script puts the
% following varibles into the workspace so that they can be used in the
% Simulink model:
% >> reference_step_xyz_yaw
% >> reference_step_period
% >> reference_sine_amplitude_xyz_yaw
% >> reference_sine_bias_xyz_yaw
% >> reference_sine_period
% >> reference_saw_tooth_amplitude_xyz_yaw
% >> reference_saw_tooth_bias_xyz_yaw
% >> reference_step_inclusion_multiplier
% >> reference_sine_wave_inclusion_multiplier
% >> reference_saw_tooth_inclusion_multiplier

