author | Andreas Thurnherr <ant@ldeo.columbia.edu> |
Sun, 21 Oct 2018 20:01:48 -0400 | |
changeset 45 | 5767cbe470a0 |
parent 43 | b63fa355644c |
child 50 | 6bfec705d25e |
permissions | -rwxr-xr-x |
40 | 1 |
#!/usr/bin/perl |
2 |
#====================================================================== |
|
3 |
# P A T C H P D 0 |
|
4 |
# doc: Tue Aug 23 20:00:15 2016 |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
5 |
# dlm: Wed Jun 13 20:35:05 2018 |
40 | 6 |
# (c) 2010 A.M. Thurnherr |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
7 |
# uE-Info: 184 88 NIL 0 0 72 2 2 4 NIL ofnI |
40 | 8 |
#====================================================================== |
9 |
||
10 |
$antsSummary = 'patch TRDI PD0 file with external attitude data'; |
|
11 |
||
12 |
# History: |
|
13 |
# Aug 23, 2016: - exported from IMP+LADCP |
|
14 |
# Aug 25, 2016: - completed basic structure |
|
15 |
# Nov 20, 2017: - major code cleanup |
|
16 |
# - added -d) to keep original data source id |
|
42 | 17 |
# Dec 9, 2017: - added $antsSuppressCommonOptions = 1; |
18 |
# Dec 23, 2017: - added support for -c |
|
19 |
# - BUG: not backward compatible with old IMP files any more |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
20 |
# Jun 13, 2017: - added pitch and roll to -o |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
21 |
# - BUG: ??? does -o handle pitch and roll ANOMALIES correctly? |
40 | 22 |
|
23 |
# PATCH-FILE REQUIREMENTS (ANTS format) |
|
24 |
# - %LADCP_pitch.mu %LADCP_roll.mu mean LADCP pitch and roll |
|
25 |
# - %IMU_hdg_offset heading offset of external IMU |
|
26 |
# - LADCP_ens ADCP ensemble number |
|
27 |
# - pitch, roll external pitch/roll *anomalies* |
|
28 |
# - hdg external heading rotated into ADCP coord system |
|
29 |
||
30 |
# PATCHED PD0 FILE: |
|
31 |
# |
|
32 |
# - pitch = RDI_pitch(mean_LADCP_gimbal_pitch + rotated_external_pitch_anomaly) |
|
33 |
# - roll = mean_LADCP_roll + rotated_external_roll_anomaly |
|
34 |
# - hdg = external_hdg - heading_offset |
|
35 |
# |
|
36 |
# - unless -d is used, every patched ensemble has set the DATA_SOURCE_ID as follows; |
|
37 |
# PROCESSING SOFTWARE NEEDS TO BE ABLE TO DEAL WITH DSID values != 0x7F |
|
38 |
# 0xA0 no values patched |
|
39 |
# 0xA1 heading patched |
|
40 |
# 0xA2 roll patched |
|
41 |
# 0xA3 roll & heading patched |
|
42 |
# 0xA4 pitch patched |
|
43 |
# 0xA5 pitch & heading patched |
|
44 |
# 0xA6 pitch & roll patched |
|
45 |
# 0xA7 pitch, roll & heading patched |
|
46 |
# |
|
47 |
# - additionally, all velocities from ensembles with missing pitch/roll/heading |
|
48 |
# values are removed unless -k is set |
|
49 |
||
50 |
($ANTS) = (`which ANTSlib` =~ m{^(.*)/[^/]*$}); |
|
51 |
($ADCP_TOOLS) = ($0 =~ m{^(.*)/[^/]*$}); |
|
52 |
||
42 | 53 |
$antsMinLibVersion = 7.0; |
54 |
$ADCP_tools_minVersion = 2.1; |
|
40 | 55 |
|
56 |
require "$ANTS/ants.pl"; |
|
57 |
require "$ANTS/libvec.pl"; |
|
58 |
require "$ANTS/libstats.pl"; |
|
59 |
require "$ADCP_TOOLS/ADCP_tools_lib.pl"; |
|
60 |
||
61 |
$antsParseHeader = 0; |
|
42 | 62 |
$antsSuppressCommonOptions = 1; |
63 |
&antsUsage('cdhko:pr',2, |
|
40 | 64 |
'[patch -p)itch] [-r)oll] [-h)eading] (none patches all)', |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
65 |
'[patch -c)lock with pre-Y2K RTC values]', |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
66 |
'[-o) <[pitch,roll,]heading-offset>] [-k)eep velocities of unpatched ensembles]', |
40 | 67 |
'[keep original -d)ata-source id]', |
68 |
'<original PD0 file> <patched PD0 file> [external attitude file]'); |
|
69 |
||
70 |
$opt_p = $opt_r = $opt_h = 1 |
|
71 |
unless ($opt_p || $opt_r || $opt_h); |
|
72 |
||
42 | 73 |
$RDI_PD0_IO::OVERRIDE_Y2K_CLOCK = $opt_c; |
74 |
||
40 | 75 |
$LADCP_file = &antsFileArg(); |
76 |
$outPD0 = $ARGV[0]; shift; |
|
77 |
||
78 |
#---------------------------------------------------------------------- |
|
79 |
# Step 1: Read LADCP Data |
|
80 |
#---------------------------------------------------------------------- |
|
81 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
82 |
readData($LADCP_file,\%LADCP); # TRDI PD0 file |
40 | 83 |
|
84 |
#---------------------------------------------------------------------- |
|
85 |
# Step 2: Process External Attidue Input to Patch PD0 file |
|
86 |
#---------------------------------------------------------------------- |
|
87 |
||
88 |
my($pr_missing,$hdg_missing) = (0,0); |
|
89 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
90 |
&antsIn(); # load first IMP record |
40 | 91 |
|
92 |
my($ensF) = &fnr('LADCP_ens'); |
|
93 |
my($pitchF) = &fnr('pitch'); |
|
94 |
my($rollF) = &fnr('roll'); |
|
95 |
my($hdgF) = &fnr('hdg'); |
|
96 |
my($LADCP_pitch_mean) = &antsRequireParam('LADCP_pitch.mu'); |
|
97 |
my($LADCP_roll_mean) = &antsRequireParam('LADCP_roll.mu'); |
|
98 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
99 |
|
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
100 |
my($pofs,$rofs) = (0,0); # apply externally supplied offset(s) |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
101 |
my($rho,$crho,$srho); |
40 | 102 |
if (defined($opt_o)) { |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
103 |
my($pofs,$rofs,$hofs) = split(/,/,$opt_o); |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
104 |
|
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
105 |
if (defined($pofs)) { # pitch and roll offsets supplied |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
106 |
croak("$0: cannot decode -o $opt_o\n") |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
107 |
unless numbersp($pofs,$rofs); |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
108 |
} else { # no pitch and roll, only heading |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
109 |
$hofs = $pofs; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
110 |
$pofs = undef; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
111 |
} |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
112 |
croak("$0: cannot decode -o $opt_o\n") |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
113 |
unless numberp($hofs); |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
114 |
# set up heading correction |
42 | 115 |
&antsAddParams('IMU_hdg_offset',$P{IMP_hdg_offset}) # backward compatibility |
116 |
if defined($P{IMP_hdg_offset}); |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
117 |
$rho = $hofs - &antsRequireParam('IMU_hdg_offset'); # calculate correction relative to already applied one |
40 | 118 |
$crho = cos(rad($rho)); |
119 |
$srho = sin(rad($rho)); |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
120 |
|
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
121 |
if (defined($pofs)) { # rotate IMP pitch and roll offsets into new LADCP frame |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
122 |
my($IMP_pitch_mean) = &antsRequireParam('IMP_pitch.mu') * $crho |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
123 |
+ &antsRequireParam('IMP_roll.mu') * $srho; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
124 |
my($IMP_roll_mean) = -&antsRequireParam('IMP_pitch.mu') * $srho |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
125 |
+ &antsRequireParam('IMP_roll.mu') * $crho; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
126 |
$LADCP_pitch_mean = $IMP_pitch_mean - $pofs; # apply externally supplied offsets |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
127 |
$LADCP_roll_mean = $IMP_roll_mean - $rofs; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
128 |
} |
40 | 129 |
} |
130 |
||
131 |
do { |
|
132 |
my($ens) = $P{RECNO}; |
|
133 |
die("assertion failed [$ants_[0][$ensF] != $LADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$LADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]") |
|
134 |
unless ($ants_[0][$ensF] == $LADCP{ENSEMBLE}[$ens]->{NUMBER}); |
|
135 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} = 0xA0; |
|
136 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
137 |
if (numbersp($ants_[0][$pitchF],$ants_[0][$rollF])) { # valid IMP data -> patch LADCP ensemble |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
138 |
if (defined($opt_o)) { # -o set: rotate pitch and roll into correct coordinates |
40 | 139 |
my($rot_p) = ($ants_[$r][$pitchF] * $crho + |
140 |
$ants_[$r][$rollF] * $srho); |
|
141 |
my($rot_r) = (-$ants_[$r][$pitchF] * $srho + |
|
142 |
$ants_[$r][$rollF] * $crho); |
|
143 |
$ants_[$r][$pitchF] = $rot_p; |
|
144 |
$ants_[$r][$rollF] = $rot_r; |
|
145 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
146 |
if ($opt_p) { # patch pitch |
40 | 147 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_p<<2); |
148 |
$LADCP{ENSEMBLE}[$ens]->{PITCH} = RDI_pitch($LADCP_pitch_mean + $ants_[0][$pitchF], |
|
149 |
$LADCP_roll_mean + $ants_[0][$rollF]); |
|
150 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
151 |
if ($opt_r) { # patch roll |
40 | 152 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_r<<1); |
153 |
$LADCP{ENSEMBLE}[$ens]->{ROLL} = $LADCP_roll_mean + $ants_[0][$rollF]; |
|
154 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
155 |
} else { # no valid IMP pitch and roll => invalidate LADCP data |
40 | 156 |
$pr_missing++; |
157 |
unless ($opt_k) { |
|
158 |
clearEns(\%LADCP,$ens); |
|
159 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0; |
|
160 |
} |
|
161 |
} |
|
162 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
163 |
if (numberp($ants_[0][$hdgF])) { # valid IMP heading |
40 | 164 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= $opt_h; |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
165 |
if (defined($opt_o)) { # apply offset on -o; otherwise, data are correctly rotated |
40 | 166 |
$ants_[0][$hdgF] -= $rho; |
167 |
$ants_[0][$hdgF] += 360 if ($ants_[0][$hdgF] < 0); |
|
168 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
169 |
$LADCP{ENSEMBLE}[$ens]->{HEADING} = $ants_[0][$hdgF] # patch heading |
40 | 170 |
if $opt_h; |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
171 |
} else { # no valid IMP heading => invalidate LADCP data |
40 | 172 |
$hdg_missing++; |
173 |
unless ($opt_k) { |
|
174 |
clearEns(\%LADCP,$ens); |
|
175 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0; |
|
176 |
} |
|
177 |
} |
|
178 |
} while (&antsIn()); |
|
179 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
180 |
$LADCP{ENSEMBLE}[0]->{DATA_SOURCE_ID} = 0x7F; # ensure correct DSID (1st ens: orig; 2nd ens: this prog) |
40 | 181 |
$LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID} = 0xA0 |
182 |
unless ($LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID}&0xF0 == 0xA0); |
|
183 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
184 |
writeData($outPD0,\%LADCP); # write new PD0 |
40 | 185 |
|
186 |
my($verb) = $opt_k ? 'retained' : 'cleared'; |
|
187 |
printf(STDERR "$outPD0: %d pitch/roll & %d heading values $verb\n",$pr_missing,$hdg_missing) |
|
188 |
if ($pr_missing+$hdg_missing); |
|
189 |
||
190 |
exit(0); |
|
191 |