Generated: Tue Feb 2 17:54:41 2010 from gps02.pl 2005/07/08 24.1 KB.
#!/perl # nmea.perl # Convert generic NMEA output into something more spreadsheet-friendly. # Understands some Garmin-specific sentences. # # Note: we depend on the $GPRMC sentence appearing once in every # "data burst" from the GPS, preferably as the first sentence. # If this is not true for a particular GPS, the code that calls # output() must be moved to a more appropriate place. # # Dave Martindale, May 2000 # Geoff McLane, July 2005 # The original (beatifully FULL) output MUST be kept, but usually # we are interested in only a FEW of the values ... # Assumtions # WGS84 datum # The range of the visible screen-coordinates is x = 0 (left) # to x = 1350 (right) and y = 0 (bottom) to y = 1000 (top): # x,y: top left 0,1000 top right 1350,1000 # bottom left 0,0 bottom right 1350,0 $maxxwid = 1350; $maxyht = 1000; #uses $SGD_PI = 3.14159265358979323846; #/* From M_PI under Linux/X86 */ $dbg2 = 1; $dbg3 = 0; $outhtml = 'tempgps1.htm'; %sentlist = (); # list, and count, of sentences found ... @ll_list = (); # list of lat,lon $ll_grp = 3; $maxtrack = 12; $dbgon = 1; # 0 for runtime $actdist = 0; $actazim = 0; if ($dbgon == 1) { $infile = 'gps01.txt'; $outfile = 'tempgps4.csv'; } else { $infile = shift or die "No input file given ...\n"; $outfile = shift or die "No output file given ...\n"; } ###$infile = "<-"; ###$outfile = ">-"; ###open (INFILE, $infile) or die "couldn't open input file $infile\n"; ###open (OUTFILE, $outfile) or die "couldn't open output file $outfile\n"; open (INFILE, "<$infile") or die "couldn't open input file $infile\n"; open (OUTFILE, ">$outfile") or die "couldn't open output file $outfile\n"; print "Running ...gps02.pl ...\n"; print OUTFILE "GPRMC,,,,,,,,"; print OUTFILE "GPGGA,,,,,,,,,,"; print OUTFILE "GPGSA,,,,,,,,,,,,,,,,,"; print OUTFILE "GPGSV,", "," x (4 * $maxtrack); print OUTFILE "GPVTG,,,,"; print OUTFILE "GPGLL,,,,"; print OUTFILE "PGRME,,,"; print OUTFILE "PGRMZ,,"; print OUTFILE "PGRMM"; print OUTFILE "\n"; print OUTFILE "Time,Valid,Lat,Long,SOG,CMG,Date,MVar,"; # GPRMC print OUTFILE "Time,Lat,Long,Qual,Sats,HDOP,Alt,GeoH,DGPSAge,DGPSID,"; # GPGGA print OUTFILE "Auto,Dim,PRN,,,,,,,,,,,,PDOP,HDOP,VDOP,"; # GPGSA print OUTFILE "View,"; # GPGSV for ($sat = 1; $sat <= $maxtrack; $sat++) { print OUTFILE "PRN ", $sat, ",Elev,Azim,S/N,"; } print OUTFILE "CRS (T),CRS (M),SOG (Kt),SOG (km/h),"; # GPVTG print OUTFILE "Lat,Long,Time,Valid,"; # GPGLL print OUTFILE "HPE,VPE,SPE,"; # PGRME print OUTFILE "Alt,Dim,"; # PGRMZ print OUTFILE "Datum"; # PGRMM print OUTFILE "\n"; $outgpsline = 0; $invcnt = 0; # invalid sentences $currhr = 25; # invalid hour $currmin = 60; # invalid minute $currsec = 60; # invalid second $currposlaty = 0; $currposlonx = 0; $sentcnt = 0; $minlat = 90; $maxlat = -90; $maxlon = -180; $minlon = 180; sub do_file_output { print OUTFILE $time_rmc, ',', $ok_rmc, ',', $lat_rmc, ',', $long_rmc, ',', $speed, ',', $cmg, ',', $date, ',', $mvar, ','; print OUTFILE $time_gga, ',', $lat_gga, ',', $long_gga, ',', $fixqual, ',', $nsat, ',', $hdop_gga, ',', $alt_gga, ',', $gheight, ',', $DGPS_age, ',', $DGPS_ID, ','; print OUTFILE $fixsel, ',', $fixdeg, ',', join(',', @satsused), ',', $pdop, ',', $hdop, ',', $vdop, ','; print OUTFILE $nsiv, ','; for ($sat = 0; $sat < $maxtrack; $sat++) { if ($sat < $nsiv) { print OUTFILE $prn[$sat], ',', $elev[$sat], ',', $azim[$sat], ',', $signal[$sat], ','; } else { print OUTFILE ",,,,"; } } print OUTFILE $crs_t, ',', $crs_m, ',', $sog_kt, ',', $sog_km, ','; print OUTFILE $lat_gll, ',', $long_gll, ',', $time_gll, ',', $ok_gll, ','; print OUTFILE $hpe, ',', $vpe, ',', $epe, ','; print OUTFILE $alt_rmz, ',', $alt_type, ','; print OUTFILE $datum; print OUTFILE "\n"; $outgpsline++; } sub output { do_file_output(); if ($ok_rmc eq 'A') { # valid record # print OUTFILE $time_rmc, ',', $ok_rmc, ',', $lat_rmc, ',', # $long_rmc, ',', $speed, ',', $cmg, ',', $date, ',', # $mvar, ','; # $time_rmc = '13:02:10' HH:MM:SS format my (@hms) = split ':', $time_rmc; # split TIME # $date = 26/07/05 # split DATE my (@dte) = split '/', $date; # split date $currhr = $hms[0]; $currmin = $hms[1]; $currsec = $hms[2]; $currday = $dte[0]; $currmth = $dte[1]; $curryr = $dte[2]; if( ( ($currhr >= 0) && ($currhr < 24) ) && ( ($currmin >= 0) && ($currmin < 60) ) && ( ($currsec >= 0) && ($currsec < 60) ) ) { $currtimed = "$currhr:$currmin:$currsec"; $currdated = "$curryr/$currmth/$currday"; $dispdt = "$currdated $currtimed"; # got a VALID time my (@ll) = ($lat_rmc, $long_rmc, $alt_gga); push(@ll_list, @ll); if (($currposlaty != $lat_rmc ) || ($currposlonx != $long_rmc) ) { # position CHANGED, but by HOW MUCH? $mets = -1; if ($sentcnt) { $mets = dist_m ( 10, $lat_rmc, $long_rmc, $currposlaty, $currposlonx ); } #print $currhr, ":", $currmin, ":", $currsec, #print $dispdt, print $currtimed, " at lat ", $lat_rmc, " long ", $long_rmc, " $mets meters ($actdist)\n"; $currposlaty = $lat_rmc; $currposlonx = $long_rmc; if ($currposlaty < $minlat) { $minlat = $currposlaty } if ($currposlaty > $maxlat) { $maxlat = $currposlaty } if ($currposlonx < $minlon) { $minlon = $currposlonx } if ($currposlonx > $maxlon) { $maxlon = $currposlonx } $sentcnt++; } else { # repeating SAME POSITION print "SAME POSITION: $currhr", ":", $currmin, ":", $currsec, "\n"; } } else { print "OUT-OF-TIME: ", $currhr, ":", $currmin, ":", $currsec, "\n"; } } else { $invcnt++; } } # @_ = (3352.3955,S) == -33 52.3955 == -33.8732583333333 sub latitude { my ($deg, $min) = unpack "a2a*", $_[0]; my $lat = $deg + $min / 60; $lat = - $lat if $_[1] =~ /[Ss]/; return $lat; } # @_ = (15113.3678,E) == +151 13.6678 == 151.222796666667 sub longitude { my ($deg, $min) = unpack "a3a*", $_[0]; my $long = $deg + $min / 60; $long = - $long if $_[1] =~ /[Ww]/; return $long; } sub add_2_list { my ($tx) = @_; if (exists $sentlist{$tx} ) { # list, and count, of sentences found ... $sentlist{$tx}++; # bump the count } else { $sentlist{$tx} = 1; if ($dbgon > 1) { print "$tx\n"; } } } $first = 1; $linecnt = 0; while ($line = <INFILE>) { $linecnt++; # count each line .... chomp($line); @field = split /[,*]/, $line; # split on comma, and * (checksum) $csent = $field[0]; # get this sentence if (length($csent) == 6) { # NMEA length, at least if (substr($csent,0,1) eq '$') { add_2_list ( $csent ); # keep list of sentence type found } else { print "What is this? [$csent] of ", length($csent), " bytes\n"; } } SWITCH: { # recommended minimum specific GPS/Transit data if ($field[0] eq '$GPRMC') { # We don't know exactly what sentences to expect # from an arbitrary GPS, but we assume that RMC # will always be included. Each time we see this # sentence, print out the accumulated information # from the previous burst. output() unless ($first) ; $first = 0; # Now process the new RMC record - example ### $GPRMC,130210.030,A,3352.3955,S,15113.3678,E,0.257256,125.59,260605,,*10 $time_rmc = join ':', unpack "a2" x 3, $field[1]; $ok_rmc = $field[2]; $lat_rmc = latitude(@field[3..4]); $long_rmc = longitude(@field[5..6]); $speed = $field[7]; $cmg = $field[8]; $date = join '/', unpack "a2" x 3, $field[9]; $mvar = $field[10] . $field[11]; # field[12] is checksum last SWITCH; } # GPS fix data if ($field[0] eq '$GPGGA') { $time_gga = join ':', unpack "a2" x 3, $field[1]; $lat_gga = latitude(@field[2..3]); $long_gga = longitude(@field[4..5]); $fixqual = $field[6]; $nsat = $field[7]; $hdop_gga = $field[8]; $alt_gga = $field[9]; # $field[10] is altitude units (always M) $gheight = $field[11]; # $field[12] is geoid height units (always M) $DGPS_age = $field[13]; $DGPS_ID = $field[14]; # field[15] is checksum; last SWITCH; } # GPS DOP and active satellites if ($field[0] eq '$GPGSA') { $fixsel = $field[1]; # A for auto selection, M for manual $fixdeg = $field[2]; # 2 or 3 for 2D or 3D fix @satsused = @field[3..14]; $pdop = $field[15]; $hdop = $field[16]; $vdop = $field[17]; # field[18] is checksum; last SWITCH; } # satellites in view if ($field[0] eq '$GPGSV') { # $field[1] is total number of sentences for full data # $field[2] is current sentence number $nsiv = $field[3]; # # Unpack next 16 fields as PRN, elevation, azimuth, and # signal quality for next 4 satellites # $sat = ($field[2] - 1) * 4; $lim = $sat + 4; if ($nsiv < $lim) { $lim = $nsiv; } $f = 4; while ($sat < $lim) { $prn[$sat] = $field[$f++]; $elev[$sat] = $field[$f++]; $azim[$sat] = $field[$f++]; $sig = $field[$f++]; $sig = '' if $sig == 0; $signal[$sat] = $sig; $sat++; } # field[20] is checksum; last SWITCH; } # track/ground speed if ($field[0] eq '$GPVTG') { $crs_t = $field[1]; # $field[2] is reference (always T); $crs_m = $field[3]; # $field[4] is reference (always M); $sog_kt = $field[5]; # $field[6] is units (always N); $sog_km = $field[7]; # $field[8] is units (always K); # $field[9] is checksum; last SWITCH; } # position error info if ($field[0] eq '$PGRME') { $hpe = $field[1]; # $field[2] is units (always M) $vpe = $field[3]; # $field[4] is units (always M) $epe = $field[5]; # $field[6] is units (always M) # field[7] is checksum; last SWITCH; } # latitude/longitude if ($field[0] eq '$GPGLL') { $lat_gll = latitude(@field[1..2]); $long_gll = longitude(@field[3..4]); $time_gll = join ':', unpack "a2" x 3, $field[5]; $ok_gll = $field[6]; # field[7] is checksum; last SWITCH; } # altitude if ($field[0] eq '$PGRMZ') { $alt_rmz = $field[1]; # $field[2] is units (always F) $alt_type = $field[3]; # 2: user alt, 3: 3D fix # field[4] is checksum; last SWITCH; } # datum if ($field[0] eq '$PGRMM') { $datum = $field[1]; # field[2] is checksum; last SWITCH; } } } close INFILE; close OUTFILE; print "min lat $minlat, max lat $maxlat, diff = ", $maxlat - $minlat, " degs.\n"; print "min lon $minlon, max lon $maxlon, diff = ", $maxlon - $minlon, " degs.\n"; print "Processed $linecnt lines, in file $infile ...\n"; print "Wrote $outgpsline GPS lines to $outfile ...\n"; print "Got ", scalar @ll_list, " in list ...\n"; foreach $key (keys %sentlist) { print $key, " ", $sentlist{$key}, "\n"; } do_html_file(); if ($dbgon > 1) { system($outfile) ### = 'tempgps1.csv'; } system($outhtml); #// from #// ********************************************************************* #// sg_geodesy.cxx -- routines to convert between geodetic and geocentric #// coordinate systems. #// #// Copied and adapted directly from LaRCsim/ls_geodesy.c #// #// See below for the complete original LaRCsim comments. #// #// $Id: sg_geodesy.cxx,v 1.2 2002/12/31 14:47:36 david Exp $ #// #// Direct and inverse distance functions #// #// Proceedings of the 7th International Symposium on Geodetic #// Computations, 1985 #// #// "The Nested Coefficient Method for Accurate Solutions of Direct and #// Inverse Geodetic Problems With Any Length" #// #// Zhang Xue-Lian #// pp 747-763 #// #// modified for FlightGear to use WGS84 only -- Norman Vine # ###$GEOD_INV_PI = $SGD_PI; # = 3.14159265358979323846; #/* From M_PI under Linux/X86 */ #// s == distance #// az = azimuth #// for WGS_84 a = 6378137.000, rf = 298.257223563; #static double M0( double e2 ) { sub M0 { my ($e2) = @_; #//double e4 = e2*e2; #return GEOD_INV_PI*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + # e2*(5.0/256.0) )))/2.0; return (3.14159265358979323846*(1.0 - e2*( 1.0/4.0 + e2*( 3.0/64.0 + e2*(5.0/256.0) )))/2.0); } #// given, alt, lat1, lon1, az1 and distance (s), calculate lat2, lon2 #// and az2. Lat, lon, and azimuth are in degrees. distance in meters #int geo_direct_wgs_84( double alt, double lat1, double lon1, double az1, # double s, double *lat2, double *lon2, double *az2 ) $ret_lat2 = 0; $ret_lon2 = 0; $ret_az2 = 0; sub geo_direct_wgs_84 { #($alt, $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2) = @_; ($alt, $lat1, $lon1, $az1, $s) = @_; $a = 6378137.000; $rf = 298.257223563; $RADDEG = (GEOD_INV_PI)/180.0; $testv = 1.0E-10; $f = ( $rf > 0.0 ? 1.0/$rf : 0.0 ); $b = $a*(1.0-$f); $e2 = $f*(2.0-$f); $phi1 = $lat1*$RADDEG; $lam1 = $lon1*$RADDEG; $sinphi1 = sin($phi1); $cosphi1 = cos($phi1); $azm1 = $az1*$RADDEG; $sinaz1 = sin($azm1); $cosaz1 = cos($azm1); ###if( fabs($s) < 0.01 ) { ###// distance < centimeter => congruency if( abs($s) < 0.01 ) { ###// distance < centimeter => congruency ##if( $s < 0.01 ) { ###// distance < centimeter => congruency $ret_lat2 = $lat1; $ret_lon2 = $lon1; $ret_az2 = 180.0 + $az1; if( $ret_az2 > 360.0 ) { $ret_az2 -= 360.0 }; return 0; } elsif( $cosphi1 ) { #// non-polar origin #// u1 is reduced latitude $tanu1 = sqrt(1.0-$e2)*$sinphi1/$cosphi1; $sig1 = atan2($tanu1,$cosaz1); $cosu1 = 1.0/sqrt( 1.0 + $tanu1*$tanu1 ); $sinu1 = $tanu1*$cosu1; $sinaz = $cosu1*$sinaz1; $cos2saz = 1.0-$sinaz*$sinaz; $us = $cos2saz*$e2/(1.0-$e2); #// Terms $ta = 1.0+$us*(4096.0+$us*(-768.0+$us*(320.0-175.0*$us)))/16384.0; $tb = $us*(256.0+$us*(-128.0+$us*(74.0-47.0*$us)))/1024.0; $tc = 0; #// FIRST ESTIMATE OF SIGMA (SIG) $first = $s/($b*$ta); ###// !! $sig = $first; $c2sigm = 0; $sinsig = 0; $cossig = 0; $temp = 0; $denom = 0; $rnumer = 0; $dlams = 0; $dlam = 0; do { $c2sigm = cos(2.0*$sig1+$sig); $sinsig = sin($sig); $cossig = cos($sig); $temp = $sig; $sig = $first + $tb*$sinsig*($c2sigm+$tb*($cossig*(-1.0+2.0*$c2sigm*$c2sigm) - $tb*$c2sigm*(-3.0+4.0*$sinsig*$sinsig) *(-3.0+4.0*$c2sigm*$c2sigm)/6.0) /4.0); } while( abs($sig-$temp) > $testv); #// LATITUDE OF POINT 2 #// DENOMINATOR IN 2 PARTS (TEMP ALSO USED LATER) $temp = $sinu1*$sinsig-$cosu1*$cossig*$cosaz1; $denom = (1.0-$f)*sqrt($sinaz*$sinaz+$temp*$temp); #// NUMERATOR $rnumer = $sinu1*$cossig+$cosu1*$sinsig*$cosaz1; $ret_lat2 = atan2($rnumer,$denom)/$RADDEG; #// DIFFERENCE IN LONGITUDE ON AUXILARY SPHERE (DLAMS ) $rnumer = $sinsig*$sinaz1; $denom = $cosu1*$cossig-$sinu1*$sinsig*$cosaz1; $dlams = atan2($rnumer,$denom); #// TERM C $tc = $f*$cos2saz*(4.0+$f*(4.0-3.0*$cos2saz))/16.0; #// DIFFERENCE IN LONGITUDE $dlam = $dlams-(1.0-$tc)*$f*$sinaz*($sig+$tc*$sinsig* ($c2sigm+ $tc*$cossig*(-1.0+2.0* $c2sigm*$c2sigm))); $ret_lon2 = ($lam1+$dlam)/$RADDEG; if ($ret_lon2 > 180.0 ) { $ret_lon2 -= 360.0; } if ($ret_lon2 < -180.0 ) { $ret_lon2 += 360.0; } #// AZIMUTH - FROM NORTH $ret_az2 = atan2(-$sinaz,$temp)/$RADDEG; if ( abs($ret_az2) < $testv ) { $ret_az2 = 0.0; } if( $ret_az2 < 0.0) { $ret_az2 += 360.0; } return 0; } else { # // phi1 == 90 degrees, polar origin $dM = $a*M0($e2) - $s; $paz = ( $phi1 < 0.0 ? 180.0 : 0.0 ); ###return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM,lat2,lon2,az2 ); return geo_direct_wgs_84( alt, 0.0, lon1, paz, dM ); } } #// These are hard numbers from the WGS84 standard. DON'T MODIFY #// unless you want to change the datum. $EQURAD = 6378137; $iFLATTENING = 298.257223563; ###$GEOD_INV_PI = 3.14159265358979323846; #/* From M_PI under Linux/X86 */ #// given alt, lat1, lon1, lat2, lon2, calculate starting and ending #// az1, az2 and distance (s). Lat, lon, and azimuth are in degrees. #// distance in meters #int geo_inverse_wgs_84( double alt, double lat1, # double lon1, double lat2, # double lon2, double *az1, double *az2, # double *s ) $r_az1 = 0; $r_az2 = 0; $r_s = 0; sub geo_inverse_wgs_84 { ( $alt, $lat1, $lon1, $lat2, $lon2 ) = @_; $a = 6378137; # was $EQURAD $rf = 298.257223563; # was $iFLATTENING $iter=0; ### $RADDEG = ($GEOD_INV_PI)/180.0; $RADDEG = (3.14159265358979323846)/180.0; $testv = 1.0E-10; $f = ( $rf > 0.0 ? 1.0/$rf : 0.0 ); $b = $a*(1.0-$f); #// double e2 = f*(2.0-f); // unused in this routine $phi1 = $lat1*$RADDEG; $lam1 = $lon1*$RADDEG; $sinphi1 = sin($phi1); $cosphi1 = cos($phi1); $phi2 = $lat2*$RADDEG; $lam2 = $lon2*$RADDEG; $sinphi2 = sin($phi2); $cosphi2 = cos($phi2); if ($dbg3) { print "geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat2, $lon2 )\n"; print "Div by square of b=$b derived from a=$a ...\n"; print "RADDEG=$RADDEG is equal GEOD_INV_PI=",$RADDEG * 180.0, " div by 180 ...\n"; print "lam1=$lam1 and lam2=$lam2 ...\n"; print 'very small value =', $testv, "\n"; } if( (abs($lat1-$lat2) < $testv && ( abs($lon1-$lon2) < $testv) || abs($lat1-90.0) < $testv ) ) { #// TWO STATIONS ARE IDENTICAL : SET DISTANCE & AZIMUTHS TO ZERO */ $r_az1 = 0.0; $r_az2 = 0.0; $r_s = 0.0; print "geo_inverse_wgs_84 (IDENTICAL STATION)\n" if $dbg3; return 0; } elsif( abs($cosphi1) < $testv ) { #// initial point is polar #$k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1, az1,az2,s ); print "doing geo_inverse_wgs_84 ( $alt, $lat2, $lon2, $lat1, $lon1 )\n" if $dbg3; $k = geo_inverse_wgs_84( alt, lat2,lon2,lat1,lon1 ); #$k = $k; #// avoid compiler error since return result is unused $b = $r_az1; $r_az1 = $r_az2; $r_az2 = $b; return 0; } elsif( abs($cosphi2) < $testv ) { #// terminal point is polar $rr_lon1 = $lon1 + 180.0; #$k = geo_inverse_wgs_84( alt, lat1, lon1, lat1, _lon1, # az1, az2, s ); print "doing geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat1, $rr_lon1 )\n" if $dbg3; $k = geo_inverse_wgs_84( $alt, $lat1, $lon1, $lat1, $rr_lon1 ); #k = k; // avoid compiler error since return result is unused $r_s /= 2.0; $r_az2 = $r_az1 + 180.0; if( $r_az2 > 360.0 ) { $r_az2 -= 360.0; } return 0; } elsif( (abs( abs($lon1-$lon2) - 180 ) < $testv) && (abs($lat1+$lat2) < $testv) ) { print "Geodesic passes through the pole (antipodal) ...\n" if $dbg3; $s1 = 0; $s2 = 0; #geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2, az1,az2, &s1 ); #geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2, az1,az2, &s2 ); print "doing geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat1, $lon2 )\n" if $dbg3; geo_inverse_wgs_84( alt, lat1,lon1, lat1,lon2 ); $s1 = $r_s; print "doing geo_inverse_wgs_84 ( $alt, $lat2, $lon2, $lat1, $lon2 )\n" if $dbg3; geo_inverse_wgs_84( alt, lat2,lon2, lat1,lon2 ); $s2 = $r_s; $r_az2 = $r_az1; $r_s = $s1 + $s2; return 0; } else { print "antipodal and polar points don't get here ...\n" if $dbg3; $dlam = $lam2 - $lam1; $dlams = $dlam; $sdlams = 0; $cdlams = 0; $sig = 0; $sinsig = 0; $cossig = 0; $sinaz = 0; $cos2saz = 0; $c2sigm = 0; $tc = 0; $temp = 0; $us = 0; $rnumer = 0; $denom = 0; $ta = 0; $tb = 0; $cosu1 = 0; $sinu1 = 0; $sinu2 = 0; $cosu2 = 0; #// Reduced latitudes $temp = (1.0-$f)*$sinphi1/$cosphi1; $cosu1 = 1.0/sqrt(1.0+$temp*$temp); $sinu1 = $temp*$cosu1; $temp = (1.0-$f)*$sinphi2/$cosphi2; $cosu2 = 1.0/sqrt(1.0+$temp*$temp); $sinu2 = $temp*$cosu2; do { $sdlams = sin($dlams); $cdlams = cos($dlams); if ($dbg3) { print "part1 = sq $cosu2 times sq $sdlams\n"; $part1 = $cosu2*$cosu2*$sdlams*$sdlams; # plus $part2 = ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams); # times $part3 = ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams); print "got part1=$part1 + ( part2=$part2 * part3=$part3 )\n"; } $sinsig = sqrt($cosu2*$cosu2*$sdlams*$sdlams+ ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams)* ($cosu1*$sinu2-$sinu1*$cosu2*$cdlams)); $cossig = $sinu1*$sinu2+$cosu1*$cosu2*$cdlams; print "Got sinsig=$sinsig cosu2=$cosu2 sdlams=$sdlams\n" if $dbg3; $sig = atan2($sinsig,$cossig); $sinaz = $cosu1*$cosu2*$sdlams/$sinsig; $cos2saz = 1.0-$sinaz*$sinaz; $c2sigm = ($sinu1 == 0.0 || $sinu2 == 0.0 ? $cossig : $cossig-2.0*$sinu1*$sinu2/$cos2saz); $tc = $f*$cos2saz*(4.0+$f*(4.0-3.0*$cos2saz))/16.0; $temp = $dlams; $dlams = $dlam+(1.0-$tc)*$f*$sinaz* ($sig+$tc*$sinsig* ($c2sigm+$tc*$cossig*(-1.0+2.0*$c2sigm*$c2sigm))); ###if (abs($dlams) > $GEOD_INV_PI && $iter++ > 50) { if (( abs($dlams) > 3.14159265358979323846 ) && ($iter++ > 50)) { return $iter; } } while ( abs($temp-$dlams) > $testv); print "Div by square of b=$b ...\n" if $dbg3; $us = $cos2saz*($a*$a-$b*$b)/($b*$b); ## // !! #// BACK AZIMUTH FROM NORTH $rnumer = -($cosu1*$sdlams); $denom = $sinu1*$cosu2-$cosu1*$sinu2*$cdlams; $r_az2 = atan2($rnumer,$denom)/$RADDEG; if( abs($r_az2) < $testv ) {$r_az2 = 0.0;} if($r_az2 < 0.0) { $r_az2 += 360.0; } #// FORWARD AZIMUTH FROM NORTH $rnumer = $cosu2*$sdlams; $denom = $cosu1*$sinu2-$sinu1*$cosu2*$cdlams; $r_az1 = atan2($rnumer,$denom)/$RADDEG; if( abs($r_az1) < $testv ) {$r_az1 = 0.0; } if( $r_az1 < 0.0) { $r_az1 += 360.0; } #// Terms a & b $ta = 1.0+$us*(4096.0+$us*(-768.0+$us*(320.0-175.0*$us)))/16384.0; $tb = $us*(256.0+$us*(-128.0+$us*(74.0-47.0*$us)))/1024.0; #// GEODETIC DISTANCE $r_s = $b * $ta * ( $sig - $tb * $sinsig * ($c2sigm + $tb * ( $cossig * ( -1.0 + 2.0 * $c2sigm * $c2sigm ) - $tb * $c2sigm * ( -3.0 + 4.0 * $sinsig * $sinsig ) * ( -3.0 + 4.0 * $c2sigm * $c2sigm ) / 6.0 ) / 4.0 ) ); return 0; } } sub dist_m { ( $alt, $lat1, $lon1, $lat2, $lon2 ) = @_; geo_inverse_wgs_84 ( $alt, $lat1, $lon1, $lat2, $lon2 ); print "Distance = $r_s metres ...\n" if $dbg3; $az = (int(($r_az1 + 0.05) * 10) /10); $s = (int(($r_s + 0.05) * 10) /10); $actdist = $s; $actazim = $az; ###return ($r_s . '@' . $r_az1); return ($s . '@' . $az); } sub html_head { my ($fh, $hdr) = @_; print $fh <<"EOF"; <html> <head> <title>$hdr</title> </head> <body> <h1 align="center">$hdr</h1> EOF } sub new_row { ($tx) = @_; return "<tr>$tx</tr>\n"; } sub in_col { ($tx) = @_; return "<td>$tx</td>"; } sub in_colb { ($tx) = @_; return in_col("<b>$tx</b>"); } $totlat = 0; $totlon = 0; $totalt = 0; $avlat = 0; $avlon = 0; $avalt = 0; $clat = 0; $clon = 0; $calt = 0; $llat = 0; $llon = 0; $lalt = 0; $actdist = 0; $actazim = 0; $OF = 0; sub do_html_file { ## OUPUT HTML open ($OF, ">$outhtml") or die "warning: couldn't open output file [$outhtml]!\n"; html_head ( \*$OF, $outhtml ); print $OF "<center>\n"; print $OF "<table border=\"1\">\n"; for ($i = 0; $i < $outgpsline; $i++) { $i2 = $i * $ll_grp; $clat = $ll_list[$i2]; $clon = $ll_list[$i2+1]; $calt = $ll_list[$i2+2]; $totlat += $clat; $totlon += $clon; $totalt += $calt; #print OF new_row( in_col($clat) . in_col($clon) . in_col($calt) ); #print "lat $ll_list[$i2], lon $ll_list[$i2+1]"; print "lat $clat, lon $clon, alt $calt"; $mets = 0; if ($i) { $mets = dist_m ( 10, $clat, $clon, $llat, $llon ); ###print " at $mets m."; print " at $actdist m."; } print "\n"; ###print OF new_row( in_col($clat) . in_col($clon) . in_col($calt) . in_col($mets) ); print $OF new_row( in_col($clat) . in_col($clon) . in_col($calt) . in_col($actdist) . in_col($actazim) ); $avlat = (($avlat * $i) + $clat) / ($i + 1); $avlon = (($avlon * $i) + $clon) / ($i + 1); $avalt = (($avalt * $i) + $calt) / ($i + 1); $llat = $clat; $llon = $clon; $lalt = $calt; } print $OF new_row( in_colb($totlat / $i) . in_colb($totlon / $i) . in_colb($totalt / $i) . in_colb($actdist) . in_colb($actazim) ); print $OF new_row( in_colb($avlat) . in_colb($avlon) . in_colb($avalt) . in_colb($actdist) . in_colb($actazim) ); print $OF "</table>\n"; print $OF "</center>\n"; print $OF "</body>\n"; print $OF "</html>\n"; close $OF; } ## done HTML file #eof