% Generate transfer functions for loop shaping performance requirements
% from parameters specified in uav_params.m
%
% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt
%
% Arguments:
%   PARAMS  Struct of design parameters and constants generated by uav_params
%   PLOT    When set to 'true' it plots the inverse magnitude of the
%           performance transfer function
%
% Return value:
%   PERF    Struct performance transfer functions


function [perf] = uav_performance_musyn(params, do_plots)

% Laplace variable
s = tf('s');

% Bandwitdhs
T_alpha = params.actuators.ServoSecondsTo60Deg;
T_omega = 0.1;

T_xy = 6;
T_z = 9;

% Inverse performance functions
W_Palpha = tf(.2);
W_Pomega = tf(.2);

W_Ralpha = 1 / ((s * T_alpha)^2 + 2 * T_alpha * s + 1);
W_Romega = 1 / ((s * T_omega)^2 + 2 * T_omega * s + 1);

W_Rxy = 1 / ((s * T_xy)^2 + 2 * T_xy * .8 * s + 1);
W_Rz = 1 / ((s * T_z)^2 + 2 * T_z * s + 1);

W_Pxy = tf(1);
W_Pz = tf(1);

W_Pxydot = tf(.01);
W_Pzdot = tf(.1);

W_Pphitheta = tf(.005);
W_Ppsi = tf(.2);

% Construct performance vector by combining xy and z
W_Rref = blkdiag(W_Rxy * eye(2), W_Rz);

W_PP = blkdiag(W_Pxy * eye(2), W_Pz);
W_PPdot = blkdiag(W_Pxydot * eye(2), W_Pzdot);
W_PTheta = blkdiag(W_Pphitheta * eye(2), W_Ppsi);

perf = struct(...
  'FlapAngle', W_Palpha * eye(4), ...
  'Thrust', W_Pomega, ...
  'ReferenceFilter', W_Rref, ...
  'FlapAngleFilter', W_Ralpha * eye(4), ...
  'ThrusterFilter', W_Romega, ...
  'Position', W_PP, ...
  'Velocity', W_PPdot, ...
  'Angles', W_PTheta);

if do_plots
  % Bode plots of performance requirements
  figure; hold on;

  bodemag(W_Palpha);
  bodemag(W_Pomega);
  bodemag(W_Pxy);
  bodemag(W_Pz);
  bodemag(W_Pxydot);
  bodemag(W_Pzdot);
  bodemag(W_Pphitheta);
  bodemag(W_Ppsi);

  grid on;
  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
    '$W_{P,xy}$', '$W_{P,z}$', ...
    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
    '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
    'interpreter', 'latex', 'fontSize', 8);
  title('\bfseries Inverse Performance Requirements', ...
    'interpreter', 'latex');

  % Step response of position requirements
  figure; hold on;

  step(W_Palpha);
  step(W_Pomega);
  step(W_Pxy);
  step(W_Pz);
  step(W_Pxydot);
  step(W_Pzdot);
  step(W_Pphitheta);
  step(W_Ppsi);

  grid on;
  legend('$W_{P,\alpha}$', '$W_{P,\omega}$', ...
    '$W_{P,xy}$', '$W_{P,z}$', ...
    '$W_{P,\dot{x}\dot{y}}$', '$W_{P,\dot{z}}$', ...
    '$W_{P,\phi\theta}$', '$W_{P,\psi}$', ...
    'interpreter', 'latex', 'fontSize', 8);
  title('Step responses of performance requirements');
end

end

% Make a n-order performance weight function
%
% Arguments:
%   OMEGA  Cutting frequency (-3dB)
%   A      Magnitude at DC, i.e. |Wp(0)|
%   M      Magnitude at infinity, i.e. |Wp(inf)|
%   ORD    Order
function [Wp] = make_weight(omega, A, M, ord)

if nargin > 3
  n = ord;
else
  n = 1;
end

s = tf('s');
Wp = (s / (M^(1/n)) + omega)^n / (s + omega * A^(1/n))^n;

end
% vim: ts=2 sw=2 et: