% Quadrotor State Space Model with Non-linearities
close all

%%%%%%%%%%%%%%%%%%
% Propeller motors
%%%%%%%%%%%%%%%%%%
Kt = 10^-2;   % motor torque constant [Nm/A]
Kb = 10^-2;   % motor back emf constant [Vs]
R  = 1;       % motor internal resistance [V/A]

%%%%%%%%%%%
% Propeller
%%%%%%%%%%%
p  = 1.225;   % density of air [kg/m^3]
Dp = 0.1;     % prop diameter [m]
Ct = 0.1;     % prop thrust coeficient [N s^2/(kg m)]
Cp = 0.7;     % prop drag coefficient  [N s^2/(kg m)]

b = Ct*p*Dp^4/(4*pi^2); % prop thrust factor

d = Cp*p*Dp^5/(8*pi^3); % prop drag factor

% gearbox = none so...
r = 1;   % reduction ratio
n = 1;   % gearbox efficiency

%%%%%%%%%%%%%%%%%%%%%%%%%%
% Moments of inertia
%%%%%%%%%%%%%%%%%%%%%%%%%%

m = 0.1;  % motor-propeller and mount mass [kg]
L = 0.1;  % body center to motor shaft massless arm length [m]

% Body moment of inertial about X and Y axes
Ix = 2*m*L^2;
Iy = 2*Ix;

% Z-axis is 4 motor assemblies on massless rods 
Iz = 4*m*L^2;  % [kg m^2]

% Motor and propeller referred to motor shaft
mp = 0.002; % propeller mass [kg]

Jp = 1/12 * mp * Dp^2;  % prop moment of inertia [kg m^2]

mm = 0.03;   % motor mass  [kg]
mr = mm/2;   % rotor mass  [kg]

rr = 0.01;   % rotor radius [m]

Jm = 1/2 * mr * rr^2;  % motor moment of inertia [kg m^2]

Jt = Jm + Jp;  % total rotational inertia of motor-prop assembly [kg m^2]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% State equations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% skip the body inertial effects for now...

% Maximum rate of body rotation about any axis for controllable flight
rmax = pi;  % [1/s]

% initialize rate vectors for which to determine gains
ratesteps = 10;  
%dps = [0:rmax/ratesteps:rmax];  % roll rate
dps = [1];  % yaw rate

%dth = [0:rmax/ratesteps:rmax];  % pitch rates
%dph = [0:rmax/ratesteps:rmax];  % yaw rates
dth = [1];  % pitch rates
dph = [1];  % roll rates

N = length(dps);

izxoy = (Iz - Ix)/(2*Iy);
iyzox = (Iy - Iz)/(2*Ix);
ixyoz = (Ix - Iy)/(2*Iz);

C = [1 0 1 0 1 0];

q=1;
Q = [q 0 0 0 0 0;
     0 0 0 0 0 0;
     0 0 q 0 0 0;
     0 0 0 0 0 0;
     0 0 0 0 q/10 0;
     0 0 0 0 0 0];

 R = [1 0 0 0;
      0 1 0 0;
      0 0 1 0;
      0 0 0 1];
  
N = 0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize the sim
tout=0;  
iph = pi/4;  % roll
ith = pi/4;     % pitch
ips = pi/4;     % yaw
initcons = [iph 0 ith 0 ips 0];
ic = initcons;    % initial conditions: phi, theta, psi (roll, pitch, yaw)
tend = 1.5;       % simulation end time

xout = ic;
xfb = ic;

tstep = .02;

tout  = 0;
refin = [0 0 0 0 0 0];
refout= refin;

yout  = C.*ic;

eout  = refout-yout;          % error output init


tm = 0;

vidfile = VideoWriter('quad_regulator_sim.mp4','MPEG-4');
open(vidfile);
fig = figure('Name','Quadrotor Attitude Regulator');
[plt, rplt, tplt, yplt] = paintbody([0 0 0], yout(1), yout(3), yout(5),tout,xout,tend);
MM = getframe(fig);
writeVideo(vidfile,MM);   

pause(4);

while tm < tend, 

    % linearized plant has angular rates in the state transition matrix
    % the plant model thus changes over the flight envelope
    % update the model and recompute the gain.

    % an implementation would most likely involve a gain lookup table.
    % for an estimated phidot, thetadot, psidot as an index into a 3-dim
    % lookup table. However, for this simulation we just update the
    % model and recompute the gain directly.

    A  = [0 1 0 0 0 0;
         0 0 0 xfb(6)*iyzox 0 xfb(4)*iyzox;
         0 0 0 1 0 0;
         0 xfb(6)*izxoy 0 0 0 xfb(2)*izxoy;
         0 0 0 0 0 1;
         0 xfb(4)*ixyoz 0 xfb(2)*ixyoz 0 0];

    B  = [0 0 0 0;
          L/Ix 0 0 Jt/Ix*xfb(4);
          0 0 0 0;
          0 L/Iy 0 Jt/Iy*xfb(2);
          0 0 0 0;
          0 0 1/Iz 0];

    % recompute gain K  
    [K, S, CLP] = lqr(A,B,Q,R, N);

    % full state feedback
    % u is cross-body torque
    u = -K*(xfb)';
    
    % u is the cross-body and yaw torque command input.
    % To simulate bias and disturbance in this command torque
    % being applied to the platform run it into a motor speed model
    % where desired speeds can be estimated, perturbed, and supplied
    % out as torque inputs to the plant: the copter body.

    % Remove comments on next two lines to map control output u
    % through to a prop speed model and back out as cross-body torque
    
    %    Tout = BodyTorqueToPropSpeed(u);
    %    u=Tout;
    
    % Initial conditions for next iteration are the states of the last
    % output of the solver for this iteration
    icon = xfb;

    % invoke the ODE solver to compute state variables for each interval of
    % tspan. "rpcont" if where the state-space model lives so examine that.
    tspan = 0:tstep/10:tstep;    
    [t,x] = ode45(@(t,x) ssodefunc(t,x,A,B,C,u), tspan, icon);

    % post-solver iteration data collection

    % the solver output time vector is zero-based. Offset by iteration start
    % time
    tt = tm+t;  
    tout = [tout; tt]; % append this iteration time vector to total sim time
    xout = [xout; x];  % append iteration states to sim state data

    sz = size(x);      % size of the state vector

    xfb=x(sz(1),:);

    % Apply the state-space 'C' matrix to the state matrix to obtain angular
    % output for this iteration
    yo = C.*x;

    % the "sampled" output for the next iteration is that last position
    % solved in this iteration.
    yout = yo(sz(1),:);

    % set the start time tm for the next iteraton.
    tm = tm + tstep;   

    paintbody_update(plt, rplt, tplt, yplt,[0 0 0], yout(1), yout(3), yout(5),tout,xout);
   MM = getframe(fig);
   writeVideo(vidfile,MM);   
    
end

close(vidfile);


