% Retrieve or compute parameters for ducted-fan VTOL micro-UAV.
%
% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt

function [params] = uav_params()

% Unit of measurements unless specified otherwise are
% Mass in kg
% Lenghts in m
% Time in s
% Frequencies in Hz
% Angular velocities in rad / s

params = struct();

% ------------------------------------------------------------------------
% Physical constants

params.physics = struct(...
  'Gravity', 9.81, ...
  'AirDensity', 1.204 ... % kg / m^3
);

% ------------------------------------------------------------------------
% Mechanical measurements

% Inertia marix at center of mass,
% numbers from CAD are in g * mm^2, convert to kg * m^2
J = [
  5.856E+08, 1.121E+06, -3.506E+05;
  1.121E+06, 4.222E+08, 6.643E+06;
  -3.506E+05, 6.643E+06, 4.678E+08
] * 1e-9;

% Approximate propeller with a disk
m_prop = 200e-3; % mass of propeller
r_prop = 85e-3; % radius of propeller
J_prop = .5 * m_prop * r_prop^2;

params.mechanical = struct(...
  'Mass', 3.0, ...
  'DuctRadius', 46e-3, ...
  'DuctHeight', 171e-3, ...
  'FlapZDistance', 98e-3, ... % flap distance along z from center of mass
  'InertiaTensor', J, ...
  'GyroscopicInertiaZ', J_prop ... % assume small angle
);

% ------------------------------------------------------------------------
% Actuator limits and measurements

% Servos usually give a "speed" in seconds / 60 degrees without load
servo_speed = 0.13; % seconds / 60 deg
servo_angular_velocity = (60 / servo_speed) * (pi / 180); % rad / s

params.actuators = struct(...
  'PropellerMaxAngularVelocity', 620.7, ... % in rad / s
  'ServoAbsMaxAngle', 20 * pi / 180, ... % in radians
  'ServoMaxTorque', 4.3 * 1e-2, ... % in kg / m
  'ServoNominalAngularVelocity', servo_angular_velocity ...
);

% IMU runs in NDOF mode
params.measurements = struct(...
  'SensorFusionDelay', 20e-3, ... % in s
  'LIDARAccuracy', 10e-3, ... % in m
  'LIDARMaxDistance', 40, ... % inm
  'LIDARBandwidth', 100, ... % in Hz for z position
  'IMUBandwidth', 100 ... % in Hz
);

% ------------------------------------------------------------------------
% Aerodynamics modelling

% Compute thrust proportionality factor from actuator limits
% from thrust relation F = k * omega^2.
% FIXME: this is not ideal, need better measurements
omega_max = params.actuators.PropellerMaxAngularVelocity;
F_max = 38.637; % in N (measured)
k_T = F_max / omega_max^2;

% FIXME: LiftCoefficient comes from
% https://scienceworld.wolfram.com/physics/LiftCoefficient.html
params.aerodynamics = struct(...
  'ThrustOmegaProp', k_T, ... % in s^2 / (rad * N)
  'FlapArea', 23e-3 * 10e-3, ... % in m^2
  'DragCoefficients', [1, .1], ... % TODO
  'LiftCoefficient', 2 * pi ... % TODO
);


% ------------------------------------------------------------------------
% Linearization point of non-linear dynamics.

% Compute theoretical thrust required to make the UAV hover
% from the relation mg = k * omega^2
% FIXME: This value should probably be replaced with a measurement
g = params.physics.Gravity;
m = params.mechanical.Mass;
k = params.aerodynamics.ThrustOmegaProp;

omega_hover = sqrt(m * g / k);

params.linearization = struct(...
  'PadeApproxOrder', 2, ...
  'Position', [0; 0; -2], ... % in inertial frame, z points down
  'Velocity', [0; 0; 0], ... % in inertial frame
  'Angles', [0; 0; pi / 4], ...   % in body frame
  'AngularVelocities', [0; 0; 0], ... % in body frame
  'Inputs', [0; 0; 0; 0; omega_hover] ... % Flaps at rest and turbine at X
);

% ------------------------------------------------------------------------
% Performance requirements

params.performance = struct(...
  'ReferencePosMaxDistance', 1, ... % m
  'HorizontalPosMaxError', 2, ... % m
  'HorizontalMaxSpeed', 2, ... % m / s
  'HorizontalSettleTime', 6, ... % s
  'VerticalPosMaxError', 2, ... % m
  'VerticalMaxSpeed', 2, ... % m / s
  'VerticalSettleTime', 4, ... % s
  'AngleMaxPitchRoll', 10 * pi / 180, ... % in rad
  'AngleMaxYaw', pi, ... % rad
  'AngleMaxAngularRate', 20 * pi / 180 ... % rad / s
);

% Scaling matrices (because signals are normalized wrt parameter performance)
p = params.performance;

params.ErrorScalingMatrix = blkdiag(...
  eye(4) * params.actuators.ServoAbsMaxAngle, ...
  params.actuators.PropellerMaxAngularVelocity ...
    - params.linearization.Inputs(5), ...
  eye(2) * p.HorizontalPosMaxError, ...
  p.VerticalPosMaxError, ...
  eye(2) * p.HorizontalMaxSpeed, ...
  p.VerticalMaxSpeed, ...
  eye(2) * p.AngleMaxPitchRoll, ...
  p.AngleMaxYaw);

params.OutputScalingMatrix = blkdiag(...
  eye(2) * p.HorizontalPosMaxError, ...
  p.VerticalPosMaxError, ...
  eye(2) * p.HorizontalMaxSpeed, ...
  p.VerticalMaxSpeed, ...
  eye(2) * p.AngleMaxPitchRoll, ...
  p.AngleMaxYaw, ...
  eye(3) * p.AngleMaxAngularRate);


horiz_settle_speed = (1 - exp(-1)) / params.performance.HorizontalSettleTime;
if horiz_settle_speed > params.performance.HorizontalMaxSpeed
  fprintf(['Contradictory performance requirements: ' ...
    'velocity needed to attain horizontal settle time is ' ...
    'higher than HorizontalMaxSpeed']);
end

vert_settle_speed = (1 - exp(-1)) / params.performance.VerticalSettleTime;
if vert_settle_speed > params.performance.VerticalMaxSpeed
  fprintf(['Contradictory performance requirements: ' ...
    'velocity needed to attain vertical settle time is ' ...
    'higher than VerticalMaxSpeed']);
end

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