0
|
1 |
#======================================================================
|
|
2 |
# R D I _ C O O R D S . P L
|
|
3 |
# doc: Sun Jan 19 17:57:53 2003
|
8
|
4 |
# dlm: Sun Jan 15 20:04:13 2012
|
0
|
5 |
# (c) 2003 A.M. Thurnherr
|
8
|
6 |
# uE-Info: 33 74 NIL 0 0 72 0 2 4 NIL ofnI
|
0
|
7 |
#======================================================================
|
|
8 |
|
|
9 |
# RDI Workhorse Coordinate Transformations
|
|
10 |
|
|
11 |
# HISTORY:
|
|
12 |
# Jan 19, 2003: - written
|
|
13 |
# Jan 21, 2003: - made it obey HEADING_BIAS (magnetic declination)
|
|
14 |
# Jan 22, 3003: - corrected magnetic declination
|
|
15 |
# Feb 16, 2003: - use pitch correction from RDI manual
|
|
16 |
# Oct 11, 2003: - BUG: return value of atan() had been interpreted
|
|
17 |
# as degrees instead of radians
|
|
18 |
# Feb 27, 2004: - added velApplyHdgBias()
|
|
19 |
# - changed non-zero HEADING_ALIGNMENT from error to warning
|
|
20 |
# Sep 16, 2005: - added deg() for [mkprofile]
|
|
21 |
# Aug 26, 2006: - BUG: incorrect transformation for uplookers
|
|
22 |
# Nov 30, 2007: - optimized &velInstrumentToEarth(), velBeamToInstrument()
|
|
23 |
# - added support for 3-beam solutions
|
|
24 |
# Feb 12, 2008: - added threeBeamFlag
|
|
25 |
# Mar 18, 2009: - added &gimbal_pitch(), &angle_from_vertical()
|
|
26 |
# May 19, 2009: - added &velBeamToVertical()
|
|
27 |
# May 23, 2009: - debugged & renamed to &velBeamToBPEarth
|
|
28 |
# May 23, 2010: - changed prototypes of rad() & deg() to conform to ANTS
|
5
|
29 |
# Dec 20, 2010: - cosmetics
|
6
|
30 |
# Dec 23, 2010: - added &velBeamToBPInstrument
|
|
31 |
# Jan 22, 2011: - made velApplyHdgBias calculate sin/cos every time to allow
|
|
32 |
# per-ensemble corrections
|
8
|
33 |
# Jan 15, 2012: - replaced defined(@...) by (@...) to get rid of warning
|
0
|
34 |
|
|
35 |
use strict;
|
|
36 |
use POSIX;
|
|
37 |
|
|
38 |
my($PI) = 3.14159265358979;
|
|
39 |
|
|
40 |
sub rad(@) { return $_[0]/180 * $PI; }
|
|
41 |
sub deg(@) { return $_[0]/$PI * 180; }
|
|
42 |
|
|
43 |
$RDI_Coords::minValidVels = 3; # 3-beam solutions ok
|
|
44 |
|
|
45 |
$RDI_Coords::threeBeam_1 = 0; # stats
|
|
46 |
$RDI_Coords::threeBeam_2 = 0;
|
|
47 |
$RDI_Coords::threeBeam_3 = 0;
|
|
48 |
$RDI_Coords::threeBeam_4 = 0;
|
|
49 |
$RDI_Coords::fourBeam = 0;
|
|
50 |
|
|
51 |
$RDI_Coords::threeBeamFlag = 0; # flag last transformation
|
|
52 |
|
|
53 |
{ # STATIC SCOPE
|
|
54 |
my(@B2I);
|
|
55 |
|
|
56 |
sub velBeamToInstrument(@)
|
|
57 |
{
|
|
58 |
my($dta,$v1,$v2,$v3,$v4) = @_;
|
|
59 |
return undef unless (defined($v1) + defined($v2) +
|
|
60 |
defined($v3) + defined($v4)
|
|
61 |
>= $RDI_Coords::minValidVels);
|
|
62 |
|
8
|
63 |
unless (@B2I) {
|
0
|
64 |
# print(STDERR "RDI_Coords::minValidVels = $RDI_Coords::minValidVels\n");
|
|
65 |
my($a) = 1 / (2 * sin(rad($dta->{BEAM_ANGLE})));
|
|
66 |
my($b) = 1 / (4 * cos(rad($dta->{BEAM_ANGLE})));
|
|
67 |
my($c) = $dta->{CONVEX_BEAM_PATTERN} ? 1 : -1;
|
|
68 |
my($d) = $a / sqrt(2);
|
|
69 |
@B2I = ([$c*$a, -$c*$a, 0, 0 ],
|
|
70 |
[0, 0, -$c*$a, $c*$a],
|
|
71 |
[$b, $b, $b, $b ],
|
|
72 |
[$d, $d, -$d, -$d ]);
|
|
73 |
# print(STDERR "@{$B2I[0]}\n@{$B2I[1]}\n@{$B2I[2]}\n@{$B2I[3]}\n");
|
|
74 |
}
|
|
75 |
|
|
76 |
if (!defined($v1)) { # 3-beam solutions
|
|
77 |
$RDI_Coords::threeBeamFlag = 1;
|
|
78 |
$RDI_Coords::threeBeam_1++;
|
|
79 |
$v1 = -($v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][0];
|
|
80 |
} elsif (!defined($v2)) {
|
|
81 |
$RDI_Coords::threeBeamFlag = 1;
|
|
82 |
$RDI_Coords::threeBeam_2++;
|
|
83 |
$v2 = -($v1*$B2I[3][0]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][1];
|
|
84 |
} elsif (!defined($v3)) {
|
|
85 |
$RDI_Coords::threeBeamFlag = 1;
|
|
86 |
$RDI_Coords::threeBeam_3++;
|
|
87 |
$v3 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v4*$B2I[3][3])/$B2I[3][2];
|
|
88 |
} elsif (!defined($v4)) {
|
|
89 |
$RDI_Coords::threeBeamFlag = 1;
|
|
90 |
$RDI_Coords::threeBeam_4++;
|
|
91 |
$v4 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2])/$B2I[3][3];
|
|
92 |
} else {
|
|
93 |
$RDI_Coords::threeBeamFlag = 0;
|
|
94 |
$RDI_Coords::fourBeam++;
|
|
95 |
}
|
|
96 |
|
|
97 |
return ($v1*$B2I[0][0]+$v2*$B2I[0][1],
|
|
98 |
$v3*$B2I[1][2]+$v4*$B2I[1][3],
|
|
99 |
$v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3],
|
|
100 |
$v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]);
|
|
101 |
}
|
|
102 |
} # STATIC SCOPE
|
|
103 |
|
|
104 |
{ # STATIC SCOPE
|
|
105 |
my($hdg,$pitch,$roll,@I2E);
|
|
106 |
|
|
107 |
sub velInstrumentToEarth(@)
|
|
108 |
{
|
|
109 |
my($dta,$ens,$v1,$v2,$v3,$v4) = @_;
|
|
110 |
return undef unless (defined($v1) && defined($v2) &&
|
|
111 |
defined($v3) && defined($v4));
|
|
112 |
|
|
113 |
unless (@I2E &&
|
|
114 |
$hdg == $dta->{ENSEMBLE}[$ens]->{HEADING}
|
|
115 |
- $dta->{HEADING_BIAS} &&
|
|
116 |
$pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} &&
|
|
117 |
$roll == $dta->{ENSEMBLE}[$ens]->{ROLL}) {
|
|
118 |
printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n",
|
|
119 |
$dta->{HEADING_ALIGNMENT})
|
|
120 |
if ($dta->{HEADING_ALIGNMENT});
|
|
121 |
$hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS};
|
|
122 |
$pitch = $dta->{ENSEMBLE}[$ens]->{PITCH};
|
|
123 |
$roll = $dta->{ENSEMBLE}[$ens]->{ROLL};
|
|
124 |
my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));
|
|
125 |
my($sh,$ch) = (sin(rad($hdg)), cos(rad($hdg)));
|
|
126 |
my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch));
|
|
127 |
my($sr,$cr) = (sin(rad($roll)), cos(rad($roll)));
|
|
128 |
@I2E = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}
|
|
129 |
? (
|
|
130 |
[-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr],
|
|
131 |
[-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr],
|
|
132 |
[+$cp*$sr, $sp, -$cp*$cr, ],
|
|
133 |
) : (
|
|
134 |
[$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr],
|
|
135 |
[$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr],
|
|
136 |
[-$cp*$sr, $sp, $cp*$cr, ],
|
|
137 |
);
|
|
138 |
}
|
|
139 |
return ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2],
|
|
140 |
$v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2],
|
|
141 |
$v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],
|
|
142 |
$v4);
|
|
143 |
|
|
144 |
}
|
|
145 |
} # STATIC SCOPE
|
|
146 |
|
|
147 |
#======================================================================
|
5
|
148 |
# velBeamToBPEarth(@) calculates the vertical- and horizontal vels
|
0
|
149 |
# from the two beam pairs separately. Note that (w1+w2)/2 is
|
|
150 |
# identical to the w estimated according to RDI without 3-beam
|
|
151 |
# solutions.
|
|
152 |
#======================================================================
|
|
153 |
|
|
154 |
{ # STATIC SCOPE
|
|
155 |
my($TwoCosBAngle,$TwoSinBAngle);
|
|
156 |
|
|
157 |
sub velBeamToBPEarth(@)
|
|
158 |
{
|
|
159 |
my($dta,$ens,$b1,$b2,$b3,$b4) = @_;
|
|
160 |
my($v12,$w12,$v34,$w34);
|
|
161 |
|
|
162 |
return (undef,undef,undef,undef)
|
|
163 |
unless defined($b1) && defined($b2) && defined($b3) && defined($b4);
|
|
164 |
|
|
165 |
unless (defined($TwoCosBAngle)) {
|
|
166 |
$TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE}));
|
|
167 |
$TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE}));
|
|
168 |
}
|
|
169 |
my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL});
|
|
170 |
my($sr) = sin($roll); my($cr) = cos($roll);
|
|
171 |
my($pitch) = atan(tan(rad($dta->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch
|
|
172 |
my($sp) = sin($pitch); my($cp) = cos($pitch);
|
|
173 |
|
|
174 |
# Sign convention:
|
|
175 |
# - refer to Coord manual Fig. 3
|
|
176 |
# - v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP
|
|
177 |
# with beam 3 pointing north (heading = 0)
|
|
178 |
# - w is +ve upward, regardless of instrument orientation
|
|
179 |
|
|
180 |
my($v12_ic) = ($b1-$b2)/$TwoSinBAngle; # instrument coords with w vertical up
|
|
181 |
my($w12_ic) = ($b1+$b2)/$TwoCosBAngle;
|
|
182 |
$w12_ic *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
|
|
183 |
my($v34_ic) = ($b3-$b4)/$TwoSinBAngle;
|
|
184 |
my($w34_ic) = ($b3+$b4)/$TwoCosBAngle;
|
|
185 |
$w34_ic *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
|
|
186 |
|
|
187 |
if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) { # beampair Earth coords
|
|
188 |
$w12 = $w12_ic*$cr + $v12_ic*$sr - $v34_ic*$sp;
|
|
189 |
$v12 = $v12_ic*$cr - $w12_ic*$sr + $w34_ic*$sp;
|
|
190 |
$w34 = $w34_ic*$cp - $v34_ic*$sp + $v12_ic*$sr;
|
|
191 |
$v34 = $v34_ic*$cp + $w34_ic*$sp - $w12_ic*$sr;
|
|
192 |
} else {
|
|
193 |
$w12 = $w12_ic*$cr - $v12_ic*$sr - $v34_ic*$sp;
|
|
194 |
$v12 = $v12_ic*$cr + $w12_ic*$sr + $w34_ic*$sp;
|
|
195 |
$w34 = $w34_ic*$cp - $v34_ic*$sp - $v12_ic*$sr;
|
|
196 |
$v34 = $v34_ic*$cp + $w34_ic*$sp + $w12_ic*$sr;
|
|
197 |
}
|
|
198 |
|
|
199 |
return ($v12,$w12,$v34,$w34);
|
|
200 |
}
|
|
201 |
}
|
|
202 |
|
5
|
203 |
#===================================================================
|
|
204 |
# velBeamToBPInstrument(@) calculates the instrument-coordinate vels
|
|
205 |
# from the two beam pairs separately.
|
|
206 |
#===================================================================
|
|
207 |
|
|
208 |
{ # STATIC SCOPE
|
|
209 |
my($TwoCosBAngle,$TwoSinBAngle);
|
|
210 |
|
|
211 |
sub velBeamToBPInstrument(@)
|
|
212 |
{
|
|
213 |
my($dta,$ens,$b1,$b2,$b3,$b4) = @_;
|
|
214 |
my($v12,$w12,$v34,$w34);
|
|
215 |
|
|
216 |
unless (defined($TwoCosBAngle)) {
|
|
217 |
$TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE}));
|
|
218 |
$TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE}));
|
|
219 |
}
|
|
220 |
my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL});
|
|
221 |
my($sr) = sin($roll); my($cr) = cos($roll);
|
|
222 |
my($pitch) = atan(tan(rad($dta->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch
|
|
223 |
my($sp) = sin($pitch); my($cp) = cos($pitch);
|
|
224 |
|
|
225 |
# Sign convention:
|
|
226 |
# - refer to Coord manual Fig. 3
|
|
227 |
# - v12 is horizontal velocity from beam1 to beam2
|
|
228 |
# - w is +ve upward, regardless of instrument orientation
|
|
229 |
|
|
230 |
if (defined($b1) && defined($b2)) {
|
|
231 |
$v12 = ($b1-$b2)/$TwoSinBAngle;
|
|
232 |
$w12 = ($b1+$b2)/$TwoCosBAngle;
|
|
233 |
$w12 *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
|
|
234 |
}
|
|
235 |
if (defined($b3) && defined($b4)) {
|
|
236 |
$v34 = ($b3-$b4)/$TwoSinBAngle;
|
|
237 |
$w34 = ($b3+$b4)/$TwoCosBAngle;
|
|
238 |
$w34 *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
|
|
239 |
}
|
|
240 |
|
|
241 |
return ($v12,$w12,$v34,$w34);
|
|
242 |
}
|
|
243 |
}
|
|
244 |
|
0
|
245 |
#======================================================================
|
|
246 |
# velApplyHdgBias() applies the heading bias, which is used to correct
|
|
247 |
# for magnetic declination for data recorded in Earth-coordinates ONLY.
|
|
248 |
# Bias correction for beam-coordinate data is done in velInstrumentToEarth()
|
|
249 |
#======================================================================
|
|
250 |
|
6
|
251 |
sub velApplyHdgBias(@)
|
|
252 |
{
|
|
253 |
my($dta,$ens,$v1,$v2,$v3,$v4) = @_;
|
|
254 |
return undef unless (defined($v1) && defined($v2));
|
0
|
255 |
|
6
|
256 |
my($sh) = sin(rad(-$dta->{HEADING_BIAS}));
|
|
257 |
my($ch) = cos(rad(-$dta->{HEADING_BIAS}));
|
0
|
258 |
|
6
|
259 |
return ( $v1*$ch + $v2*$sh,
|
|
260 |
-$v1*$sh + $v2*$ch,
|
|
261 |
$v3 ,
|
|
262 |
$v4 );
|
|
263 |
}
|
0
|
264 |
|
|
265 |
#----------------------------------------------------------------------
|
|
266 |
# Pitch/Roll Functions
|
|
267 |
#----------------------------------------------------------------------
|
|
268 |
|
|
269 |
sub gimbal_pitch($$) # RDI coord trans manual
|
|
270 |
{
|
5
|
271 |
my($RDI_pitch,$RDI_roll) = @_;
|
|
272 |
return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))));
|
0
|
273 |
}
|
|
274 |
|
|
275 |
# - angle from vertical is home grown and should be treated with caution
|
|
276 |
# - angle between two unit vectors given by acos(v1 dot v2)
|
|
277 |
# - vertical unit vector v1 = (0 0 1) => dot product = z-component of v2
|
|
278 |
# - when vertical unit vector is pitched in x direction, followed by
|
|
279 |
# roll in y direction:
|
|
280 |
# x = sin(pitch)
|
|
281 |
# y = cos(pitch) * sin(roll)
|
|
282 |
# z = cos(pitch) * cos(roll)
|
|
283 |
# has been checked with sqrt(x^2+y^2+z^2) == 1
|
|
284 |
# - for small angles, this is very similar to sqrt(pitch^2+roll^2)
|
|
285 |
|
|
286 |
sub angle_from_vertical($$)
|
|
287 |
{
|
5
|
288 |
my($RDI_pitch,$RDI_roll) = @_;
|
|
289 |
my($rad_pitch) = atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll)));
|
|
290 |
return deg(acos(cos($rad_pitch) * cos(rad($RDI_roll))));
|
0
|
291 |
}
|
|
292 |
|
|
293 |
1;
|