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: Mon Nov 27 07:13:25 2017 |
4 # dlm: Wed Mar 28 12:30:12 2018 |
5 # (c) 2003 A.M. Thurnherr |
5 # (c) 2003 A.M. Thurnherr |
6 # uE-Info: 58 62 NIL 0 0 72 10 2 4 NIL ofnI |
6 # uE-Info: 109 22 NIL 0 0 72 10 2 4 NIL ofnI |
7 #====================================================================== |
7 #====================================================================== |
8 |
8 |
9 # RDI Workhorse Coordinate Transformations |
9 # RDI Workhorse Coordinate Transformations |
10 |
10 |
11 # HISTORY: |
11 # HISTORY: |
54 # Jul 7, 2016: - added velEarthToBPw() with algorithm debugged and verified |
54 # Jul 7, 2016: - added velEarthToBPw() with algorithm debugged and verified |
55 # by Paul Wanis from TRDI |
55 # by Paul Wanis from TRDI |
56 # Oct 12, 2017: - documentation |
56 # Oct 12, 2017: - documentation |
57 # Nov 26, 2017: - BUG: velBeamtoBPEarth() did not respect missing values |
57 # Nov 26, 2017: - BUG: velBeamtoBPEarth() did not respect missing values |
58 # Nov 27, 2017: - BUG: numbersp() from [antslib.pl] was used |
58 # Nov 27, 2017: - BUG: numbersp() from [antslib.pl] was used |
|
59 # Mar 28, 2018: - added &loadInstrumentTransformation() |
59 |
60 |
60 use strict; |
61 use strict; |
61 use POSIX; |
62 use POSIX; |
62 |
63 |
63 my($PI) = 3.14159265358979; |
64 my($PI) = 3.14159265358979; |
72 $RDI_Coords::minValidVels = 3; # 3-beam solutions ok (velBeamToInstrument) |
73 $RDI_Coords::minValidVels = 3; # 3-beam solutions ok (velBeamToInstrument) |
73 $RDI_Coords::binMapping = 'linterp'; # 'linterp' or 'none' (earthVels, BPearthVels) |
74 $RDI_Coords::binMapping = 'linterp'; # 'linterp' or 'none' (earthVels, BPearthVels) |
74 $RDI_Coords::beamTransformation = 'LHR90'; # set to 'RDI' to use 1st order transformations from RDI manual |
75 $RDI_Coords::beamTransformation = 'LHR90'; # set to 'RDI' to use 1st order transformations from RDI manual |
75 |
76 |
76 #---------------------------------------------------------------------- |
77 #---------------------------------------------------------------------- |
77 # beam to earth transformation |
78 # beam to earth transformation |
|
79 # - loadInstrumentTransformation(filename) loads a file that contains the |
|
80 # output from the PS3 command, which includes the instrument transformation |
|
81 # matrix as follows: |
|
82 # Instrument Transformation Matrix (Down): Q14: |
|
83 # 1.4689 -1.4682 0.0030 -0.0035 24067 -24055 49 -58 |
|
84 # -0.0036 0.0029 -1.4664 1.4673 -59 48 -24025 24041 |
|
85 # 0.2658 0.2661 0.2661 0.2657 4355 4359 4359 4354 |
|
86 # 1.0373 1.0382 -1.0385 -1.0373 16995 17010 -17015 -16995 |
78 #---------------------------------------------------------------------- |
87 #---------------------------------------------------------------------- |
79 |
88 |
80 $RDI_Coords::threeBeam_1 = 0; # stats from velBeamToInstrument |
89 $RDI_Coords::threeBeam_1 = 0; # stats from velBeamToInstrument |
81 $RDI_Coords::threeBeam_2 = 0; |
90 $RDI_Coords::threeBeam_2 = 0; |
82 $RDI_Coords::threeBeam_3 = 0; |
91 $RDI_Coords::threeBeam_3 = 0; |
85 |
94 |
86 $RDI_Coords::threeBeamFlag = 0; # flag last transformation |
95 $RDI_Coords::threeBeamFlag = 0; # flag last transformation |
87 |
96 |
88 { # STATIC SCOPE |
97 { # STATIC SCOPE |
89 my(@B2I); |
98 my(@B2I); |
|
99 |
|
100 sub loadInstrumentTransformation($) |
|
101 { |
|
102 die("loadInstrumentTransformation(): B2I matrix already defined\n") |
|
103 if (@B2I); |
|
104 open(ITF,$_[0]) || die("$_[0]: $!\n"); |
|
105 my($row) = 0; |
|
106 while (<ITF>) { |
|
107 if ($row == 0) { |
|
108 next unless m{^Instrument Transformation Matrix \(Down\):}; |
|
109 $row = 1; |
|
110 } elsif ($row <= 4) { |
|
111 my(@vals) = split; |
|
112 die("$_[0]: cannot decode row #$row of Instrument Transformation Matrix\n") |
|
113 unless (@vals == 8); |
|
114 for (my($i)=0; $i<4; $i++) { |
|
115 die("$_[0]: cannot decode row #$row of Instrument Transformation Matrix\n") |
|
116 unless numberp($vals[$i]); |
|
117 $B2I[$row-1][$i] = $vals[$i]; |
|
118 } |
|
119 $row++; |
|
120 } else { |
|
121 last; |
|
122 } |
|
123 } |
|
124 die("$_[0]: cannot decode Instrument Transformation Matrix (row = $row)\n") |
|
125 unless ($row == 5); |
|
126 close(ITF); |
|
127 } |
90 |
128 |
91 sub velBeamToInstrument(@) |
129 sub velBeamToInstrument(@) |
92 { |
130 { |
93 my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_; |
131 my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_; |
94 return undef unless (defined($v1) + defined($v2) + |
132 return undef unless (defined($v1) + defined($v2) + |
132 $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
170 $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3], |
133 $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
171 $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]); |
134 } |
172 } |
135 } # STATIC SCOPE |
173 } # STATIC SCOPE |
136 |
174 |
137 #---------------------------------------------------------------------- |
175 #-------------------------------------------------------------------------------------------------------------- |
138 # velInstrumentToEarth(\%ADCP,ens,v1,v2,v3,v4) => (u,v,w,e) |
176 # velInstrumentToEarth(\%ADCP,ens,v1,v2,v3,v4) => (u,v,w,e) |
139 # - $RDI_Coords::beamTransformation = 'LHR90' |
177 # - $RDI_Coords::beamTransformation = 'LHR90' |
140 # - from Lohrmann, Hackett & Roet (J. Tech., 1990) |
178 # - from Lohrmann, Hackett & Roet (J. Tech., 1990) |
141 # - eq A1 maps to RDI matrix M (sec 5.6) with |
179 # - eq A1 maps to RDI matrix M (sec 5.6) with |
142 # alpha = roll |
180 # alpha = roll |
143 # beta = gimball_pitch |
181 # beta = gimball_pitch |
144 # psi = calculation_pitch |
182 # psi (pitch used for calculation) = asin{sin(beta) cos(alpha) / sqrt[1- sin(alpha)^2 sin(beta)^2]} |
145 # psi = asin{sin(beta) cos(alpha) / sqrt[1- sin(alpha)^2 sin(beta)^2]} |
|
146 # - (I only checked for 0 heading, but this is sufficient) |
183 # - (I only checked for 0 heading, but this is sufficient) |
147 # - $RDI_Coords::beamTransformation = 'RDI' |
184 # - $RDI_Coords::beamTransformation = 'RDI' |
148 # - default prior to LADCP_w V1.3 |
185 # - default prior to LADCP_w V1.3 |
149 # - from RDI manual |
186 # - from RDI manual |
150 # - 99% accurate for p/r<8deg |
187 # - 99% accurate for p/r<8deg |
151 # => 1cm/s error for 1m/s winch speed! |
188 # => 1cm/s error for 1m/s winch speed! |
152 #---------------------------------------------------------------------- |
189 #-------------------------------------------------------------------------------------------------------------- |
153 |
190 |
154 { # STATIC SCOPE |
191 { # STATIC SCOPE |
155 my($hdg,$pitch,$roll,@I2E); |
192 my($hdg,$pitch,$roll,@I2E); |
156 |
193 |
157 sub velInstrumentToEarth(@) |
194 sub velInstrumentToEarth(@) |