#!/usr/bin/perl
#======================================================================
# P A T C H P D 0
# doc: Tue Aug 23 20:00:15 2016
# dlm: Wed Jun 13 20:35:05 2018
# (c) 2010 A.M. Thurnherr
# uE-Info: 184 88 NIL 0 0 72 2 2 4 NIL ofnI
#======================================================================
$antsSummary = 'patch TRDI PD0 file with external attitude data';
# History:
# Aug 23, 2016: - exported from IMP+LADCP
# Aug 25, 2016: - completed basic structure
# Nov 20, 2017: - major code cleanup
# - added -d) to keep original data source id
# Dec 9, 2017: - added $antsSuppressCommonOptions = 1;
# Dec 23, 2017: - added support for -c
# - BUG: not backward compatible with old IMP files any more
# Jun 13, 2017: - added pitch and roll to -o
# - BUG: ??? does -o handle pitch and roll ANOMALIES correctly?
# PATCH-FILE REQUIREMENTS (ANTS format)
# - %LADCP_pitch.mu %LADCP_roll.mu mean LADCP pitch and roll
# - %IMU_hdg_offset heading offset of external IMU
# - LADCP_ens ADCP ensemble number
# - pitch, roll external pitch/roll *anomalies*
# - hdg external heading rotated into ADCP coord system
# PATCHED PD0 FILE:
#
# - pitch = RDI_pitch(mean_LADCP_gimbal_pitch + rotated_external_pitch_anomaly)
# - roll = mean_LADCP_roll + rotated_external_roll_anomaly
# - hdg = external_hdg - heading_offset
#
# - unless -d is used, every patched ensemble has set the DATA_SOURCE_ID as follows;
# PROCESSING SOFTWARE NEEDS TO BE ABLE TO DEAL WITH DSID values != 0x7F
# 0xA0 no values patched
# 0xA1 heading patched
# 0xA2 roll patched
# 0xA3 roll & heading patched
# 0xA4 pitch patched
# 0xA5 pitch & heading patched
# 0xA6 pitch & roll patched
# 0xA7 pitch, roll & heading patched
#
# - additionally, all velocities from ensembles with missing pitch/roll/heading
# values are removed unless -k is set
($ANTS) = (`which ANTSlib` =~ m{^(.*)/[^/]*$});
($ADCP_TOOLS) = ($0 =~ m{^(.*)/[^/]*$});
$antsMinLibVersion = 7.0;
$ADCP_tools_minVersion = 2.1;
require "$ANTS/ants.pl";
require "$ANTS/libvec.pl";
require "$ANTS/libstats.pl";
require "$ADCP_TOOLS/ADCP_tools_lib.pl";
$antsParseHeader = 0;
$antsSuppressCommonOptions = 1;
&antsUsage('cdhko:pr',2,
'[patch -p)itch] [-r)oll] [-h)eading] (none patches all)',
'[patch -c)lock with pre-Y2K RTC values]',
'[-o) <[pitch,roll,]heading-offset>] [-k)eep velocities of unpatched ensembles]',
'[keep original -d)ata-source id]',
'<original PD0 file> <patched PD0 file> [external attitude file]');
$opt_p = $opt_r = $opt_h = 1
unless ($opt_p || $opt_r || $opt_h);
$RDI_PD0_IO::OVERRIDE_Y2K_CLOCK = $opt_c;
$LADCP_file = &antsFileArg();
$outPD0 = $ARGV[0]; shift;
#----------------------------------------------------------------------
# Step 1: Read LADCP Data
#----------------------------------------------------------------------
readData($LADCP_file,\%LADCP); # TRDI PD0 file
#----------------------------------------------------------------------
# Step 2: Process External Attidue Input to Patch PD0 file
#----------------------------------------------------------------------
my($pr_missing,$hdg_missing) = (0,0);
&antsIn(); # load first IMP record
my($ensF) = &fnr('LADCP_ens');
my($pitchF) = &fnr('pitch');
my($rollF) = &fnr('roll');
my($hdgF) = &fnr('hdg');
my($LADCP_pitch_mean) = &antsRequireParam('LADCP_pitch.mu');
my($LADCP_roll_mean) = &antsRequireParam('LADCP_roll.mu');
my($pofs,$rofs) = (0,0); # apply externally supplied offset(s)
my($rho,$crho,$srho);
if (defined($opt_o)) {
my($pofs,$rofs,$hofs) = split(/,/,$opt_o);
if (defined($pofs)) { # pitch and roll offsets supplied
croak("$0: cannot decode -o $opt_o\n")
unless numbersp($pofs,$rofs);
} else { # no pitch and roll, only heading
$hofs = $pofs;
$pofs = undef;
}
croak("$0: cannot decode -o $opt_o\n")
unless numberp($hofs);
# set up heading correction
&antsAddParams('IMU_hdg_offset',$P{IMP_hdg_offset}) # backward compatibility
if defined($P{IMP_hdg_offset});
$rho = $hofs - &antsRequireParam('IMU_hdg_offset'); # calculate correction relative to already applied one
$crho = cos(rad($rho));
$srho = sin(rad($rho));
if (defined($pofs)) { # rotate IMP pitch and roll offsets into new LADCP frame
my($IMP_pitch_mean) = &antsRequireParam('IMP_pitch.mu') * $crho
+ &antsRequireParam('IMP_roll.mu') * $srho;
my($IMP_roll_mean) = -&antsRequireParam('IMP_pitch.mu') * $srho
+ &antsRequireParam('IMP_roll.mu') * $crho;
$LADCP_pitch_mean = $IMP_pitch_mean - $pofs; # apply externally supplied offsets
$LADCP_roll_mean = $IMP_roll_mean - $rofs;
}
}
do {
my($ens) = $P{RECNO};
die("assertion failed [$ants_[0][$ensF] != $LADCP{ENSEMBLE}[$ens]->{NUMBER} --- 1-$LADCP{ENSEMBLE}[0]->{NUMBER} + $P{RECNO} + $d]")
unless ($ants_[0][$ensF] == $LADCP{ENSEMBLE}[$ens]->{NUMBER});
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} = 0xA0;
if (numbersp($ants_[0][$pitchF],$ants_[0][$rollF])) { # valid IMP data -> patch LADCP ensemble
if (defined($opt_o)) { # -o set: rotate pitch and roll into correct coordinates
my($rot_p) = ($ants_[$r][$pitchF] * $crho +
$ants_[$r][$rollF] * $srho);
my($rot_r) = (-$ants_[$r][$pitchF] * $srho +
$ants_[$r][$rollF] * $crho);
$ants_[$r][$pitchF] = $rot_p;
$ants_[$r][$rollF] = $rot_r;
}
if ($opt_p) { # patch pitch
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_p<<2);
$LADCP{ENSEMBLE}[$ens]->{PITCH} = RDI_pitch($LADCP_pitch_mean + $ants_[0][$pitchF],
$LADCP_roll_mean + $ants_[0][$rollF]);
}
if ($opt_r) { # patch roll
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= ($opt_r<<1);
$LADCP{ENSEMBLE}[$ens]->{ROLL} = $LADCP_roll_mean + $ants_[0][$rollF];
}
} else { # no valid IMP pitch and roll => invalidate LADCP data
$pr_missing++;
unless ($opt_k) {
clearEns(\%LADCP,$ens);
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
}
}
if (numberp($ants_[0][$hdgF])) { # valid IMP heading
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID} |= $opt_h;
if (defined($opt_o)) { # apply offset on -o; otherwise, data are correctly rotated
$ants_[0][$hdgF] -= $rho;
$ants_[0][$hdgF] += 360 if ($ants_[0][$hdgF] < 0);
}
$LADCP{ENSEMBLE}[$ens]->{HEADING} = $ants_[0][$hdgF] # patch heading
if $opt_h;
} else { # no valid IMP heading => invalidate LADCP data
$hdg_missing++;
unless ($opt_k) {
clearEns(\%LADCP,$ens);
$LADCP{ENSEMBLE}[$ens]->{DATA_SOURCE_ID}= 0xA0;
}
}
} while (&antsIn());
$LADCP{ENSEMBLE}[0]->{DATA_SOURCE_ID} = 0x7F; # ensure correct DSID (1st ens: orig; 2nd ens: this prog)
$LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID} = 0xA0
unless ($LADCP{ENSEMBLE}[1]->{DATA_SOURCE_ID}&0xF0 == 0xA0);
writeData($outPD0,\%LADCP); # write new PD0
my($verb) = $opt_k ? 'retained' : 'cleared';
printf(STDERR "$outPD0: %d pitch/roll & %d heading values $verb\n",$pr_missing,$hdg_missing)
if ($pr_missing+$hdg_missing);
exit(0);