Script for determining helicopter setup equilibrium conditions

determine_equilibrium.m

% Example usage

eq_angle_deg = 15; % degrees
data = load('/MATLAB Drive/ramp_test.mat').IO_data;
[x_eq, u_eq] = get_eq_conditions(eq_angle_deg, data);

% Output

fprintf('x_eq(1) = %.3f\n',x_eq(1));
fprintf('x_eq(2) = %.3f\n',x_eq(2));
fprintf('x_eq(3) = %.3f\n',x_eq(3));
fprintf('u_eq = %.3f\n',u_eq);

function [x_eq, u_eq] = get_eq_conditions(eq_angle_deg, data, offset)

x_1 = data(2, :);
x_1 = lowpass(x_1, 0.1);
w_1 = data(4, :);
w_1 = lowpass(w_1, 0.1);
u_1 = data(5, :);

x_eq = [0 0 0];

x_eq(1) = deg2rad(eq_angle_deg); % eq angle

assert(abs(x_eq(1)) < max(abs(x_1)), "Equilibrium angle larger than what the motor can produce");

[~, ix] = min(abs(x_1-x_eq(1)));
u_eq = u_1(ix);
x_eq(2) = 0; % eq velocity (always 0)
x_eq(3) = w_1(ix); % eq rpm

% Optional graphs for debugging
figure(1);
plot(u_1, rad2deg(x_1), 'r-', u_eq, rad2deg(x_eq(1)), 'p', 'MarkerSize', 10, 'MarkerFaceColor', [0, 0, 0]);
figure(2);
plot(u_1, w_1, 'r-', u_eq, x_eq(3), 'p', 'MarkerSize', 10, 'MarkerFaceColor', [0, 0, 0]);

end