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 Jan 5 13:56:55 2016 |
4 # dlm: Sun Jan 31 12:42:43 2016 |
5 # (c) 2003 A.M. Thurnherr |
5 # (c) 2003 A.M. Thurnherr |
6 # uE-Info: 185 37 NIL 0 0 72 0 2 4 NIL ofnI |
6 # uE-Info: 185 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: |
38 # May 29, 2014: - BUG: vertical velocity can be calculated even without |
38 # May 29, 2014: - BUG: vertical velocity can be calculated even without |
39 # heading |
39 # heading |
40 # - removed some old debug statements |
40 # - removed some old debug statements |
41 # - removed unused code from &velBeamToBPInstrument |
41 # - removed unused code from &velBeamToBPInstrument |
42 # Jan 5, 2016: - added &velEarthToInstrument(@), &velInstrumentToBeam(@) |
42 # Jan 5, 2016: - added &velEarthToInstrument(@), &velInstrumentToBeam(@) |
|
43 # Jan 9, 2016: - added &velEarthToBeam(), &velBeamToEarth() |
43 |
44 |
44 use strict; |
45 use strict; |
45 use POSIX; |
46 use POSIX; |
46 |
47 |
47 my($PI) = 3.14159265358979; |
48 my($PI) = 3.14159265358979; |
168 sub velEarthToInstrument(@) |
169 sub velEarthToInstrument(@) |
169 { |
170 { |
170 my($dta,$ens,$u,$v,$w,$ev) = @_; |
171 my($dta,$ens,$u,$v,$w,$ev) = @_; |
171 |
172 |
172 unless (@E2I) { |
173 unless (@E2I) { |
173 $hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS} if defined($dta->{ENSEMBLE}[$ens]->{HEADING}); |
174 $hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS} |
|
175 if defined($dta->{ENSEMBLE}[$ens]->{HEADING}); |
174 $pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
176 $pitch = $dta->{ENSEMBLE}[$ens]->{PITCH}; |
175 $roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
177 $roll = $dta->{ENSEMBLE}[$ens]->{ROLL}; |
176 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
178 my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll))); |
177 my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg))) |
179 my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg))) |
178 if defined($hdg); |
180 if defined($hdg); |
179 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
181 my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch)); |
180 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
182 my($sr,$cr) = (sin(rad($roll)), cos(rad($roll))); |
181 @E2I = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
183 @E2I = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP} |
182 ? ([$ch*-$cr+$sh*$sp*-$sr, $ch*$sp*-$sr-$sh*-$cr, $cp*-$sr], |
184 ? ( |
|
185 [$ch*-$cr+$sh*$sp*-$sr, $ch*$sp*-$sr-$sh*-$cr, $cp*-$sr], |
183 [$sh*$cp, $ch*$cp, $sp ], |
186 [$sh*$cp, $ch*$cp, $sp ], |
184 [$ch*-$sr-$sh*$sp*-$cr, -$sh*-$sr-$ch*$sp*-$cr, $cp*-$cr]) |
187 [$ch*-$sr-$sh*$sp*-$cr, -$sh*-$sr-$ch*$sp*-$cr, $cp*-$cr] |
185 : ([$ch*$cr+$sh*$sp*$sr, $ch*$sp*$sr-$sh*$cr, $cp*$sr ], |
188 ) : ( |
|
189 [$ch*$cr+$sh*$sp*$sr, $ch*$sp*$sr-$sh*$cr, $cp*$sr ], |
186 [$sh*$cp, $ch*$cp, $sp ], |
190 [$sh*$cp, $ch*$cp, $sp ], |
187 [$ch*$sr-$sh*$sp*$cr, -$sh*$sr-$ch*$sp*$cr, $cp*$cr ]); |
191 [$ch*$sr-$sh*$sp*$cr, -$sh*$sr-$ch*$sp*$cr, $cp*$cr ] |
|
192 ); |
188 } |
193 } |
189 |
194 |
190 return defined($dta->{ENSEMBLE}[$ens]->{HEADING}) |
195 return defined($dta->{ENSEMBLE}[$ens]->{HEADING}) |
191 ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2], |
196 ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2], |
192 $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2], |
197 $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2], |
226 -$y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d), |
231 -$y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d), |
227 $y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d)); |
232 $y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d)); |
228 |
233 |
229 } |
234 } |
230 } # STATIC SCOPE |
235 } # STATIC SCOPE |
|
236 |
|
237 #---------------------------------------------------------------------- |
|
238 # velEarthToBeam() combines velEarthToInstrument and velInstrumentToBeam |
|
239 #---------------------------------------------------------------------- |
|
240 |
|
241 sub velEarthToBeam(@) |
|
242 { |
|
243 my($dta,$ens,$u,$v,$w,$ev) = @_; |
|
244 return velInstrumentToBeam($dta, |
|
245 velEarthToInstrument($dta,$ens,$u,$v,$w,$ev)); |
|
246 } |
231 |
247 |
232 #====================================================================== |
248 #====================================================================== |
233 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
249 # velBeamToBPEarth(@) calculates the vertical- and horizontal vels |
234 # from the two beam pairs separately. Note that (w1+w2)/2 is |
250 # from the two beam pairs separately. Note that (w1+w2)/2 is |
235 # identical to the w estimated according to RDI without 3-beam |
251 # identical to the w estimated according to RDI without 3-beam |