Professional Documents
Culture Documents
Matlab Drone Specs
Matlab Drone Specs
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 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
use_estimation = false
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
methods
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_history = [];
Drone.vel_xyz_history = [];
Drone.path_len = 0;
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;
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);
self.delta = temp3(1:4);
self.full_command = temp3(5:end);
self.compute_dynamics(wind, time);
self.compute_kinematics(time);
self.update_battery();
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;
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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function update_actuators(self)
% UPDATE_ACTUATORS: Model for the motor dynamics
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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];
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);
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