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: Sat Feb 22 09:43:27 2014 |
4 # dlm: Tue Mar 4 13:35:21 2014 |
5 # (c) 2003 A.M. Thurnherr |
5 # (c) 2003 A.M. Thurnherr |
6 # uE-Info: 37 0 NIL 0 0 72 0 2 4 NIL ofnI |
6 # uE-Info: 273 60 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 |
38 |
38 use strict; |
39 use strict; |
39 use POSIX; |
40 use POSIX; |
40 |
41 |
41 my($PI) = 3.14159265358979; |
42 my($PI) = 3.14159265358979; |
109 |
110 |
110 sub velInstrumentToEarth(@) |
111 sub velInstrumentToEarth(@) |
111 { |
112 { |
112 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
113 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
113 return undef unless (defined($v1) && defined($v2) && |
114 return undef unless (defined($v1) && defined($v2) && |
114 defined($v3) && defined($v4)); |
115 defined($v3) && defined($v4) && |
|
116 defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
|
117 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
|
118 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
115 |
119 |
116 unless (@I2E && |
120 unless (@I2E && |
117 $hdg == $dta->{ENSEMBLE}[$ens]->{HEADING} |
121 $hdg == $dta->{ENSEMBLE}[$ens]->{HEADING} |
118 - $dta->{HEADING_BIAS} && |
122 - $dta->{HEADING_BIAS} && |
119 $pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
123 $pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} && |
160 sub velBeamToBPEarth(@) |
164 sub velBeamToBPEarth(@) |
161 { |
165 { |
162 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
166 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
163 my($v12,$w12,$v34,$w34); |
167 my($v12,$w12,$v34,$w34); |
164 |
168 |
|
169 return (undef,undef,undef,undef) |
|
170 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
|
171 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
|
172 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
|
173 |
165 unless (defined($TwoCosBAngle)) { |
174 unless (defined($TwoCosBAngle)) { |
166 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
175 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
167 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
176 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
168 } |
177 } |
169 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
178 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
214 sub velBeamToBPInstrument(@) |
223 sub velBeamToBPInstrument(@) |
215 { |
224 { |
216 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
225 my($dta,$ens,$b1,$b2,$b3,$b4) = @_; |
217 my($v12,$w12,$v34,$w34); |
226 my($v12,$w12,$v34,$w34); |
218 |
227 |
|
228 return (undef,undef,undef,undef) |
|
229 unless (defined($dta->{ENSEMBLE}[$ens]->{PITCH}) && |
|
230 defined($dta->{ENSEMBLE}[$ens]->{ROLL}) && |
|
231 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
|
232 |
219 unless (defined($TwoCosBAngle)) { |
233 unless (defined($TwoCosBAngle)) { |
220 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
234 $TwoCosBAngle = 2 * cos(rad($dta->{BEAM_ANGLE})); |
221 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
235 $TwoSinBAngle = 2 * sin(rad($dta->{BEAM_ANGLE})); |
222 } |
236 } |
223 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
237 my($roll) = rad($dta->{ENSEMBLE}[$ens]->{ROLL}); |
252 #====================================================================== |
266 #====================================================================== |
253 |
267 |
254 sub velApplyHdgBias(@) |
268 sub velApplyHdgBias(@) |
255 { |
269 { |
256 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
270 my($dta,$ens,$v1,$v2,$v3,$v4) = @_; |
257 return undef unless (defined($v1) && defined($v2)); |
271 return (undef,undef,undef,undef) |
|
272 unless (defined($v1) && defined($v2) && |
|
273 defined($dta->{ENSEMBLE}[$ens]->{HEADING})); |
258 |
274 |
259 my($sh) = sin(rad(-$dta->{HEADING_BIAS})); |
275 my($sh) = sin(rad(-$dta->{HEADING_BIAS})); |
260 my($ch) = cos(rad(-$dta->{HEADING_BIAS})); |
276 my($ch) = cos(rad(-$dta->{HEADING_BIAS})); |
261 |
277 |
262 return ( $v1*$ch + $v2*$sh, |
278 return ( $v1*$ch + $v2*$sh, |
270 #---------------------------------------------------------------------- |
286 #---------------------------------------------------------------------- |
271 |
287 |
272 sub gimbal_pitch($$) # RDI coord trans manual |
288 sub gimbal_pitch($$) # RDI coord trans manual |
273 { |
289 { |
274 my($RDI_pitch,$RDI_roll) = @_; |
290 my($RDI_pitch,$RDI_roll) = @_; |
|
291 return 'nan' unless defined($RDI_pitch) && defined($RDI_roll); |
275 return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll)))); |
292 return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll)))); |
276 } |
293 } |
277 |
294 |
278 sub RDI_pitch($$) |
295 sub RDI_pitch($$) |
279 { |
296 { |
280 my($gimbal_pitch,$roll) = @_; |
297 my($gimbal_pitch,$roll) = @_; |
|
298 return 'nan' unless defined($gimbal_pitch) && defined($roll); |
281 return deg(atan(tan(rad($gimbal_pitch))/cos(rad($roll)))); |
299 return deg(atan(tan(rad($gimbal_pitch))/cos(rad($roll)))); |
282 } |
300 } |
283 |
301 |
284 sub tilt_azimuth($$) |
302 sub tilt_azimuth($$) |
285 { |
303 { |
286 my($gimbal_pitch,$roll) = @_; |
304 my($gimbal_pitch,$roll) = @_; |
|
305 return 'nan' unless defined($gimbal_pitch) && defined($roll); |
287 return angle(deg(atan2(sin(rad($gimbal_pitch)),sin(rad($roll))))); |
306 return angle(deg(atan2(sin(rad($gimbal_pitch)),sin(rad($roll))))); |
288 } |
307 } |
289 |
308 |
290 # - angle from vertical is home grown and should be treated with caution |
309 # - angle from vertical is home grown |
291 # - angle between two unit vectors given by acos(v1 dot v2) |
310 # - 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 |
311 # - 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 |
312 # - when vertical unit vector is pitched in x direction, followed by |
294 # roll in y direction: |
313 # roll in y direction: |
295 # x = sin(pitch) |
314 # x = sin(pitch) |
299 # - for small angles, this is very similar to sqrt(pitch^2+roll^2) |
318 # - for small angles, this is very similar to sqrt(pitch^2+roll^2) |
300 |
319 |
301 sub angle_from_vertical($$) |
320 sub angle_from_vertical($$) |
302 { |
321 { |
303 my($RDI_pitch,$RDI_roll) = @_; |
322 my($RDI_pitch,$RDI_roll) = @_; |
|
323 return 'nan' unless defined($RDI_pitch) && defined($RDI_roll); |
304 my($rad_pitch) = atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))); |
324 my($rad_pitch) = atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))); |
305 return deg(acos(cos($rad_pitch) * cos(rad($RDI_roll)))); |
325 return deg(acos(cos($rad_pitch) * cos(rad($RDI_roll)))); |
306 } |
326 } |
307 |
327 |
308 1; |
328 1; |