author | Andreas Thurnherr <ant@ldeo.columbia.edu> |
Sun, 12 Apr 2020 20:03:22 -0400 | |
changeset 51 | 148c092b3a09 |
parent 50 | 6bfec705d25e |
child 52 | 5b07a9b89aee |
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 |
|
50 | 5 |
# dlm: Mon Jul 1 16:56:51 2019 |
40 | 6 |
# (c) 2010 A.M. Thurnherr |
50 | 7 |
# uE-Info: 189 0 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? |
50 | 22 |
# Jun 30, 2019: - -o did not work with single argument |
40 | 23 |
|
24 |
# PATCH-FILE REQUIREMENTS (ANTS format) |
|
25 |
# - %LADCP_pitch.mu %LADCP_roll.mu mean LADCP pitch and roll |
|
26 |
# - %IMU_hdg_offset heading offset of external IMU |
|
27 |
# - LADCP_ens ADCP ensemble number |
|
28 |
# - pitch, roll external pitch/roll *anomalies* |
|
29 |
# - hdg external heading rotated into ADCP coord system |
|
30 |
||
31 |
# PATCHED PD0 FILE: |
|
32 |
# |
|
33 |
# - pitch = RDI_pitch(mean_LADCP_gimbal_pitch + rotated_external_pitch_anomaly) |
|
34 |
# - roll = mean_LADCP_roll + rotated_external_roll_anomaly |
|
35 |
# - hdg = external_hdg - heading_offset |
|
36 |
# |
|
37 |
# - unless -d is used, every patched ensemble has set the DATA_SOURCE_ID as follows; |
|
38 |
# PROCESSING SOFTWARE NEEDS TO BE ABLE TO DEAL WITH DSID values != 0x7F |
|
39 |
# 0xA0 no values patched |
|
40 |
# 0xA1 heading patched |
|
41 |
# 0xA2 roll patched |
|
42 |
# 0xA3 roll & heading patched |
|
43 |
# 0xA4 pitch patched |
|
44 |
# 0xA5 pitch & heading patched |
|
45 |
# 0xA6 pitch & roll patched |
|
46 |
# 0xA7 pitch, roll & heading patched |
|
47 |
# |
|
48 |
# - additionally, all velocities from ensembles with missing pitch/roll/heading |
|
49 |
# values are removed unless -k is set |
|
50 |
||
51 |
($ANTS) = (`which ANTSlib` =~ m{^(.*)/[^/]*$}); |
|
52 |
($ADCP_TOOLS) = ($0 =~ m{^(.*)/[^/]*$}); |
|
53 |
||
42 | 54 |
$antsMinLibVersion = 7.0; |
55 |
$ADCP_tools_minVersion = 2.1; |
|
40 | 56 |
|
57 |
require "$ANTS/ants.pl"; |
|
58 |
require "$ANTS/libvec.pl"; |
|
59 |
require "$ANTS/libstats.pl"; |
|
60 |
require "$ADCP_TOOLS/ADCP_tools_lib.pl"; |
|
61 |
||
62 |
$antsParseHeader = 0; |
|
42 | 63 |
$antsSuppressCommonOptions = 1; |
64 |
&antsUsage('cdhko:pr',2, |
|
40 | 65 |
'[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
|
66 |
'[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
|
67 |
'[-o) <[pitch,roll,]heading-offset>] [-k)eep velocities of unpatched ensembles]', |
40 | 68 |
'[keep original -d)ata-source id]', |
69 |
'<original PD0 file> <patched PD0 file> [external attitude file]'); |
|
70 |
||
71 |
$opt_p = $opt_r = $opt_h = 1 |
|
72 |
unless ($opt_p || $opt_r || $opt_h); |
|
73 |
||
42 | 74 |
$RDI_PD0_IO::OVERRIDE_Y2K_CLOCK = $opt_c; |
75 |
||
40 | 76 |
$LADCP_file = &antsFileArg(); |
77 |
$outPD0 = $ARGV[0]; shift; |
|
78 |
||
79 |
#---------------------------------------------------------------------- |
|
80 |
# Step 1: Read LADCP Data |
|
81 |
#---------------------------------------------------------------------- |
|
82 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
83 |
readData($LADCP_file,\%LADCP); # TRDI PD0 file |
40 | 84 |
|
85 |
#---------------------------------------------------------------------- |
|
86 |
# Step 2: Process External Attidue Input to Patch PD0 file |
|
87 |
#---------------------------------------------------------------------- |
|
88 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
89 |
&antsIn(); # load first IMP record |
40 | 90 |
|
91 |
my($ensF) = &fnr('LADCP_ens'); |
|
92 |
my($pitchF) = &fnr('pitch'); |
|
93 |
my($rollF) = &fnr('roll'); |
|
94 |
my($hdgF) = &fnr('hdg'); |
|
95 |
my($LADCP_pitch_mean) = &antsRequireParam('LADCP_pitch.mu'); |
|
96 |
my($LADCP_roll_mean) = &antsRequireParam('LADCP_roll.mu'); |
|
97 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
98 |
|
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
99 |
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
|
100 |
my($rho,$crho,$srho); |
40 | 101 |
if (defined($opt_o)) { |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
102 |
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
|
103 |
|
50 | 104 |
if (defined($rofs)) { # pitch and roll offsets supplied |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
105 |
croak("$0: cannot decode -o $opt_o\n") |
50 | 106 |
unless numbersp($pofs,$rofs,$hofs); |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
107 |
} 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
|
108 |
$hofs = $pofs; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
109 |
$pofs = undef; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
110 |
} |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
111 |
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
|
112 |
unless numberp($hofs); |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
113 |
# set up heading correction |
42 | 114 |
&antsAddParams('IMU_hdg_offset',$P{IMP_hdg_offset}) # backward compatibility |
115 |
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
|
116 |
$rho = $hofs - &antsRequireParam('IMU_hdg_offset'); # calculate correction relative to already applied one |
40 | 117 |
$crho = cos(rad($rho)); |
118 |
$srho = sin(rad($rho)); |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
119 |
|
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
120 |
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
|
121 |
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
|
122 |
+ &antsRequireParam('IMP_roll.mu') * $srho; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
123 |
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
|
124 |
+ &antsRequireParam('IMP_roll.mu') * $crho; |
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
125 |
$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
|
126 |
$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
|
127 |
} |
40 | 128 |
} |
129 |
||
50 | 130 |
my($pr_missing,$hdg_missing) = (0,0); |
131 |
my($missing_pr_block_len,$missing_hdg_block_len); |
|
132 |
||
40 | 133 |
do { |
50 | 134 |
my($ADCP_deployed); |
40 | 135 |
my($ens) = $P{RECNO}; |
136 |
die("assertion failed [$ants_[0][$ensF] != $LADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$LADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]") |
|
137 |
unless ($ants_[0][$ensF] == $LADCP{ENSEMBLE}[$ens]->{NUMBER}); |
|
138 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} = 0xA0; |
|
139 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
140 |
if (numbersp($ants_[0][$pitchF],$ants_[0][$rollF])) { # valid IMP data -> patch LADCP ensemble |
50 | 141 |
$ADCP_deployed = 1; |
142 |
$missing_pr_block_len = 0; |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
143 |
if (defined($opt_o)) { # -o set: rotate pitch and roll into correct coordinates |
40 | 144 |
my($rot_p) = ($ants_[$r][$pitchF] * $crho + |
145 |
$ants_[$r][$rollF] * $srho); |
|
146 |
my($rot_r) = (-$ants_[$r][$pitchF] * $srho + |
|
147 |
$ants_[$r][$rollF] * $crho); |
|
148 |
$ants_[$r][$pitchF] = $rot_p; |
|
149 |
$ants_[$r][$rollF] = $rot_r; |
|
150 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
151 |
if ($opt_p) { # patch pitch |
40 | 152 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_p<<2); |
153 |
$LADCP{ENSEMBLE}[$ens]->{PITCH} = RDI_pitch($LADCP_pitch_mean + $ants_[0][$pitchF], |
|
154 |
$LADCP_roll_mean + $ants_[0][$rollF]); |
|
155 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
156 |
if ($opt_r) { # patch roll |
40 | 157 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_r<<1); |
158 |
$LADCP{ENSEMBLE}[$ens]->{ROLL} = $LADCP_roll_mean + $ants_[0][$rollF]; |
|
159 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
160 |
} else { # no valid IMP pitch and roll => invalidate LADCP data |
50 | 161 |
$pr_missing++ if $ADCP_deployed; # don't count missing before deployment |
162 |
$missing_pr_block_len++; |
|
40 | 163 |
unless ($opt_k) { |
164 |
clearEns(\%LADCP,$ens); |
|
165 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0; |
|
166 |
} |
|
167 |
} |
|
168 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
169 |
if (numberp($ants_[0][$hdgF])) { # valid IMP heading |
40 | 170 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= $opt_h; |
50 | 171 |
$missing_hdg_block_len = 0; |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
172 |
if (defined($opt_o)) { # apply offset on -o; otherwise, data are correctly rotated |
40 | 173 |
$ants_[0][$hdgF] -= $rho; |
174 |
$ants_[0][$hdgF] += 360 if ($ants_[0][$hdgF] < 0); |
|
175 |
} |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
176 |
$LADCP{ENSEMBLE}[$ens]->{HEADING} = $ants_[0][$hdgF] # patch heading |
40 | 177 |
if $opt_h; |
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
178 |
} else { # no valid IMP heading => invalidate LADCP data |
50 | 179 |
$hdg_missing++ if $ADCP_deployed; |
180 |
$missing_hdg_block_len++; |
|
40 | 181 |
unless ($opt_k) { |
182 |
clearEns(\%LADCP,$ens); |
|
183 |
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0; |
|
184 |
} |
|
185 |
} |
|
186 |
} while (&antsIn()); |
|
50 | 187 |
$pr_missing -= missing_pr_block_len; # don't count final block (post recovery) |
188 |
$hdg_missing -= missing_hdg_block_len; |
|
40 | 189 |
|
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
190 |
$LADCP{ENSEMBLE}[0]->{DATA_SOURCE_ID} = 0x7F; # ensure correct DSID (1st ens: orig; 2nd ens: this prog) |
40 | 191 |
$LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID} = 0xA0 |
192 |
unless ($LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID}&0xF0 == 0xA0); |
|
193 |
||
43
b63fa355644c
commit to merge with changes from EN620
A.M. Thurnherr <athurnherr@yahoo.com>
parents:
42
diff
changeset
|
194 |
writeData($outPD0,\%LADCP); # write new PD0 |
40 | 195 |
|
196 |
my($verb) = $opt_k ? 'retained' : 'cleared'; |
|
197 |
printf(STDERR "$outPD0: %d pitch/roll & %d heading values $verb\n",$pr_missing,$hdg_missing) |
|
198 |
if ($pr_missing+$hdg_missing); |
|
199 |
||
200 |
exit(0); |
|
201 |