Mercurial Mercurial > cgi-bin > ladcp-cgi-bin > hgweb.cgi > ADCP_tools / comparison
summary | shortlog | changelog | graph | tags | bookmarks | branches | files | changeset | file | latest | revisions | annotate | diff | comparison | raw | help
Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
RDI_Coords.pl
changeset 32 7155adf61d77
parent 31 b6ca27a1d19c
child 34 3b4bcd55e1ea
equal deleted inserted replaced
31:b6ca27a1d19c 32:7155adf61d77 
     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: Sun Jan 31 12:42:43 2016
 
     4 #                    dlm: Mon Feb 29 18:03:31 2016
 
     5 #                    (c) 2003 A.M. Thurnherr
 
     5 #                    (c) 2003 A.M. Thurnherr
 
     6 #                    uE-Info: 185 0 NIL 0 0 72 0 2 4 NIL ofnI
 
     6 #                    uE-Info: 44 85 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:
 
    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 #	Jan  9, 2016: - added &velEarthToBeam(), &velBeamToEarth()
 
       
    44 #	Feb 29, 2016: - debugged & verified velEarthToInstrument(), velInstrumentToBeam()
 
       
    45 #				  - added velBeamToEarth()
 
    44 
    46 
    45 use strict;
 
    47 use strict;
 
    46 use POSIX;
 
    48 use POSIX;
 
    47 
    49 
    48 my($PI) = 3.14159265358979;
 
    50 my($PI) = 3.14159265358979;
 
   155 				  $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],
 
   157 				  $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],
 
   156 				  $v4);
 
   158 				  $v4);
 
   157 	}
 
   159 	}
 
   158 } # STATIC SCOPE
 
   160 } # STATIC SCOPE
 
   159 
   161 
       
   162 
       
   163 sub velBeamToEarth(@)
 
       
   164 {
 
       
   165 	my($dtaR,$e,@v) = @_;
 
       
   166 	return velInstrumentToEarth($dtaR,$e,velBeamToInstrument($dtaR,@v));
 
       
   167 }
 
       
   168 
   160 #----------------------------------------------------------------------
 
   169 #----------------------------------------------------------------------
 
   161 # velEarthToInstrument() transforms earth to instrument coordinates
 
   170 # velEarthToInstrument() transforms earth to instrument coordinates
 
   162 #	- based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual)
 
   171 #	- based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual)
 
       
   172 #	- code was verified for both down- and uplookers
 
   163 #	- missing heading data (IMP) causes undef beam velocities
 
   173 #	- missing heading data (IMP) causes undef beam velocities
 
   164 #----------------------------------------------------------------------
 
   174 #----------------------------------------------------------------------
 
   165 
   175 
   166 { # STATIC SCOPE
 
   176 { # STATIC SCOPE
 
   167 	my($hdg,$pitch,$roll,@E2I);
 
   177 	my($hdg,$pitch,$roll,@E2I);
 
   168 
   178 
   169 	sub velEarthToInstrument(@)
 
   179 	sub velEarthToInstrument(@)
 
   170 	{
 
   180 	{
 
   171 		my($dta,$ens,$u,$v,$w,$ev) = @_;
 
   181 		my($dta,$ens,$u,$v,$w,$ev) = @_;
 
   172 
   182 
   173 		unless (@E2I) {
 
   183 		unless (@E2I &&
 
       
   184 				$pitch == $dta->{ENSEMBLE}[$ens]->{PITCH} &&
 
       
   185 				$roll  == $dta->{ENSEMBLE}[$ens]->{ROLL}) {
 
   174 			$hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS} 
   186 			$hdg = $dta->{ENSEMBLE}[$ens]->{HEADING} - $dta->{HEADING_BIAS} 
   175 				if defined($dta->{ENSEMBLE}[$ens]->{HEADING});
 
   187 				if defined($dta->{ENSEMBLE}[$ens]->{HEADING});
 
   176 			$pitch = $dta->{ENSEMBLE}[$ens]->{PITCH};
 
   188 			$pitch = $dta->{ENSEMBLE}[$ens]->{PITCH};
 
   177 			$roll  = $dta->{ENSEMBLE}[$ens]->{ROLL};
 
   189 			$roll  = $dta->{ENSEMBLE}[$ens]->{ROLL};
 
   178 			my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));
 
   190 			my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));
 
       
   191 			my($useRoll) = ($dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) ? $roll+180 : $roll;
 
   179 			my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))
 
   192 			my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))
 
   180 				if defined($hdg);				
   193 				if defined($hdg);				
   181 			my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch));
 
   194 			my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch));
 
   182 			my($sr,$cr) = (sin(rad($roll)),	cos(rad($roll)));
 
   195 			my($sr,$cr) = (sin(rad($useRoll)),	  cos(rad($useRoll)));
 
   183 			@E2I = $dta->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}
 
   196 			@E2I = ([$ch*$cr+$sh*$sp*$sr,	 $ch*$sp*$sr-$sh*$cr,	-$cp*$sr],		# M^-1 = R^-1 * P^-1 * R^-1
 
   184 				 ? (
 
   197 				    [$sh*$cp,				 $ch*$cp,				$sp	],
 
   185 					[$ch*-$cr+$sh*$sp*-$sr,	 $ch*$sp*-$sr-$sh*-$cr,	$cp*-$sr],
 
   198 				    [$ch*$sr-$sh*$sp*$cr,	-$sh*$sr-$ch*$sp*$cr,	$cp*$cr]);
 
   186 				    [$sh*$cp,				 $ch*$cp,				$sp		],
 
       
   187 				    [$ch*-$sr-$sh*$sp*-$cr,	-$sh*-$sr-$ch*$sp*-$cr,	$cp*-$cr]
 
       
   188 				 ) : (
 
       
   189 					[$ch*$cr+$sh*$sp*$sr,	 $ch*$sp*$sr-$sh*$cr,	$cp*$sr	],
 
       
   190 				    [$sh*$cp,				 $ch*$cp,				$sp		],
 
       
   191 				    [$ch*$sr-$sh*$sp*$cr,	-$sh*$sr-$ch*$sp*$cr,	$cp*$cr	]
 
       
   192 				 );
 
       
   193 		}
 
   199 		}
 
   194 
   200 
   195 		return defined($dta->{ENSEMBLE}[$ens]->{HEADING})
 
   201 		return defined($dta->{ENSEMBLE}[$ens]->{HEADING})
 
   196 			   ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2],
 
   202 			   ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2],
 
   197 				  $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2],
 
   203 				  $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2],
ADCP_tools