|
1 #====================================================================== |
|
2 # R D I _ C O O R D S . P L |
|
3 # doc: Sun Jan 19 17:57:53 2003 |
|
4 # dlm: Sun May 23 22:47:32 2010 |
|
5 # (c) 2003 A.M. Thurnherr |
|
6 # uE-Info: 28 74 NIL 0 0 72 2 2 4 NIL ofnI |
|
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 |
|
29 |
|
30 use strict; |
|
31 use POSIX; |
|
32 |
|
33 my($PI) = 3.14159265358979; |
|
34 |
|
35 sub rad(@) { return $_[0]/180 * $PI; } |
|
36 sub deg(@) { return $_[0]/$PI * 180; } |
|
37 |
|
38 $RDI_Coords::minValidVels = 3; # 3-beam solutions ok |
|
39 |
|
40 $RDI_Coords::threeBeam_1 = 0; # stats |
|
41 $RDI_Coords::threeBeam_2 = 0; |
|
42 $RDI_Coords::threeBeam_3 = 0; |
|
43 $RDI_Coords::threeBeam_4 = 0; |
|
44 $RDI_Coords::fourBeam = 0; |
|
45 |
|
46 $RDI_Coords::threeBeamFlag = 0; # flag last transformation |
|
47 |
|
48 { # STATIC SCOPE |
|
49 my(@B2I); |
|
50 |
|
51 sub velBeamToInstrument(@) |
|
52 { |
|
53 my($dta,$v1,$v2,$v3,$v4) = @_; |
|
54 return undef unless (defined($v1) + defined($v2) + |
|
55 defined($v3) + defined($v4) |
|
56 >= $RDI_Coords::minValidVels); |
|
57 |
|
58 unless (defined(@B2I)) { |
|
59 # print(STDERR "RDI_Coords::minValidVels = $RDI_Coords::minValidVels\n"); |
|
60 my($a) = 1 / (2 * sin(rad($dta->{BEAM_ANGLE}))); |
|
61 my($b) = 1 / (4 * cos(rad($dta->{BEAM_ANGLE}))); |
|
62 my($c) = $dta->{CONVEX_BEAM_PATTERN} ? 1 : -1; |
|
63 my($d) = $a / sqrt(2); |
|
64 @B2I = ([$c*$a, -$c*$a, 0, 0 ], |
|
65 [0, 0, -$c*$a, $c*$a], |
|
66 [$b, $b, $b, $b ], |
|
67 [$d, $d, -$d, -$d ]); |
|
68 # print(STDERR "@{$B2I[0]}\n@{$B2I[1]}\n@{$B2I[2]}\n@{$B2I[3]}\n"); |
|
69 } |
|
70 |
|
71 if (!defined($v1)) { # 3-beam solutions |
|
72 $RDI_Coords::threeBeamFlag = 1; |
|
73 $RDI_Coords::threeBeam_1++; |
|
74 $v1 = -($v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][0]; |
|
75 } elsif (!defined($v2)) { |
|
76 $RDI_Coords::threeBeamFlag = 1; |
|
77 $RDI_Coords::threeBeam_2++; |
|
78 $v2 = -($v1*$B2I[3][0]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][1]; |
|
79 } elsif (!defined($v3)) { |
|
80 $RDI_Coords::threeBeamFlag = 1; |
|
81 $RDI_Coords::threeBeam_3++; |
|
82 $v3 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v4*$B2I[3][3])/$B2I[3][2]; |
|
83 } elsif (!defined($v4)) { |
|
84 $RDI_Coords::threeBeamFlag = 1; |
|
85 $RDI_Coords::threeBeam_4++; |
|
86 $v4 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2])/$B2I[3][3]; |
|
87 } else { |
|
88 $RDI_Coords::threeBeamFlag = 0; |
|
89 $RDI_Coords::fourBeam++; |
|
90 } |
|
91 |
|
92 return ($v1*$B2I[0][0]+$v2*$B2I[0][1], |
|
93 $v3*$B2I[1][2]+$v4*$B2I[1][3], |
|
94 $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
|
95 $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
|
96 } |
|
97 } # STATIC SCOPE |
|
98 |
|
99 { # STATIC SCOPE |
|
100 my($hdg,$pitch,$roll,@I2E); |
|
101 |
|
102 sub velInstrumentToEarth(@) |
|
103 { |
|
104 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
|
105 return undef unless (defined($v1) && defined($v2) && |
|
106 defined($v3) && defined($v4)); |
|
107 |
|
108 unless (@I2E && |
|
109 $hdg == $dta->{ENSEMBLE}[$ens]->{HEADING} |
|
110 - $dta->{HEADING_BIAS} && |
|
111 $pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
|
112 $roll == $dta->{ENSEMBLE}[$ens]->{ROLL}) { |
|
113 printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n", |
|
114 $dta->{HEADING_ALIGNMENT}) |
|
115 if ($dta->{HEADING_ALIGNMENT}); |
|
116 $hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS}; |
|
117 $pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
|
118 $roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
|
119 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
|
120 my($sh,$ch) = (sin(rad($hdg)), cos(rad($hdg))); |
|
121 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
|
122 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
|
123 @I2E = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
|
124 ? ( |
|
125 [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
|
126 [-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr], |
|
127 [+$cp*$sr, $sp, -$cp*$cr, ], |
|
128 ) : ( |
|
129 [$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr], |
|
130 [$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr], |
|
131 [-$cp*$sr, $sp, $cp*$cr, ], |
|
132 ); |
|
133 } |
|
134 return ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2], |
|
135 $v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2], |
|
136 $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2], |
|
137 $v4); |
|
138 |
|
139 } |
|
140 } # STATIC SCOPE |
|
141 |
|
142 #====================================================================== |
|
143 # velBeamToBPEarth3(@) calculates the vertical- and horizontal vels |
|
144 # from the two beam pairs separately. Note that (w1+w2)/2 is |
|
145 # identical to the w estimated according to RDI without 3-beam |
|
146 # solutions. |
|
147 #====================================================================== |
|
148 |
|
149 { # STATIC SCOPE |
|
150 my($TwoCosBAngle,$TwoSinBAngle); |
|
151 |
|
152 sub velBeamToBPEarth(@) |
|
153 { |
|
154 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
|
155 my($v12,$w12,$v34,$w34); |
|
156 |
|
157 return (undef,undef,undef,undef) |
|
158 unless defined($b1) && defined($b2) && defined($b3) && defined($b4); |
|
159 |
|
160 unless (defined($TwoCosBAngle)) { |
|
161 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
|
162 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
|
163 } |
|
164 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
|
165 my($sr) = sin($roll); my($cr) = cos($roll); |
|
166 my($pitch) = atan(tan(rad($dta->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch |
|
167 my($sp) = sin($pitch); my($cp) = cos($pitch); |
|
168 |
|
169 # Sign convention: |
|
170 # - refer to Coord manual Fig. 3 |
|
171 # - v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP |
|
172 # with beam 3 pointing north (heading = 0) |
|
173 # - w is +ve upward, regardless of instrument orientation |
|
174 |
|
175 my($v12_ic) = ($b1-$b2)/$TwoSinBAngle; # instrument coords with w vertical up |
|
176 my($w12_ic) = ($b1+$b2)/$TwoCosBAngle; |
|
177 $w12_ic *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
|
178 my($v34_ic) = ($b3-$b4)/$TwoSinBAngle; |
|
179 my($w34_ic) = ($b3+$b4)/$TwoCosBAngle; |
|
180 $w34_ic *= -1 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}); |
|
181 |
|
182 if ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) { # beampair Earth coords |
|
183 $w12 = $w12_ic*$cr + $v12_ic*$sr - $v34_ic*$sp; |
|
184 $v12 = $v12_ic*$cr - $w12_ic*$sr + $w34_ic*$sp; |
|
185 $w34 = $w34_ic*$cp - $v34_ic*$sp + $v12_ic*$sr; |
|
186 $v34 = $v34_ic*$cp + $w34_ic*$sp - $w12_ic*$sr; |
|
187 } else { |
|
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 } |
|
193 |
|
194 return ($v12,$w12,$v34,$w34); |
|
195 } |
|
196 } |
|
197 |
|
198 #====================================================================== |
|
199 # velApplyHdgBias() applies the heading bias, which is used to correct |
|
200 # for magnetic declination for data recorded in Earth-coordinates ONLY. |
|
201 # Bias correction for beam-coordinate data is done in velInstrumentToEarth() |
|
202 #====================================================================== |
|
203 |
|
204 { # STATIC SCOPE |
|
205 my($sh,$ch); |
|
206 |
|
207 sub velApplyHdgBias(@) |
|
208 { |
|
209 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
|
210 return undef unless (defined($v1) && defined($v2)); |
|
211 |
|
212 unless (defined($sh)) { |
|
213 printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n", |
|
214 $dta->{HEADING_ALIGNMENT}) |
|
215 if ($dta->{HEADING_ALIGNMENT}); |
|
216 $sh = sin(rad(-$dta->{HEADING_BIAS})); |
|
217 $ch = cos(rad(-$dta->{HEADING_BIAS})); |
|
218 } |
|
219 |
|
220 return ( $v1*$ch + $v2*$sh, |
|
221 -$v1*$sh + $v2*$ch, |
|
222 $v3 , |
|
223 $v4 ); |
|
224 } |
|
225 } # STATIC SCOPE |
|
226 |
|
227 #---------------------------------------------------------------------- |
|
228 # Pitch/Roll Functions |
|
229 #---------------------------------------------------------------------- |
|
230 |
|
231 sub gimbal_pitch($$) # RDI coord trans manual |
|
232 { |
|
233 my($tilt1,$tilt2) = @_; |
|
234 return deg(atan(tan(rad($tilt1)) * cos(rad($tilt2)))); |
|
235 } |
|
236 |
|
237 # - angle from vertical is home grown and should be treated with caution |
|
238 # - angle between two unit vectors given by acos(v1 dot v2) |
|
239 # - vertical unit vector v1 = (0 0 1) => dot product = z-component of v2 |
|
240 # - when vertical unit vector is pitched in x direction, followed by |
|
241 # roll in y direction: |
|
242 # x = sin(pitch) |
|
243 # y = cos(pitch) * sin(roll) |
|
244 # z = cos(pitch) * cos(roll) |
|
245 # has been checked with sqrt(x^2+y^2+z^2) == 1 |
|
246 # - for small angles, this is very similar to sqrt(pitch^2+roll^2) |
|
247 |
|
248 sub angle_from_vertical($$) |
|
249 { |
|
250 my($tilt1,$tilt2) = @_; |
|
251 my($rad_pitch) = atan(tan(rad($tilt1)) * cos(rad($tilt2))); |
|
252 return deg(acos(cos($rad_pitch) * cos(rad($tilt2)))); |
|
253 } |
|
254 |
|
255 1; |