1 #====================================================================== |
1 #====================================================================== |
2 # R D I _ C O O R D S . P L |
2 # R D I _ C O O R D S . P L |
3 # doc: Sun Jan 19 17:57:53 2003 |
3 # doc: Sun Jan 19 17:57:53 2003 |
4 # dlm: Tue Mar 4 13:35:21 2014 |
4 # dlm: Thu May 29 09:19:54 2014 |
5 # (c) 2003 A.M. Thurnherr |
5 # (c) 2003 A.M. Thurnherr |
6 # uE-Info: 273 60 NIL 0 0 72 0 2 4 NIL ofnI |
6 # uE-Info: 282 0 NIL 0 0 72 0 2 4 NIL ofnI |
7 #====================================================================== |
7 #====================================================================== |
8 |
8 |
9 # RDI Workhorse Coordinate Transformations |
9 # RDI Workhorse Coordinate Transformations |
10 |
10 |
11 # HISTORY: |
11 # HISTORY: |
32 # per-ensemble corrections |
32 # per-ensemble corrections |
33 # Jan 15, 2012: - replaced defined(@...) by (@...) to get rid of warning |
33 # Jan 15, 2012: - replaced defined(@...) by (@...) to get rid of warning |
34 # Aug 7, 2013: - BUG: &velBeamToBPInstrument did not return any val unless |
34 # Aug 7, 2013: - BUG: &velBeamToBPInstrument did not return any val unless |
35 # all beam velocities are defined |
35 # all beam velocities are defined |
36 # Nov 27, 2013: - added &RDI_pitch(), &tilt_azimuth() |
36 # Nov 27, 2013: - added &RDI_pitch(), &tilt_azimuth() |
37 # Mar 4, 2014: - added support for missing PITCH/ROLL/HEADING |
37 # Mar 4, 2014: - added support for ensembles with missing PITCH/ROLL/HEADING |
|
38 # May 29, 2014: - BUG: vertical velocity can be calculated even without |
|
39 # heading |
|
40 # - removed some old debug statements |
|
41 # - removed unused code from &velBeamToBPInstrument |
38 |
42 |
39 use strict; |
43 use strict; |
40 use POSIX; |
44 use POSIX; |
41 |
45 |
42 my($PI) = 3.14159265358979; |
46 my($PI) = 3.14159265358979; |
63 return undef unless (defined($v1) + defined($v2) + |
67 return undef unless (defined($v1) + defined($v2) + |
64 defined($v3) + defined($v4) |
68 defined($v3) + defined($v4) |
65 >= $RDI_Coords::minValidVels); |
69 >= $RDI_Coords::minValidVels); |
66 |
70 |
67 unless (@B2I) { |
71 unless (@B2I) { |
68 # print(STDERR "RDI_Coords::minValidVels = $RDI_Coords::minValidVels\n"); |
|
69 my($a) = 1 / (2 * sin(rad($dta->{BEAM_ANGLE}))); |
72 my($a) = 1 / (2 * sin(rad($dta->{BEAM_ANGLE}))); |
70 my($b) = 1 / (4 * cos(rad($dta->{BEAM_ANGLE}))); |
73 my($b) = 1 / (4 * cos(rad($dta->{BEAM_ANGLE}))); |
71 my($c) = $dta->{CONVEX_BEAM_PATTERN} ? 1 : -1; |
74 my($c) = $dta->{CONVEX_BEAM_PATTERN} ? 1 : -1; |
72 my($d) = $a / sqrt(2); |
75 my($d) = $a / sqrt(2); |
73 @B2I = ([$c*$a, -$c*$a, 0, 0 ], |
76 @B2I = ([$c*$a, -$c*$a, 0, 0 ], |
74 [0, 0, -$c*$a, $c*$a], |
77 [0, 0, -$c*$a, $c*$a], |
75 [$b, $b, $b, $b ], |
78 [$b, $b, $b, $b ], |
76 [$d, $d, -$d, -$d ]); |
79 [$d, $d, -$d, -$d ]); |
77 # print(STDERR "@{$B2I[0]}\n@{$B2I[1]}\n@{$B2I[2]}\n@{$B2I[3]}\n"); |
|
78 } |
80 } |
79 |
81 |
80 if (!defined($v1)) { # 3-beam solutions |
82 if (!defined($v1)) { # 3-beam solutions |
81 $RDI_Coords::threeBeamFlag = 1; |
83 $RDI_Coords::threeBeamFlag = 1; |
82 $RDI_Coords::threeBeam_1++; |
84 $RDI_Coords::threeBeam_1++; |
112 { |
114 { |
113 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
115 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
114 return undef unless (defined($v1) && defined($v2) && |
116 return undef unless (defined($v1) && defined($v2) && |
115 defined($v3) && defined($v4) && |
117 defined($v3) && defined($v4) && |
116 defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
118 defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
117 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
119 defined($dta->{ENSEMBLE}[$ens]->{ROLL})); |
118 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
|
119 |
120 |
120 unless (@I2E && |
121 unless (@I2E && |
121 $hdg == $dta->{ENSEMBLE}[$ens]->{HEADING} |
|
122 - $dta->{HEADING_BIAS} && |
|
123 $pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
122 $pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
124 $roll == $dta->{ENSEMBLE}[$ens]->{ROLL}) { |
123 $roll == $dta->{ENSEMBLE}[$ens]->{ROLL}) { |
125 printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n", |
124 printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n", |
126 $dta->{HEADING_ALIGNMENT}) |
125 $dta->{HEADING_ALIGNMENT}) |
127 if ($dta->{HEADING_ALIGNMENT}); |
126 if ($dta->{HEADING_ALIGNMENT}); |
128 $hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS}; |
127 $hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS} |
|
128 if defined($dta->{ENSEMBLE}[$ens]->{HEADING}); |
129 $pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
129 $pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
130 $roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
130 $roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
131 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
131 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
132 my($sh,$ch) = (sin(rad($hdg)), cos(rad($hdg))); |
132 my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg))) |
|
133 if defined($hdg); |
133 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
134 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
134 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
135 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
135 @I2E = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
136 @I2E = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
136 ? ( |
137 ? ( |
137 [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
138 [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr], |
141 [$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr], |
142 [$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr], |
142 [$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr], |
143 [$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr], |
143 [-$cp*$sr, $sp, $cp*$cr, ], |
144 [-$cp*$sr, $sp, $cp*$cr, ], |
144 ); |
145 ); |
145 } |
146 } |
146 return ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2], |
147 return defined($dta->{ENSEMBLE}[$ens]->{HEADING}) |
147 $v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2], |
148 ? ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2], |
148 $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2], |
149 $v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2], |
149 $v4); |
150 $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2], |
150 |
151 $v4) |
|
152 : (undef,undef, |
|
153 $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2], |
|
154 $v4); |
151 } |
155 } |
152 } # STATIC SCOPE |
156 } # STATIC SCOPE |
153 |
157 |
154 #====================================================================== |
158 #====================================================================== |
155 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
159 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
166 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
170 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
167 my($v12,$w12,$v34,$w34); |
171 my($v12,$w12,$v34,$w34); |
168 |
172 |
169 return (undef,undef,undef,undef) |
173 return (undef,undef,undef,undef) |
170 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
174 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
171 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
175 defined($dta->{ENSEMBLE}[$ens]->{ROLL})); |
172 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
|
173 |
176 |
174 unless (defined($TwoCosBAngle)) { |
177 unless (defined($TwoCosBAngle)) { |
175 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
178 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
176 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
179 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
177 } |
180 } |
225 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
228 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
226 my($v12,$w12,$v34,$w34); |
229 my($v12,$w12,$v34,$w34); |
227 |
230 |
228 return (undef,undef,undef,undef) |
231 return (undef,undef,undef,undef) |
229 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
232 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
230 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
233 defined($dta->{ENSEMBLE}[$ens]->{ROLL})); |
231 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
|
232 |
234 |
233 unless (defined($TwoCosBAngle)) { |
235 unless (defined($TwoCosBAngle)) { |
234 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
236 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
235 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
237 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
236 } |
238 } |
237 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
|
238 my($sr) = sin($roll); my($cr) = cos($roll); |
|
239 my($pitch) = atan(tan(rad($dta->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch |
|
240 my($sp) = sin($pitch); my($cp) = cos($pitch); |
|
241 |
239 |
242 # Sign convention: |
240 # Sign convention: |
243 # - refer to Coord manual Fig. 3 |
241 # - refer to Coord manual Fig. 3 |
244 # - v12 is horizontal velocity from beam1 to beam2 |
242 # - v12 is horizontal velocity from beam1 to beam2 |
245 # - w is +ve upward, regardless of instrument orientation |
243 # - w is +ve upward, regardless of instrument orientation |