Code covered by the BSD License

# Aerial Recovery Concept Demo (Gauss's Principle)

### Liang Sun (view profile)

28 Feb 2013 (Updated )

Demonstrate the concept of aerial recovery of miniature aerial vehicles using a towed-cable system.

mothership_ctrl(uu,P)
```function sys = mothership_ctrl(uu,P)

[c, R, mothership, t] = processInput(uu,P);

% track orbit
P.mothership.dir = -1;
phi_d = trackOrbit(mothership, c, R, P.mothership.dir, P, t);

% command the roll rate to drive phi to phi_d
u_phi = sat( P.mothership.k_roll*(phi_d - mothership.phi), P.mothership.phidotbar );

% maintain initial altitude (elevators)
u_gam = sat( -P.mothership.k_hdot*P.mothership.V*sin(mothership.gam)...
+ P.mothership.k_h*(-P.mothership.d + mothership.d), P.mothership.gamdotbar);

sys = [u_phi; u_gam];

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

%
%=============================================================================
% trackOrbit
% track an orbit with center c, radius R, and direction dir (+1=CW, -1=CCW)
% returns the desired roll angle
%=============================================================================
%
function phi_d = trackOrbit(uav, c, R, dir, P, t);
persistent chi_diff

V = P.mothership.V;

chi = wrap2pi( uav.chi );

if t < .1,
chi_diff = 0;
end

d = norm([uav.n; uav.e] - [c(1);c(2)]);
gam = wrap2pi( atan2(uav.e-c(2), uav.n-c(1)) );

chi_d = wrap2pi(gam + dir*pi/2 + dir*atan(P.mothership.k_orbit*(d-R)));
chi_d_dot = V/d*sin(chi-gam) +dir*P.mothership.k_orbit*cos(chi-gam)/(1+(P.mothership.k_orbit*(d-R))^2);

chi_diff = unwrap(wrap2pi(chi-chi_d), chi_diff);
phi_d = sat(atan( V/P.g*( chi_d_dot - P.mothership.k_roll*chi_diff ) ), P.mothership.phibar);

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

%
%=============================================================================
% unwrap
% ensures that there are not jumps of +- 2pi in the signal
%=============================================================================
%
function x = unwrap(x, x_old)
while x-x_old > pi,
x = x-2*pi;
end
while x-x_old < -pi,
x = x+2*pi;
end

%
%=============================================================================
% process input
%   fill pixel, uav, and target structures
%=============================================================================
function [c,R,mothership,t] = processInput(uu,P)

t = uu(end);

c = [uu(1); uu(2)];

R = uu(3);

% mothership states
NN = 3;
mothership.n   = uu(1+NN);       % north position of UAV
mothership.e   = uu(2+NN);       % east position of UAV
mothership.d   = uu(3+NN);       % down position of UAV
mothership.chi = uu(4+NN);       % course
mothership.phi = uu(5+NN);       % roll
mothership.gam = uu(6+NN);       % flight path angle (pitch)

```