Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: sailboat add matlab VPP tool

Browse Source
This commit is contained in:
IamPete1 2018-10-05 23:23:08 +01:00 committed by Randy Mackay
parent 1792438660
commit e31f98157b
1 changed files with 159 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
libraries/SITL/tools/Sailboat_VPP.m Normal file
View File

@ -0,0 +1,159 @@
clc
clear all
close all
%{
Very basic Velocity Prediction Program, this duplicates the physics used in the sailboat SITL model. It can be used to generate polar plots to check
STIL physics is sensible.
In the future it is hoped this program can be used as a comparison for a built in polar generator that is yet to be added to the ArduRover code.
Alternatively a drag polar generated here can be used directly in the code to select optimal sailing angles.
Sail lift, drag and Hull drag not considered properly, boat heel not considered. The polar generated is however realistic for a 'generic' sailboat.
%}
sail_aoa_in = [0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 179.9]; % deg
lift_curve_in = [0.00, 0.50, 1.00, 1.10, 0.95, 0.75, 0.60, 0.40, 0.20, 0.00, -0.20, -0.40, -0.60, -0.75, -0.95, -1.10, -1.00, -0.50, 0];
drag_curve_in = [0.10, 0.10, 0.20, 0.40, 0.80, 1.20, 1.50, 1.70, 1.90, 1.95, 1.90, 1.70, 1.50, 1.20, 0.80, 0.40, 0.20, 0.10, 0.10];
min_sail_angle = 0; % deg
max_sail_angle = 90; % deg
% intpolate to more detailed curves
sail_aoa = sail_aoa_in(1):0.1:sail_aoa_in(end)-0.1;
lift_curve = interp1(sail_aoa_in, lift_curve_in, sail_aoa);
drag_curve = interp1(sail_aoa_in, drag_curve_in, sail_aoa);
% Plot lift curve
figure
subplot(2,1,1)
hold all
title('Lift and Drag Curves')
scatter(sail_aoa_in,lift_curve_in,'*','r')
plot(sail_aoa,lift_curve,'r')
ylabel('Cl')
subplot(2,1,2)
hold all
scatter(sail_aoa_in,drag_curve_in,'*','b')
plot(sail_aoa,drag_curve,'b')
xlabel('AoA (deg)')
ylabel('Cd')
%% VPP
% Do for range of sailing angles and wind speeds, increse speed until foward force equals zero
wind_speed = 3:2:15; % m/s
heading = 0:1:179; % deg
speed_step = 0.01; % m/s
for n = 1:length(wind_speed)
for m = 1:length(heading)
thrust = 1;
boat_speed{n,m}(1) = 0;
j = 0;
while thrust > 0 && boat_speed{n,m}(end) < 10
j = j + 1;
if j == 1
boat_speed{n,m}(j) = speed_step;
else
boat_speed{n,m}(j) = boat_speed{n,m}(j-1) + speed_step;
end
% calculate apparent wind in earth-frame (this is the direction the wind is coming from)
wind_apparent_speed{n,m}(j) = sqrt(wind_speed(n)^2 + boat_speed{n,m}(j)^2 + 2 * wind_speed(n) * boat_speed{n,m}(j) * cosd(heading(m)));
% Aparent wind angle in body frame
wind_apparent_dir_bf{n,m}(j) = acosd((wind_speed(n) * cosd(heading(m)) + boat_speed{n,m}(j))/wind_apparent_speed{n,m}(j));
% calculate lift at all angles-of-attack from wind to mainsail
% calculate Lift force (perpendicular to wind direction) and Drag force (parallel to wind direction)
lift = lift_curve * wind_apparent_speed{n,m}(j);
drag = drag_curve * wind_apparent_speed{n,m}(j);
% rotate lift and drag from wind frame into body frame
sin_rot = sind(wind_apparent_dir_bf{n,m}(j));
cos_rot = cosd(wind_apparent_dir_bf{n,m}(j));
force_fwd = (lift * sin_rot) - (drag * cos_rot);
% accel in body frame due acceleration from sail and deceleration from hull friction
hull_drag = 0.5 * boat_speed{n,m}(j)^2;
thrust_range = force_fwd - hull_drag;
% angle of sail to boat
sail_boat_angle = wind_apparent_dir_bf{n,m}(j) - sail_aoa;
% discard imposible sail angles
index = find(sail_boat_angle > max_sail_angle | sail_boat_angle < min_sail_angle);
thrust_range(index) = NaN;
% select sail angle that gives best thrust
index = find(thrust_range == max(thrust_range),1);
if ~isempty(index)
thrust = thrust_range(index);
sail_force{n,m}(j) = force_fwd(index);
sail_angle_trim{n,m}(j) = sail_boat_angle(index);
hull_drag_trim{n,m}(j) = hull_drag;
else
thrust = 0;
end
end
max_boat_speed(n,m) = boat_speed{n,m}(j);
% Set minumum values to NaN to make plot neater
if max_boat_speed(n,m) == speed_step
max_boat_speed(n,m) = NaN;
end
end
end
%% Polar plot
figure
for n = 1:length(wind_speed)
polarplot(deg2rad(heading),max_boat_speed(n,:))
hold on
legend_val{n} = sprintf('%g m/s',wind_speed(n));
end
ax = gca;
d = ax.ThetaDir;
ax.ThetaDir = 'clockwise';
ax.ThetaZeroLocation = 'top';
legend(legend_val)
title('Polar Plot, boat speed (m/s) vs heading (deg)')
%% Plot sail thrust and hull drag for a given heading
plot_heading = 50;
plot_wind_speed = 5;
% convet from values to indexs
plot_heading = find(heading == plot_heading);
plot_wind_speed = find(wind_speed == plot_wind_speed);
if isempty(plot_heading) || isempty(plot_wind_speed)
fprintf('Plot headings and or windspeed not within the range caulated')
return
end
figure
subplot(2,1,1)
hold all
title(sprintf('Thrust vs drag for heading of %g deg and wind speed of %g m/s',heading(plot_heading),wind_speed(plot_wind_speed)))
yyaxis left
plot(boat_speed{plot_wind_speed,plot_heading},wind_apparent_speed{plot_wind_speed,plot_heading})
ylabel('Apparent wind speed (m/s)')
yyaxis right
plot(boat_speed{plot_wind_speed,plot_heading},wind_apparent_dir_bf{plot_wind_speed,plot_heading})
ylabel('Apparent wind direction (deg)')
subplot(2,1,2)
hold all
plot(boat_speed{plot_wind_speed,plot_heading},sail_force{plot_wind_speed,plot_heading})
plot(boat_speed{plot_wind_speed,plot_heading},hull_drag_trim{plot_wind_speed,plot_heading})
legend('Sail Forwards force','Hull drag','Location','northwest')
ylabel('Force (N)')
xlabel('Boat speed (m/s)')