-
Notifications
You must be signed in to change notification settings - Fork 1
/
fixcompass.m
90 lines (77 loc) · 2.89 KB
/
fixcompass.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
%======================================================================
% F I X C O M P A S S . M
% doc: Fri Dec 30 14:59:27 2011
% dlm: Sun Mar 6 12:15:39 2016
% (c) 2011 A.M. Thurnherr
% uE-Info: 42 0 NIL 0 0 72 0 2 4 NIL ofnI
%======================================================================
function [d,p]=fixcompass(d,p)
%
% fix compass issues
%
% p.fix_compass=1 apply offset only
% p.fix_compass=2 fix up looking compass to down looking compass + offset
% p.fix_compass=3 fix down looking compass to up looking compass + offset
%
% p.hdg_offset will apply a fixed rotation to the compass. In most cases you
% will want to set the compass that you trust to 0 and rotate the other one
% by the difference in direction between the two beam 3 headings as mounted
% on the frame
% : [downoffset upoffset]
%
% once you done this rotup2down gets disabled since I assume you know what the offset was
%
% M. Visbeck LDEO 2004
%
% Changes by ant:
% Dec 30, 2011: - BUG: p.fix_compass = 2 and = 3 did not work; bug report and fix
% provided by Roman Tarakanov <[email protected]> today
% bug fix verified with PALE data (old code produces garbage results)
% Mar 6, 2016: - BUG: p.fix_compass = 1 did not work for single-head profiles
disp('FIXCOMPASS adjust compass')
% use tilt sensors to figure out allingment between instruments
if p.fix_compass > 1
diary off
hoff2=fminsearch('checktilt',0,[],[d.rol(2,:);d.pit(2,:);d.rol(1,:);d.pit(1,:)]);
diary on
disp([' mean instrument alignment based on tilt is ',num2str(hoff2),' deg'])
end
[lb,lt]=size(d.ru);
if p.fix_compass==2
p=setdefv(p,'hdg_offset',[0 hoff2]);
elseif p.fix_compass==3
p=setdefv(p,'hdg_offset',[-hoff2 0]);
else
p=setdefv(p,'hdg_offset',[0 0]);
end
p=setdefv(p,'hdg_offset_used',[0 0]);
rot=p.hdg_offset;
rotv(1,:)=ones(1,lt)*rot(1);
disp([' offset down looking compass by : ',num2str(rot(1))])
p.hdg_offset_used(1)=p.hdg_offset_used(1)+rot(1);
if p.fix_compass==3
disp(' use up looking compass on down looking ADCP')
rotv(1,:)=d.hdg(2,:)-d.hdg(1,:)+rot(1);
p.rotup2down=0;
end
if length(d.izu)>0
rotv(2,:)=ones(1,lt)*rot(2);
disp([' offset up looking compass by : ',num2str(rot(2))])
p.hdg_offset_used(2)=p.hdg_offset_used(2)+rot(2);
if p.fix_compass==2
disp(' use down looking compass on up looking ADCP')
rotv(2,:)=d.hdg(1,:)-d.hdg(2,:)+rot(2);
p.rotup2down=0;
end
end
rotm=meshgrid(rotv(1,:),d.izd);
[d.ru(d.izd,:),d.rv(d.izd,:)]=uvrot(d.ru(d.izd,:),d.rv(d.izd,:),rotm);
if length(rotv(1,:))==size(d.bvel,1)
[d.bvel(:,1),d.bvel(:,2)]=uvrot(d.bvel(:,1),d.bvel(:,2),rotv(1,:)');
else
[d.bvel(1,:),d.bvel(2,:)]=uvrot(d.bvel(1,:),d.bvel(2,:),rotv(1,:));
end
if length(d.izu)>0
rotm=meshgrid(rotv(2,:),d.izu);
[d.ru(d.izu,:),d.rv(d.izu,:)]=uvrot(d.ru(d.izu,:),d.rv(d.izu,:),rotm);
end