author | A.M. Thurnherr <athurnherr@yahoo.com> |
Sat, 22 Feb 2014 10:56:01 +0000 | |
changeset 14 | 8c79b38a7086 |
parent 13 | b176da8559b3 |
child 16 | 68a9fc5e7d45 |
permissions | -rw-r--r-- |
0 | 1 |
#====================================================================== |
2 |
# R D I _ C O O R D S . P L |
|
3 |
# doc: Sun Jan 19 17:57:53 2003 |
|
14 | 4 |
# dlm: Wed Nov 27 11:21:49 2013 |
0 | 5 |
# (c) 2003 A.M. Thurnherr |
14 | 6 |
# uE-Info: 287 70 NIL 0 0 72 2 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 |
13
b176da8559b3
before implementing WBWens (PD0 writing)
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
8
diff
changeset
|
34 |
# Aug 7, 2013: - BUG: &velBeamToBPInstrument did not return any val unless |
b176da8559b3
before implementing WBWens (PD0 writing)
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
8
diff
changeset
|
35 |
# all beam velocities are defined |
14 | 36 |
# Nov 27, 2013: - added &RDI_pitch(), &tilt_azimuth() |
0 | 37 |
|
38 |
use strict; |
|
39 |
use POSIX; |
|
40 |
||
41 |
my($PI) = 3.14159265358979; |
|
42 |
||
43 |
sub rad(@) { return $_[0]/180 * $PI; } |
|
44 |
sub deg(@) { return $_[0]/$PI * 180; } |
|
45 |
||
46 |
$RDI_Coords::minValidVels = 3; # 3-beam solutions ok |
|
47 |
||
48 |
$RDI_Coords::threeBeam_1 = 0; # stats |
|
49 |
$RDI_Coords::threeBeam_2 = 0; |
|
50 |
$RDI_Coords::threeBeam_3 = 0; |
|
51 |
$RDI_Coords::threeBeam_4 = 0; |
|
52 |
$RDI_Coords::fourBeam = 0; |
|
53 |
||
54 |
$RDI_Coords::threeBeamFlag = 0; # flag last transformation |
|
55 |
||
56 |
{ # STATIC SCOPE |
|
57 |
my(@B2I); |
|
58 |
||
59 |
sub velBeamToInstrument(@) |
|
60 |
{ |
|
61 |
my($dta,$v1,$v2,$v3,$v4) = @_; |
|
62 |
return undef unless (defined($v1) + defined($v2) + |
|
63 |
defined($v3) + defined($v4) |
|
64 |
>= $RDI_Coords::minValidVels); |
|
65 |
||
8 | 66 |
unless (@B2I) { |
0 | 67 |
# print(STDERR "RDI_Coords::minValidVels = $RDI_Coords::minValidVels\n"); |
68 |
my($a) = 1 / (2 * sin(rad($dta->{BEAM_ANGLE}))); |
|
69 |
my($b) = 1 / (4 * cos(rad($dta->{BEAM_ANGLE}))); |
|
70 |
my($c) = $dta->{CONVEX_BEAM_PATTERN} ? 1 : -1; |
|
71 |
my($d) = $a / sqrt(2); |
|
72 |
@B2I = ([$c*$a, -$c*$a, 0, 0 ], |
|
73 |
[0, 0, -$c*$a, $c*$a], |
|
74 |
[$b, $b, $b, $b ], |
|
75 |
[$d, $d, -$d, -$d ]); |
|
76 |
# print(STDERR "@{$B2I[0]}\n@{$B2I[1]}\n@{$B2I[2]}\n@{$B2I[3]}\n"); |
|
77 |
} |
|
78 |
||
79 |
if (!defined($v1)) { # 3-beam solutions |
|
80 |
$RDI_Coords::threeBeamFlag = 1; |
|
81 |
$RDI_Coords::threeBeam_1++; |
|
82 |
$v1 = -($v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][0]; |
|
83 |
} elsif (!defined($v2)) { |
|
84 |
$RDI_Coords::threeBeamFlag = 1; |
|
85 |
$RDI_Coords::threeBeam_2++; |
|
86 |
$v2 = -($v1*$B2I[3][0]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][1]; |
|
87 |
} elsif (!defined($v3)) { |
|
88 |
$RDI_Coords::threeBeamFlag = 1; |
|
89 |
$RDI_Coords::threeBeam_3++; |
|
90 |
$v3 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v4*$B2I[3][3])/$B2I[3][2]; |
|
91 |
} elsif (!defined($v4)) { |
|
92 |
$RDI_Coords::threeBeamFlag = 1; |
|
93 |
$RDI_Coords::threeBeam_4++; |
|
94 |
$v4 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2])/$B2I[3][3]; |
|
95 |
} else { |
|
96 |
$RDI_Coords::threeBeamFlag = 0; |
|
97 |
$RDI_Coords::fourBeam++; |
|
98 |
} |
|
99 |
||
100 |
return ($v1*$B2I[0][0]+$v2*$B2I[0][1], |
|
101 |
$v3*$B2I[1][2]+$v4*$B2I[1][3], |
|
102 |
$v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
|
103 |
$v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
|
104 |
} |
|
105 |
} # STATIC SCOPE |
|
106 |
||
107 |
{ # STATIC SCOPE |
|
108 |
my($hdg,$pitch,$roll,@I2E); |
|
109 |
||
110 |
sub velInstrumentToEarth(@) |
|
111 |
{ |
|
112 |
my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
|
113 |
return undef unless (defined($v1) && defined($v2) && |
|
114 |
defined($v3) && defined($v4)); |
|
115 |
||
116 |
unless (@I2E && |
|
117 |
$hdg == $dta->{ENSEMBLE}[$ens]->{HEADING} |
|
118 |
- $dta->{HEADING_BIAS} && |
|
119 |
$pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
|
120 |
$roll == $dta->{ENSEMBLE}[$ens]->{ROLL}) { |
|
121 |
printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n", |
|
122 |
$dta->{HEADING_ALIGNMENT}) |
|
123 |
if ($dta->{HEADING_ALIGNMENT}); |
|
124 |
$hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS}; |
|
125 |
$pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
|
126 |
$roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
|
127 |
my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
|
128 |
my($sh,$ch) = (sin(rad($hdg)), cos(rad($hdg))); |
|
129 |
my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
|
130 |
my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
|
131 |
@I2E = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
|
132 |
? ( |
|
133 |
[-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
|
134 |
[-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr], |
|
135 |
[+$cp*$sr, $sp, -$cp*$cr, ], |
|
136 |
) : ( |
|
137 |
[$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr], |
|
138 |
[$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr], |
|
139 |
[-$cp*$sr, $sp, $cp*$cr, ], |
|
140 |
); |
|
141 |
} |
|
142 |
return ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2], |
|
143 |
$v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2], |
|
144 |
$v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2], |
|
145 |
$v4); |
|
146 |
||
147 |
} |
|
148 |
} # STATIC SCOPE |
|
149 |
||
150 |
#====================================================================== |
|
5 | 151 |
# velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
0 | 152 |
# from the two beam pairs separately. Note that (w1+w2)/2 is |
153 |
# identical to the w estimated according to RDI without 3-beam |
|
154 |
# solutions. |
|
155 |
#====================================================================== |
|
156 |
||
157 |
{ # STATIC SCOPE |
|
158 |
my($TwoCosBAngle,$TwoSinBAngle); |
|
159 |
||
160 |
sub velBeamToBPEarth(@) |
|
161 |
{ |
|
162 |
my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
|
163 |
my($v12,$w12,$v34,$w34); |
|
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 |
||
13
b176da8559b3
before implementing WBWens (PD0 writing)
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
8
diff
changeset
|
199 |
$v12=$w12=undef unless (defined($b1) && defined($b2)); |
b176da8559b3
before implementing WBWens (PD0 writing)
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
8
diff
changeset
|
200 |
$v34=$w34=undef unless (defined($b3) && defined($b4)); |
b176da8559b3
before implementing WBWens (PD0 writing)
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
8
diff
changeset
|
201 |
|
0 | 202 |
return ($v12,$w12,$v34,$w34); |
203 |
} |
|
204 |
} |
|
205 |
||
5 | 206 |
#=================================================================== |
207 |
# velBeamToBPInstrument(@) calculates the instrument-coordinate vels |
|
208 |
# from the two beam pairs separately. |
|
209 |
#=================================================================== |
|
210 |
||
211 |
{ # STATIC SCOPE |
|
212 |
my($TwoCosBAngle,$TwoSinBAngle); |
|
213 |
||
214 |
sub velBeamToBPInstrument(@) |
|
215 |
{ |
|
216 |
my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
|
217 |
my($v12,$w12,$v34,$w34); |
|
218 |
||
219 |
unless (defined($TwoCosBAngle)) { |
|
220 |
$TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
|
221 |
$TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
|
222 |
} |
|
223 |
my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
|
224 |
my($sr) = sin($roll); my($cr) = cos($roll); |
|
225 |
my($pitch) = atan(tan(rad($dta->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch |
|
226 |
my($sp) = sin($pitch); my($cp) = cos($pitch); |
|
227 |
||
228 |
# Sign convention: |
|
229 |
# - refer to Coord manual Fig. 3 |
|
230 |
# - v12 is horizontal velocity from beam1 to beam2 |
|
231 |
# - w is +ve upward, regardless of instrument orientation |
|
232 |
||
233 |
if (defined($b1) && defined($b2)) { |
|
234 |
$v12 = ($b1-$b2)/$TwoSinBAngle; |
|
235 |
$w12 = ($b1+$b2)/$TwoCosBAngle; |
|
236 |
$w12 *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
|
237 |
} |
|
238 |
if (defined($b3) && defined($b4)) { |
|
239 |
$v34 = ($b3-$b4)/$TwoSinBAngle; |
|
240 |
$w34 = ($b3+$b4)/$TwoCosBAngle; |
|
241 |
$w34 *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
|
242 |
} |
|
243 |
||
244 |
return ($v12,$w12,$v34,$w34); |
|
245 |
} |
|
246 |
} |
|
247 |
||
0 | 248 |
#====================================================================== |
249 |
# velApplyHdgBias() applies the heading bias, which is used to correct |
|
250 |
# for magnetic declination for data recorded in Earth-coordinates ONLY. |
|
251 |
# Bias correction for beam-coordinate data is done in velInstrumentToEarth() |
|
252 |
#====================================================================== |
|
253 |
||
6 | 254 |
sub velApplyHdgBias(@) |
255 |
{ |
|
256 |
my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
|
257 |
return undef unless (defined($v1) && defined($v2)); |
|
0 | 258 |
|
6 | 259 |
my($sh) = sin(rad(-$dta->{HEADING_BIAS})); |
260 |
my($ch) = cos(rad(-$dta->{HEADING_BIAS})); |
|
0 | 261 |
|
6 | 262 |
return ( $v1*$ch + $v2*$sh, |
263 |
-$v1*$sh + $v2*$ch, |
|
264 |
$v3 , |
|
265 |
$v4 ); |
|
266 |
} |
|
0 | 267 |
|
268 |
#---------------------------------------------------------------------- |
|
269 |
# Pitch/Roll Functions |
|
270 |
#---------------------------------------------------------------------- |
|
271 |
||
272 |
sub gimbal_pitch($$) # RDI coord trans manual |
|
273 |
{ |
|
5 | 274 |
my($RDI_pitch,$RDI_roll) = @_; |
275 |
return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll)))); |
|
0 | 276 |
} |
277 |
||
14 | 278 |
sub RDI_pitch($$) |
279 |
{ |
|
280 |
my($gimbal_pitch,$roll) = @_; |
|
281 |
return deg(atan(tan(rad($gimbal_pitch))/cos(rad($roll)))); |
|
282 |
} |
|
283 |
||
284 |
sub tilt_azimuth($$) |
|
285 |
{ |
|
286 |
my($gimbal_pitch,$roll) = @_; |
|
287 |
return angle(deg(atan2(sin(rad($gimbal_pitch)),sin(rad($roll))))); |
|
288 |
} |
|
289 |
||
0 | 290 |
# - angle from vertical is home grown and should be treated with caution |
291 |
# - angle between two unit vectors given by acos(v1 dot v2) |
|
292 |
# - vertical unit vector v1 = (0 0 1) => dot product = z-component of v2 |
|
293 |
# - when vertical unit vector is pitched in x direction, followed by |
|
294 |
# roll in y direction: |
|
295 |
# x = sin(pitch) |
|
296 |
# y = cos(pitch) * sin(roll) |
|
297 |
# z = cos(pitch) * cos(roll) |
|
298 |
# has been checked with sqrt(x^2+y^2+z^2) == 1 |
|
299 |
# - for small angles, this is very similar to sqrt(pitch^2+roll^2) |
|
300 |
||
301 |
sub angle_from_vertical($$) |
|
302 |
{ |
|
5 | 303 |
my($RDI_pitch,$RDI_roll) = @_; |
304 |
my($rad_pitch) = atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))); |
|
305 |
return deg(acos(cos($rad_pitch) * cos(rad($RDI_roll)))); |
|
0 | 306 |
} |
307 |
||
308 |
1; |