function u= FFcontroller(i,g)
global UavTeam
global taskdone
global N

M = UavTeam.AvailableNumMax;

Pcur  =  UavTeam.Team(g).Uav(i).CurrentPos;
Vcur  =  UavTeam.Team(g).Uav(i).Velocity;
Pdes  =  UavTeam.Team(g).Uav(i).Waypoint(:, UavTeam.Team(g).Uav(i).CurrentTaskNum);
rm = UavTeam.Team(g).Uav(i).r;
ra = UavTeam.Team(g).Uav(i).ra;
vmax = UavTeam.Team(g).Uav(i).vmax;
UavTeam.Team(g).Uav(i).state = 6;

%% FreeFlight状态判断条件，到家点，deliver==1
if  Pdes == UavTeam.Team(g).Uav(i).HomePos & norm(Vcur)<0.1 & norm(Pdes-Pcur)<UavTeam.Team(g).Uav(i).ra
    fprintf('%d组',g);
    fprintf('飞机%dFF中开始land \n',i);
    taskdone(M*(g-1)+i,4)=1;    % land
end
%% FreeFlight状态判断条件，到投递点，land=1
if  Pdes == UavTeam.Team(g).Uav(i).Toufangdian & norm(Vcur)<0.1 & norm(Pdes-Pcur)<UavTeam.Team(g).Uav(i).ra
    fprintf('%d组',g);
    fprintf('飞机%dFF中开始deliver \n',i);
    taskdone(M*(g-1)+i,3)=1;    % deliver
end

% Velocity to waypoint
k1 = 1;
ksi_w  = Pcur + 1/UavTeam.gain*Vcur - Pdes;
Vw = - mysat(k1*ksi_w,vmax);

% Velocity away other multicopters
Vm = [0;0];
for ggg = 1:N
    for l = 1:UavTeam.AvailableNumMax
        if i~=l && UavTeam.Team(ggg).Uav(l).state ~= 2
            k2 = 1; e = 0.000001; rs = e;
            ksimil = (Pcur - UavTeam.Team(ggg).Uav(l).CurrentPos) + 1/UavTeam.gain*(Vcur - UavTeam.Team(ggg).Uav(l).Velocity);
            temp = (1+e)*norm(ksimil) - (2*rm)*mys(norm(ksimil)/(2*rm),rs);
            bil = k2*dmysigma(norm(ksimil),2*rm,ra)/temp ...
                - k2*mysigma(norm(ksimil),2*rm,ra)*((1+e)-dmys(norm(ksimil)/(2*rm),rs))/(temp^2);
            % 分子为零的危险
            if ksimil == 0
            else
                Vm = Vm - bil*(ksimil/norm(ksimil));
            end
        end
    end
end

% 当前无障碍
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Vo=[0;0];

% Sum of all velocities
u = mysat(Vw+Vo+Vm,vmax);