aerospace/quadcopter-pd-controller/utils/QuadPlot.m
classdef QuadPlot < handle
% Visualization class for quad
properties (SetAccess = public)
k = 0
qn % quad number
time = 0 % time
state % state
des_state % desried state [x; y; z; xdot; ydot; zdot];
rot % rotation matrix body to world
color % color of quad
wingspan % wingspan
height % height of quad
motor % motor position
state_hist % position history
state_des_hist % desired position history
time_hist % time history
max_iter % max iteration
end
properties (SetAccess = private)
h_3d
h_m13 % motor 1 and 3 handle
h_m24 % motor 2 and 4 handle
h_qz % z axis of quad handle
h_qn % quad number handle
h_pos_hist % position history handle
h_pos_des_hist % desired position history handle
text_dist % distance of quad number to quad
end
methods
% Constructor
function Q = QuadPlot(qn, state, wingspan, height, color, max_iter, h_3d)
Q.qn = qn;
Q.state = state;
Q.wingspan = wingspan;
Q.color = color;
Q.height = height;
Q.rot = convertQuatToRot(Q.state(7:10));
Q.motor = calculatePos(Q.state(1:3), Q.rot, Q.wingspan, Q.height);
Q.text_dist = Q.wingspan / 3;
Q.des_state = Q.state(1:6);
Q.max_iter = max_iter;
Q.state_hist = zeros(6, max_iter);
Q.state_des_hist = zeros(6, max_iter);
Q.time_hist = zeros(1, max_iter);
% Initialize plot handle
if nargin < 7
h_3d = gca;
end
Q.h_3d = h_3d;
hold(Q.h_3d, 'on');
Q.h_pos_hist = plot3(Q.h_3d, Q.state(1), Q.state(2), Q.state(3), 'r.');
Q.h_pos_des_hist = plot3(Q.h_3d, Q.des_state(1), Q.des_state(2), Q.des_state(3), 'b.');
Q.h_m13 = plot3(Q.h_3d, ...
Q.motor(1, [1 3]), ...
Q.motor(2, [1 3]), ...
Q.motor(3, [1 3]), ...
'-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5);
Q.h_m24 = plot3(Q.h_3d, ...
Q.motor(1, [2 4]), ...
Q.motor(2, [2 4]), ...
Q.motor(3, [2 4]), ...
'-ko', 'MarkerFaceColor', Q.color, 'MarkerSize', 5);
Q.h_qz = plot3(Q.h_3d, ...
Q.motor(1, [5 6]), ...
Q.motor(2, [5 6]), ...
Q.motor(3, [5 6]), ...
'Color', Q.color, 'LineWidth', 2);
Q.h_qn = text( ...
Q.motor(1, 5) + Q.text_dist, ...
Q.motor(2, 5) + Q.text_dist, ...
Q.motor(3, 5) + Q.text_dist, num2str(qn));
hold(Q.h_3d, 'off');
end
% Update quad state
function UpdateQuadState(Q, state, time)
Q.state = state;
Q.time = time;
Q.rot = convertQuatToRot(state(7:10))'; % Q.rot needs to be body-to-world
end
% Update desired quad state
function UpdateDesiredQuadState(Q, des_state)
Q.des_state = des_state;
end
% Update quad history
function UpdateQuadHist(Q)
Q.k = Q.k + 1;
Q.time_hist(Q.k) = Q.time;
Q.state_hist(:, Q.k) = Q.state(1:6);
Q.state_des_hist(:, Q.k) = Q.des_state(1:6);
end
% Update motor position
function UpdateMotorPos(Q)
Q.motor = calculatePos(Q.state(1:3), Q.rot, Q.wingspan, Q.height);
end
% Truncate history
function TruncateHist(Q)
Q.time_hist = Q.time_hist(1:Q.k);
Q.state_hist = Q.state_hist(:, 1:Q.k);
Q.state_des_hist = Q.state_des_hist(:, 1:Q.k);
end
% Update quad plot
function UpdateQuadPlot(Q, state, des_state, time)
Q.UpdateQuadState(state, time);
Q.UpdateDesiredQuadState(des_state);
Q.UpdateQuadHist();
Q.UpdateMotorPos();
set(Q.h_m13, ...
'XData', Q.motor(1, [1 3]), ...
'YData', Q.motor(2, [1 3]), ...
'ZData', Q.motor(3, [1 3]));
set(Q.h_m24, ...
'XData', Q.motor(1, [2 4]), ...
'YData', Q.motor(2, [2 4]), ...
'ZData', Q.motor(3, [2 4]));
set(Q.h_qz, ...
'XData', Q.motor(1, [5 6]), ...
'YData', Q.motor(2, [5 6]), ...
'ZData', Q.motor(3, [5 6]));
set(Q.h_qn, 'Position', ...
[Q.motor(1, 5) + Q.text_dist, ...
Q.motor(2, 5) + Q.text_dist, ...
Q.motor(3, 5) + Q.text_dist]);
set(Q.h_pos_hist, ...
'XData', Q.state_hist(1, 1:Q.k), ...
'YData', Q.state_hist(2, 1:Q.k), ...
'ZData', Q.state_hist(3, 1:Q.k));
set(Q.h_pos_des_hist, ...
'XData', Q.state_des_hist(1, 1:Q.k), ...
'YData', Q.state_des_hist(2, 1:Q.k), ...
'ZData', Q.state_des_hist(3, 1:Q.k));
drawnow;
end
end
end