classdef Drone < handle

% DRONE : This class is meant to create and manage drones.

% Every drone is either a fixed-wing or a quadcopter (drone_type).
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Drone general properties:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% drone_type:
% enumerator class DroneType
% autopilot_version:
% 1 for quad attitude, 2 for quad speed, 3 for quad acc
% p_drone, p_battery, p_sim, p_physics, map:
% parameter structures associated to the drone (p_drone),
% the battery (p_battery), the simulation (p_sim), physics
(p_physics) or the map (map)
% pos_ned:
% vect3, position coordinates in the inertial frame
% (North, East and Down) [m]
% vel_xyz:
% vect3, velocity in body frame (vx, vy, vz) [m/s]
% attitude:
% vect3, attitude of the drone (phi, theta, psi) [rad]
% rates:
% vect3, angular rates w.r.t. phi, theta and psi [rad/s]
% pos_ned_history: matrix of size n, 3: each row is vect of previous
% pos_ned, position coordinates in the inertial frame
% (North, East and Down) [m]
%
% vel_xyz_history: matrix of size n, 3: each row is vect of previous
% vel_xyz, velocities in the body frame
% (North, East and Down) [m/s]
% prev_state:
% vect12, contains the previous state.
% state = (pos_ned, vel_xyz, attitude, rates)
% path_len:
% path lenght of the drone, from the beginning of the
% simulation
% z_hat:
% vect22, estimated extended state
% airdata:
% vect6, (va, alpha, beta, wn, we, wd)
% command:
% vect4, commmand input. The variables contained depend on
% the autopilot version
% prev_command:
% vect4, store the previous command input
% full_command:
% vect19, full command state vector, used in the function
% plot_uav_state_variable
% delta:
% vect4,
% for quadcopter: normalized angular velocities commanded to
% the 4 motors
% forces:
% vect3, 3D vector of the forces acting on the drone [N]
% computed in the body frame
% moments:
% vect3, 3D vector of the moments acting on the drone [N*m]
% computed in the body frame
% wind:
% vect6, wind and gusts in xyz coordinates
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Handles and other variables for the autopilot:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% body_handle:
% graphic handle associated to the body of the drone
% path_handle:
% graphic handle associated to the path of the drone
% waypoint_handle:
% graphic handle associated to the waypoints
% vertices:
%
% faces:
% drone (sur)faces
% face_colors:
% colors associated to the drone surfaces in the plot
% color:
% color associated to the drone for scatterplots, offline
% trajectory plots, ecc.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Variables for the battery:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% i:
% vect5, current intensity over a 5-time-step window [A]
% Q:
% capacity consumed from the beginning of the flight [Ah]
% Qt:
% capacity consumed in a time step dt [Ah]
% V:
% voltage [V]
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Variables for path planner/manager:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% path:
% vect26 // TODO: describe the content
% nb_waypoints:
% nb of waypoints that the drone follow
% waypoints:
% vect5*nb_waypoints to reshape before using
% transition_time:
% time at which the transition to the new waypoint started
% alpha:
% 1 when transition starts, 0 at the end
% command_before_transition:
% // TODO: describe the content
%

properties

% Parameters
drone_type DroneType
autopilot_version
p_drone
p_battery
p_sim
p_physics

map % FIXME: why does the drone need a map? That's not logical

% State = [pos_ned; vel_xyz; attitude; rates]

pos_ned % pn, pe, pd
vel_xyz % vx, vy, vz
attitude % phi, theta, psi
rates % p, q, r

% State history
pos_ned_history
vel_xyz_history

% Auxiliary variables
prev_state
path_len
y % sensor measurements
z_hat % esitmated extended state
airdata % air data

% Command variables
command
prev_command
full_command
delta

% Forces and moments

forces
moments
wind

% Set handles and other variables for plots

body_handle
path_handle
waypoint_handle
vertices
faces
face_colors
color
state_handle

% Variables for autopilot-quad

altitude_state
altitude_state_prev
initialize_integrator
P_h_thrust
P_roll_torque
P_pitch_torque
P_psi_torque
P_ad_thrust
P_vn_pitch
P_ve_roll
P_vd_thrust
P_ae_roll
P_an_pitch
P_pd_thrust
P_pe_roll
 P_pn_pitch

% Variables for autopilot-wing

lat_state
lat_state_prev
lat_init_integrator
lat_counter_last_change
P_phi_da
P_chi_phi
P_theta_de
P_h_theta
P_va_dt
P_va_theta

% Variables for the battery

i
Q
Qt
V

% Variables for path planner/manager

path
nb_waypoints
waypoints
transition_time
alpha
command_before_transition

use_estimation = false

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

methods

function Drone = Drone(drone_type, ...

p_drone, p_battery, p_sim, p_physics, map)

% Create a drone: assign parameters and initialize state to 0

Drone.drone_type = drone_type;

switch drone_type
case "quadcopter"
Drone.autopilot_version = 2;
case "fixed_wing"
Drone.autopilot_version = -1;
case "point_mass"
Drone.autopilot_version = 2;
end

Drone.p_drone = p_drone;
Drone.p_battery = p_battery;
Drone.p_sim = p_sim;
Drone.p_physics = p_physics;
Drone.map = map;

Drone.pos_ned = zeros(3, 1);

Drone.vel_xyz = zeros(3, 1);
 Drone.attitude = zeros(3, 1);
Drone.rates = zeros(3, 1);

Drone.pos_ned_history = [];
Drone.vel_xyz_history = [];

Drone.path_len = 0;

Drone.command = zeros(4, 1);

Drone.prev_command = zeros(4, 1);
Drone.full_command = zeros(19, 1);

Drone.z_hat = zeros(22, 1);

Drone.delta = zeros(4, 1);
Drone.airdata = zeros(6, 1);

Drone.forces = zeros(3, 1);

Drone.moments = zeros(3, 1);

Drone.body_handle = [];
Drone.path_handle = [];
Drone.waypoint_handle = [];
cmap = jet(16);
Drone.color = cmap(floor(rand*16)+1,:)';
Drone.state_handle = [];

Drone.altitude_state = 0;
Drone.altitude_state_prev = 0;
Drone.initialize_integrator = 0;
Drone.lat_state = 0;
Drone.lat_state_prev = 0;
Drone.lat_init_integrator = 0;
Drone.lat_counter_last_change = 0;

Drone.i = zeros(5, 1);

Drone.Q = p_battery.Q0;
Drone.V = p_battery.V0;

Drone.path = zeros(26, 1);

Drone.alpha = 0;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function init_rand_pos(self, map_size)
% INIT_RAND_POS: Initialize the drone to a random position
% on the map
self.pos_ned = map_size .* rand(3, 1);
self.pos_ned(3) = -self.pos_ned(3); % h = -pd
% Update pos_ned_history
if isempty(self.pos_ned_history)
self.pos_ned_history = self.pos_ned';
else
self.pos_ned_history = [self.pos_ned_history; self.pos_ned'];
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function set_pos(self, position_ned)
 % SET_POS: Set the position of the drone in the NED frame
self.pos_ned = position_ned;
% Update pos_ned_history
if isempty(self.pos_ned_history)
self.pos_ned_history = self.pos_ned';
else
self.pos_ned_history = [self.pos_ned_history; self.pos_ned'];
end
self.z_hat = true_states([self.get_state(); self.airdata; ...
NaN]);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function set_vel(self, velocity_xyz)
% SET_VEL: Set the velocity of the drone
self.vel_xyz = velocity_xyz;
if isempty(self.vel_xyz_history)
self.vel_xyz_history = self.vel_xyz';
else
self.vel_xyz_history = [self.vel_xyz_history; self.vel_xyz'];
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function state = get_state(self)
% GET_STATE: Get the state of the drone, vect12
state = [self.pos_ned; self.vel_xyz; self.attitude; self.rates];
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function set_state(self, pos_ned, vel_xyz, attitude, rates)
% SET_STATE: Set the drone state to the one passed in argument
self.set_pos(self, pos_ned)
self.set_vel(vel_xyz);
self.attitude = attitude;
self.rates = rates;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function change_param(self, autopilot_version, p_drone)
% CHANGE_PARAM: Change the drone parameters when changed from
% GUI
self.autopilot_version = autopilot_version;
self.p_drone = p_drone;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_state(self, wind, time)
% UPDATE_STATE: Compute the new drone state

self.update_sensor_measurements();
self.estimate_states(time);

% Choose the autopilot

if self.drone_type ~= "point_mass"
if self.drone_type == "fixed_wing"
temp3 = autopilot_wing(self, 0, time);
 elseif self.drone_type == "quadcopter"
temp3 = autopilot_quad(self, time);
end

self.delta = temp3(1:4);
self.full_command = temp3(5:end);
self.compute_dynamics(wind, time);
self.compute_kinematics(time);
self.update_battery();

elseif self.drone_type == "point_mass"

% Computes the new drone position with Euler forward method.
% This method does not take the attitude into account.
% We suppose that the attitude is always (0,0,0), so the
% velocity in the body frame correponds to the velocity in the
% inertial frame. Only usable with the velocity controller.

self.vel_xyz = self.command(2:4);
self.pos_ned = self.pos_ned + self.vel_xyz * self.p_sim.dt;
self.attitude(3) = self.command(1); % to plot drone psi angle

% Update pos_ned_history
if isempty(self.pos_ned_history)
self.pos_ned_history = self.pos_ned';
else
self.pos_ned_history = [self.pos_ned_history; self.pos_ned'];
end

% Update vel_xyz_history
if isempty(self.vel_xyz_history)
self.vel_xyz_history = self.vel_xyz';
else
self.vel_xyz_history = [self.vel_xyz_history; self.vel_xyz'];
end
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_prev_state(self)
% UPDATE_PREV_STATE: Update the state variables of the drone,
% by replacing the old with the new ones.

self.prev_state = self.get_state();
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_path_length(self)
% UPDATE_PATH_LENGTH: Update the total lenght of the path that
% has been flyed by a drone
self.path_len = self.path_len + ...
pdist([self.prev_state(1:3)'; self.pos_ned'], 'euclidean');
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_battery(self)
% UPDATE_BATTERY: Update the state variables of the battery
% (i, V, Q)
%
 % i : intensity [A]
% V : voltage [V]
% Q : consumed capacity [Ah]

param = self.p_battery;
% // TODO drone.update_actuators();
omega = (self.p_drone.k_omega * self.delta) * 30 / pi; % [rpm]
pow_motors = self.rpm2power(omega(1)) + self.rpm2power(omega(2)) + ...
self.rpm2power(omega(3)) + self.rpm2power(omega(4));
pow_tot = param.power_board + pow_motors;

% Update current vector (i) with the new measurement

self.i = circshift(self.i, -1);
self.i(end) = pow_tot / self.V;

% Moving-average filter on current (i)

window_size = length(self.i);
b = (1 / window_size) * ones(1, window_size);
a = 1;
signal_filt = filter(b, a, self.i);
i_filt = signal_filt(end);

self.Qt = (i_filt * self.p_sim.dt) / 3600;

self.Q = self.Q + self.Qt; %[Ah]

V_nominal = param.e0 + param.e1 * (self.Q / param.Qf) + ...

param.e2 * (self.Q / param.Qf)^2;
V_final = param.A * exp(-param.B * (param.Qf - self.Q));
self.V = V_nominal - param.R * i_filt - V_final;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function actuator_power = rpm2power(self, rpm)
% Compute the power required from a motor from its velocity
% expressed in rpm (round per minute)
param = self.p_battery;
actuator_power = param.p0 * (param.p1 * rpm^2 + param.p2 * rpm^4 + ...
param.p3 * rpm^6 + param.p4 * rpm^8); %[W]
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function Qt = compute_Qt_hover(self)
% Compute the capacity consumed in a time frame Ts for hovering
omega = (self.get_delta_hover()) * 30 / pi; % [rpm]
pow_motors = 4 * self.rpm2power(omega);
pow_tot = self.B.pow_board + pow_motors;

intensity = pow_tot / self.B.V0;

Qt = (intensity * self.p_sim.dt) / 3600; %[Ah]

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_actuators(self)
% UPDATE_ACTUATORS: Model for the motor dynamics

% equations: J omega_dot + b omega = K i

% L i_dot + R i = V - K omega
 % constants: to take from param or put in param if non-existent
b = 0; % viscous constant
L = 0.5e-3; % arm inductance
R = 0.05; % arm resistance
K = 4.2e-3; % EMF constant

% // TO DO: differentiate between commanded delta and current

% delta
omega_old = self.delta * self.p_drone.k_omega;
i_old = self.i(5);

omega_dot = (-b * omega_old + K * i_old) / self.p_drone.J_prop;

i_dot = (-K * omega_old - R * i_old + self.V) / L;

self.delta = (omega_dot * self.p_sim.dt + omega_old) /

self.p_drone.k_omega;
self.i = i_dot * self.p_sim.dt + i_old;

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
plan_path(self, path_type, time)
% PLAN_PATH: get waypoints

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function manage_path(self, time)
% MANAGE_PATH: extract path from waypoints

if self.drone_type == "fixed_wing"
self.path = path_manager_wing([self.nb_waypoints; ...
self.waypoints; self.z_hat; time], ...
self.p_drone, self.p_sim);
elseif self.drone_type == "quadcopter" || self.drone_type ==
"point_mass"
self.path = path_manager_quad([self.nb_waypoints; ...
self.waypoints; self.z_hat; time], ...
self.p_drone, self.p_sim);
end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function follow_path(self)
% FOLLOW_PATH: get command from path

if self.drone_type == "fixed_wing"
self.command = path_follower_wing(self.path, self.p_drone);
elseif self.drone_type == "quadcopter" || self.drone_type ==
"point_mass"
self.command = path_follower_quad(self.path, self.p_drone);
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function delta_hover = get_delta_hover(self)
delta_hover = sqrt((self.p_drone.mass * self.p_physics.gravity) / ...
(self.p_drone.C_prop)) / (4 * self.p_drone.k_omega);
end
 plot_state(self, time, output_rate)

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

methods (Access = private)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_dynamics(self, wind, time)
% COMPUTE_DYNAMICS: Compute forces and moments on the drone
% from its current state.

if self.drone_type == "fixed_wing"
out = forces_moments_wing(self.get_state(), self.delta, ...
wind, time, self.p_drone, self.p_physics);
elseif self.drone_type == "quadcopter"
out = forces_moments_quad(self.get_state(), self.delta, ...
wind, time, self.p_drone, self.p_sim, self.p_physics);
end

self.forces = out(1:3); % fx, fy, fz

self.moments = out(4:6); % l, m, n
self.airdata = out(7:12); % va, alpha, beta, wn, we, wd
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function compute_kinematics(self, time)
% COMPUTE_KINEMATICS: Compute the new state of the drone from
% the forces and moments applied on it.

self.update_prev_state();

x_old = self.get_state();

uu = [self.forces; self.moments];

tspan = [time time+self.p_sim.dt];

[~, x_ode] = ode23(@(t, x) kinematics_ode_fun(time, x, ...

x_old, uu, self.p_drone), tspan, x_old);

x_new = x_ode(end, :);

self.pos_ned = x_new(1:3)';
if self.pos_ned_history == [NaN, NaN, NaN]
self.pos_ned_history = self.pos_ned;
else
self.pos_ned_history = [self.pos_ned_history; self.pos_ned'];
end
self.vel_xyz = x_new(4:6)';
self.attitude = x_new(7:9)';
self.rates = x_new(10:12)';

self.update_path_length();
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_sensor_measurements(self)
 % UPDATE_SENSOR_MEASUREMENTS: Update the sensor measurements
% based on the sensor parameters.

if self.use_estimation
y_imu_baro = sensors(self.get_state(), self.forces,
self.airdata, ...
self.p_drone, self.p_physics);
y_gps = gps(self.get_state(), self.p_sim.dt, self.p_drone);

self.y = [y_imu_baro; y_gps];

end

end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function estimate_states(self, time)

if self.use_estimation
self.z_hat = estimate_states(self.y, time, self.p_sim.dt, ...
self.p_drone, self.p_physics);
else
self.z_hat = true_states([self.get_state(); self.airdata; ...
time]);
end
end

end

end