function y = autopilot(uu,Ts,C)
% autopilot
% 
% autopilot function for quadrotor restricted to cage
% 
% Modification History:
%   10/16/07 - RWB
%   11/30/07 - RWB
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% input commands
NN = 0;
eps_x = uu(1+NN);              % pixel location along camera x-axis
eps_y = uu(2+NN);              % pixel location along camera y-axis
eps_s = uu(3+NN);              % sizer of target in pixels
eps_psi = uu(4+NN);            % orientation of target in image

% estimated states
NN = NN+4;
phi   = uu(1+NN);   % roll angle
theta = uu(2+NN);   % pitch angle
p     = uu(3+NN);   % roll rate
q     = uu(4+NN);   % pitch rate
r     = uu(5+NN);   % yaw rate

% time
NN = NN+5;
t     = uu(1+NN);   % time

 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% implement autopilot modes

%---------------------------------------------------------------
% regulate pitch angle to drive x-axis error to zero
  error_x = -C.h*tan(theta - (eps_y/C.cam_pix*C.cam_fov));
  theta_c  = longitudinal_position_hold(eps_y, theta, C.h, Ts, t, C);
  Tq = pitch_attitude_hold(theta, theta_c, q, Ts, t, C);


%---------------------------------------------------------------
% regulate roll angle to drive y-axis error to zero
  error_y = C.h*tan(phi - (eps_x/C.cam_pix*C.cam_fov));
  phi_c  = lateral_position_hold(eps_x, phi, C.h, Ts, t, C);
  Tp = roll_attitude_hold(phi, phi_c, p, Ts, t, C);


%---------------------------------------------------------------
% regulate altitude based on size of object in image
  % desired pixel size of object in image
  eps_s_d = 36;

  F = C.m/cos(phi)/cos(theta)*(C.g + altitude_hold(eps_s, eps_s_d, Ts, t, C));


%---------------------------------------------------------------
% regulate heading to align with orientation of object in image
  Tr    = heading_hold(eps_psi, r, Ts, t, C);

% 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; Tp; Tq; Tr];

% desired states (for visulation and debugging)
xd = [...
        error_x;...          % xerror: position error along body x-axis
        error_y;...          % yerror: position error along body y-axis
        eps_s-eps_s_d;...    % zrror:  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
        -pi/2-eps_psi;...     % psi:   desired heading
        0;...                % desired roll rate  
        0;...                % desired pitch rate
        0;...                % desired yaw rate
    ];

% output
y = [pwm; xd];



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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% roll_attitude_hold
%  - regulate roll_attitude
%  - produces desired roll rate
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = roll_attitude_hold(phi, phi_c, p, Ts, t, C);
  persistent uik1;
  persistent udk1;
  persistent errork1;
  persistent phik1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      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 * Ts/2 * (error + errork1);
  
  % dirty 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 phi_c  = lateral_position_hold(eps_y, phi, h, Ts, t, C)
  persistent uik1;
  persistent errork1;
  persistent vhatk1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      uik1    = 0; 
      errork1 = 0;
      pek1    = 0;
      vhatk1 = 0;
  end
 
  % error equation
%  error = pe_c - pe;
  error = -h*tan(phi - (eps_y/C.cam_pix*C.cam_fov));

  % proportional term
  up = C.y_kp * error;
  
  % integral term
  ui = uik1 + C.y_ki * Ts/2 * (error + errork1);
  
  % dirty derivative of pe to get vhat
  vhat = (2*C.tau-Ts)/(2*C.tau+Ts)*vhatk1 + (2/(2*C.tau+Ts))*(error - errork1);
  ud = C.y_kd*vhat;

  
  % implement PID control
  phi_c = up + ui + ud;
  
  % saturate the roll command
  phi_c = sat(phi_c, 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, Ts, t, C);
  persistent uik1;
  persistent udk1;
  persistent errork1;
  persistent thetak1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      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 * Ts/2 * (error + errork1);
  
  % dirty 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 theta_c  = longitudinal_position_hold(eps_x, theta, h, Ts, t, C)
  persistent uik1;
  persistent errork1;
  persistent vhatk1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      uik1    = 0; 
      errork1 = 0;
      vhatk1 = 0;
  end
 
  % error equation
%  error = pe_c - pe;
  error = h*tan(theta - (eps_x/C.cam_pix*C.cam_fov));

  % proportional term1
  up = C.x_kp * error;
  
  % integral term
  ui = uik1 + C.x_ki * Ts/2 * (error + errork1);
  
  % dirty derivative of pe to get vhat
  vhat = (2*C.tau-Ts)/(2*C.tau+Ts)*vhatk1 + (2/(2*C.tau+Ts))*(error - errork1);
  ud = C.x_kd*vhat;

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

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

  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% altitude hold
%  - regulate altitude 
%  - produces force command
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F = altitude_hold(pixel_size, desired_pixel_size, Ts, t, C)
  persistent uik1;
  persistent pixel_size_k1;
  persistent pixel_size_dot_k1;
  persistent error_k1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      uik1       = 0; 
      pixel_size_k1    = pixel_size;
      pixel_size_dot_k1 = 0;
      error_k1 = 0;
  end
  
  % error equation
  error = pixel_size - desired_pixel_size;

  % proportional term
  up = C.h_kp * error;
  
  % integral term
  ui = uik1 + C.h_ki * Ts/2 * (error + error_k1);
  
  % dirty derivative of error
  pixel_size_dot = (2*C.tau-Ts)/(2*C.tau+Ts)*pixel_size_dot_k1...
      + (2/(2*C.tau+Ts))*(pixel_size - pixel_size_k1);
  error_dot = (2*C.tau-Ts)/(2*C.tau+Ts)*pixel_size_dot_k1...
      + (2/(2*C.tau+Ts))*(error - error_k1);
  ud = C.h_kd * pixel_size_dot;
  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; 
  pixel_size_k1    = pixel_size;
  pixel_size_dot_k1 = pixel_size_dot;
  error_k1 = error;
  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% heading_hold
%  - regulate heading
%  - produces torque about the yaw axis
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = heading_hold(eps_psi, r, Ts, t, C);
  persistent uik1;
  persistent errork1;
  % initialize persistent variables at beginning of simulation
  if t<0.1, 
      uik1    = 0; 
      errork1 = 0;
  end
 
  error = + pi/2 + eps_psi ;

  % proportional term
  up = C.yaw_kp * error;
  
  % integral term
  ui = uik1 + C.yaw_ki * Ts/2 * (error + errork1);
  
  % dirty 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