drogue_guidance(uu,P)

function uout = drogue_guidance(uu,P)

% process inputs
[drogue,radius_d, t] = processInputs(uu,P);

V_d = P.mothership.V/P.mothership.radius * (radius_d);
V = norm([drogue.ndot;drogue.edot;drogue.ddot]);

if t>50 && (P.drogue.CD_ctrl_flag==1)  %150
    u_delta_CD = sat( P.drogue.k_CD*(V - V_d), P.drogue.CDdotbar);
else
    u_delta_CD = 0;
end

roll_rate = 0;
theta_rate  = 0;
% output of guidance algorithm
uout = [roll_rate; theta_rate; u_delta_CD];

%=============================================================================
% wrap
% wrap chi between [-pi, pi]
%=============================================================================
function chi = wrap(chi)
while chi > pi,
    chi = chi-2*pi;
end
while chi < -pi,
    chi = chi+2*pi;
end

%=============================================================================
% sat
% saturate the input u at the constraint c
%=============================================================================
%
function out = sat(u,c)

if u>c,
    out=c;
elseif u<-c,
    out=-c;
else
    out=u;
end


%========================================================
% processInputs
%   interpret inputs create data structures
%   differentiate angles and camera values
%========================================================
function [drogue, radius_d, t] = processInputs(uu,P)

% get current time
t = uu(end);

% Drogue data
NN = 0;
drogue.n     = uu(1+NN);
drogue.e     = uu(2+NN);
drogue.d     = uu(3+NN);
drogue.ndot  = uu(4+NN);
drogue.edot  = uu(5+NN);
drogue.ddot  = uu(6+NN);
drogue.phi   = uu(7+NN);
drogue.theta = uu(8+NN);
drogue.psi   = uu(9+NN);

NN = 9;
radius_d = uu(1+NN);