www.gusucode.com > Simulink,飞行器控制源码程序 > Simulink,飞行器控制源码程序/Aerial_recovery_demo/drogue_guidance.m
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);