www.gusucode.com > Simulink——飞行器轨迹恢复源码程序 > Simulink——飞行器轨迹恢复源码程序/Aerial_recovery_demo/mothership_ctrl.m
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)