%VSDR    Variable speed dead reckon
%    Plots Odyssey II trajectory by dead reckoning using propeller rpm to
%    estimate forward speed
%
% Copyright (C) 1997, Massachussetts Institute of Technology
% B. A. Moran, Sea Grant Underwater Vehicles Laboratory
% $Revision: 1.1 $


if exist( 'do_3d_plot' ) ~= 1
    do_3d_plot = 0;
end

% Because the vehicle has a little problem with position initialization,
% we're going to try and correct the first cycle if a large jump is
% detectable.

rows = 1:length(data);
step = norm( diff( data(1:2,[m_dr_north m_dr_east])) );
if step > 100
    disp( ['ignoring 1st cycle because step = ' num2str(step) ' m...'] );
    rows(1) = [];
end

rows( ~finite(data(rows,m_propeller_speed)) ) = [];

% With the small CETUS propeller, the vehicle speed is approximately 1.6 m/s
% at propeller rpm of 442.4.  Estimate the fwd speed as linear with
% propeller at that conversion ratio.

speed = data(rows,m_propeller_speed) * 1.6 / 442.4;
[dr_east,dr_north] = drcalc( time(rows), data(rows,m_yaw), ...
    data(rows,m_pitch), speed, data(1,[m_dr_east m_dr_north]) );

if do_3d_plot == 1
    plot3( dr_east/1000, dr_north/1000, ...
        data(rows,m_depth), 'g' ), set( gca, 'zdir', 'reverse' );
    zlim = get( gca, 'zlim' );
    line( dr_east/1000, dr_north/1000, 'Color', 'r' );

    xlabel( 'East (km)' );
    ylabel( 'North (km)' );
    zlabel( 'depth (m)' );
else

    plot( dr_east/1000, dr_north/1000, 'g' )
    text( dr_east(1)/1000, dr_north(1)/1000, 'S', 'FontSize', 14 );
    ylabel( 'North (km)' );
    xlabel( 'East (km)' );
    set( gca, 'dataAspectRatio', [1 1 1] );
    zoom on
end
                                % common to 2d and 3d plots
title( ['Vehicle Trajectory by Dead Reckoning: ' run_name], 'FontSize', 10 );
