function y = autopilot(uu,C)
%
% autopilot for quadrotor
% 
% Modification History:
%   1/21/08 - RWB
%   1/31/08 - RWB
%   2/5/08 - RWB
%   2/6/08 - GTR

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% get states and commands
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  [t, xhat,commands] = get_states_commands(uu,C);
  
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% implement autopilot modes

  %---------------------------------------------------------------
% regulate altitude based on size of object in image
  u_z = sat(altitude_hold(xhat.pz-commands.pz, t, C),2,-2);
  F_thrust = C.m*(C.g - u_z)/cos(xhat.phi)/cos(xhat.theta);

% rotate positions and commanded positions into body frame:
R = [...
    cos(xhat.psi), sin(xhat.psi);...
    -sin(xhat.psi), cos(xhat.psi)];
tmp = R*[xhat.px; xhat.py];
xhat.px = tmp(1);
xhat.py = tmp(2);
tmp = R*[commands.px; commands.py];
commands.px = tmp(1);
commands.py = tmp(2);


%---------------------------------------------------------------
% regulate pitch angle to drive x-axis error to zero
  theta_max = 10*pi/180;  % maximum commanded pitch angle
  u_x  = longitudinal_position_hold(xhat.px-commands.px, xhat.pxdot, t, C);
  decouple_x = u_x/(C.g - u_z);
  theta_c  = sat(atan(decouple_x),theta_max,-theta_max);
  Tq = pitch_attitude_hold(xhat.theta, theta_c, xhat.q, t, C);

%---------------------------------------------------------------
% regulate roll angle to drive y-axis error to zero
  phi_max = 10*pi/180;  % maximum commanded roll angle
  u_y  = lateral_position_hold(xhat.py-commands.py, xhat.pydot, t, C);
  decouple_y = u_y*cos(xhat.theta)/(C.g - u_z);
  phi_c = sat(atan(decouple_y),phi_max,-phi_max);
  Tp = roll_attitude_hold(xhat.phi, phi_c, xhat.p, t, C);

%---------------------------------------------------------------
% regulate heading to align with orientation of object in image
% don't adjust heading if x or y error is too large
%   M = .05;
%   if (abs(xhat.px-commands.px)<=M) & (abs(xhat.py-commands.py)<=M)
      Tr = heading_hold(xhat.psi, commands.psi, xhat.r, t, C);
%   else
%       Tr = -C.yaw_kd*xhat.r;
%   end
  

% control output
pwm = [...
    1/4/C.kmotor_lift, 0, 1/2/C.l/C.kmotor_lift, 1/4/C.kmotor_drag;...
    1/4/C.kmotor_lift, -1/2/C.l/C.kmotor_lift, 0, -1/4/C.kmotor_drag;...
    1/4/C.kmotor_lift, 0, -1/2/C.l/C.kmotor_lift, 1/4/C.kmotor_drag;...
    1/4/C.kmotor_lift, 1/2/C.l/C.kmotor_lift, 0, -1/4/C.kmotor_drag;...
    ]*[F_thrust; Tp; Tq; Tr];

% desired states (for visulation and debugging)
xd = [...
        commands.px;...      % xerror: position error along body x-axis
        commands.py;...      % yerror: position error along body y-axis
        commands.pz;...      % zerror:  position error along body z-axis (in pixels)
        0;...                % u: desired velocity along body x-axis
        0;...                % v: desired velocity along body v-axis
        0;...                % w: desired velocity along body z-axis
        phi_c;...            % phi:   desired roll angle
        theta_c;...          % theta: desired pitch angle
        commands.psi;...     % psi:   desired heading
        0;...                % desired roll rate  
        0;...                % desired pitch rate
        0;...                % desired yaw rate
    ];

% output
xhat_ = [...
    xhat.px;...
    xhat.py;...
    xhat.pz;...
    xhat.pxdot;...
    xhat.pydot;...
    xhat.pzdot;...
    xhat.phi;... 
    xhat.theta;...
    xhat.psi;...
    xhat.p;...
    xhat.q;...
    xhat.r;...
    ];
y = [pwm; xd; xhat_];



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Autopilot functions
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% roll_attitude_hold
%  - regulate roll_attitude
%  - produces desired roll rate
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = roll_attitude_hold(phi, phi_c, p, t, C);
  persistent uik1;
  persistent udk1;
  persistent errork1;
  persistent phik1;
  % initialize persistent variables at beginning of simulation
  if t<C.Ts, 
      uik1    = 0; 
      errork1 = 0;
  end
 
  % error equation
  error = phi_c - phi;

  % proportional term
  up = C.roll_kp * error;
  
  % integral term
  ui = uik1 + C.roll_ki * C.Ts/2 * (error + errork1);
  
  % derivative term
  ud = -C.roll_kd*p;
  
  % implement PID control
  torque = up + ui + ud;

  % update stored variables
  uik1    = ui; 
  errork1 = error;
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% lateral_position_hold
%  - regulate lateral position
%  - produces desired roll command
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u_y  = lateral_position_hold(error, pydot, t, C)
  persistent uik1;
  persistent errork1;
  persistent vhatk1;
  % initialize persistent variables at beginning of simulation
  if t<C.Ts, 
      uik1    = 0; 
      errork1 = 0;
      vhatk1 = 0;
  end
 
  % proportional term
  up = -C.y_kp * error;
  
  % integral term
  ui = uik1 + C.y_ki * C.Ts/2 * (error + errork1);
  
  % dirty derivative of pe to get vhat
  vhat = (2*C.tau-C.Ts)/(2*C.tau+C.Ts)*vhatk1 + (2/(2*C.tau+C.Ts))*(error - errork1);
  ud = -C.y_kd*vhat;
%  ud = -C.y_kd*pydot;

  
  % implement PID control
  u_y = up + ui + ud;
  
  % saturate the roll command
  %u_y = sat(u_y, 20*pi/180, -20*pi/180);
  
  % update stored variables
  uik1    = ui; 
  errork1 = error;
  vhatk1  = vhat;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% pitch_attitude_hold
%  - regulate pitch attitude
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = pitch_attitude_hold(theta, theta_c, q, t, C)
  persistent uik1;
  persistent udk1;
  persistent errork1;
  persistent thetak1;
  % initialize persistent variables at beginning of simulation
  if t<C.Ts, 
      uik1    = 0; 
      errork1 = 0;
  end
 
  % error equation
  error = theta_c - theta;

  % proportional term
  up = C.pitch_kp * error;
  
  % integral term
  ui = uik1 + C.pitch_ki * C.Ts/2 * (error + errork1);
  
  % derivative term
  ud = -C.pitch_kd*q;
  
  
  % implement PID control
  torque = up + ui + ud;

  % update stored variables
  uik1    = ui; 
  errork1 = error;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% longitudinal_position_hold
%  - regulate longitudinal position
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function u_x  = longitudinal_position_hold(error, pxdot, t, C)
  persistent ui;
  persistent errork1;
  persistent vhatk1;
  % initialize persistent variables at beginning of simulation
  if t<C.Ts, 
      ui    = 0; 
      errork1 = 0;
      vhatk1 = 0;
  end
 
  % proportional term1
  up = C.x_kp * error;
  
  % integral term
  ui = ui + C.x_ki * C.Ts/2 * (error + errork1);
  
  % dirty derivative of pe to get vhat
  vhat = (2*C.tau-C.Ts)/(2*C.tau+C.Ts)*vhatk1 + (2/(2*C.tau+C.Ts))*(error - errork1);
  ud = C.x_kd*vhat;
%  ud = C.x_kd*pxdot;

  
  % implement PID control
  u_x = up + ui + ud;
  
  % saturate the roll command
  %u_x = sat(u_x, 20*pi/180, -20*pi/180);

  % update stored variables
  errork1 = error;
  vhatk1 = vhat;
  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% altitude hold
%  - regulate altitude
%  - produces force command
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F = altitude_hold(error, t, C)
  persistent uik1;
  persistent error_k1;
  persistent error_dot_k1;
  % initialize persistent variables at beginning of simulation
  if t<C.Ts, 
      uik1       = 0; 
      error_k1 = 0;
      error_dot_k1 = 0;
  end
  
  % proportional term
  up = C.h_kp * error;
  
  % integral term
  ui = uik1 + C.h_ki * C.Ts/2 * (error + error_k1);
  
  % dirty derivative of error
  error_dot = (2*C.tau-C.Ts)/(2*C.tau+C.Ts)*error_dot_k1 ...
     + (2/(2*C.tau+C.Ts))*(error - error_k1);
  ud = C.h_kd * error_dot;
  
  % implement PID control
  F = up + ui + ud;
  
  

  % saturate the force command
  %F = sat(F, 20*pi/180, -20*pi/180);

  % update stored variables
  uik1       = ui; 
  error_k1 = error;
  error_dot_k1 = error_dot;

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% heading_hold
%  - regulate heading
%  - produces torque about the yaw axis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = heading_hold(psi, psi_c, r, t, C)
  persistent uik1;
  persistent errork1;
  % initialize persistent variables at beginning of simulation
  if (t<=C.Ts) | isempty(uik1), 
      uik1    = 0; 
      errork1 = 0;
  end
 
  % error equation
  error = psi_c - psi;

  % proportional term
  up = C.yaw_kp * error;
  
  % integral term
  ui = uik1 + C.yaw_ki * C.Ts/2 * (error + errork1);
  
  % derivative term
  ud = -C.yaw_kd*r;
  
  
  % implement PID control
  torque = up + ui + ud;

  % update stored variables
  uik1    = ui; 
  errork1 = error;

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% sat
%   - saturation function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function out = sat(in, up_limit, low_limit)
  if in > up_limit,
      out = up_limit;
  elseif in < low_limit;
      out = low_limit;
  else
      out = in;
  end
  
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% use real states
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [t, x, commands] = get_states_commands(uu,C)
  NN = 0;
  t        = uu(1+NN);

  NN = NN + 1;
  commands.px  = uu(1+NN);
  commands.py  = uu(2+NN);
  commands.pz  = uu(3+NN);
  commands.psi = uu(4+NN);

  NN = NN + 4;
  x.px     = uu(1+NN);
  x.py     = uu(2+NN);
  x.pz     = uu(3+NN);
  x.pxdot  = uu(4+NN);
  x.pydot  = uu(5+NN);
  x.pzdot  = uu(6+NN);
  x.phi    = uu(7+NN);
  x.theta  = uu(8+NN);
  x.psi    = uu(9+NN);
  x.p      = uu(10+NN);
  x.q      = uu(11+NN);
  x.r      = uu(12+NN);