Generated: Sun Aug 21 11:11:05 2011 from gps03.pl 2010/12/21 36.7 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 = 'tempgps3.htm'; @ll_list = (); # list of lat,lon $ll_grp = 5; ### 3; $maxtrack = 12; $dbgon = 1; # 0 for runtime $actdist = 0; $actazim = 0; # top left $s_min_lon = 151.222; $s_max_lat = -33.874; $s_deg_wid = 0.002; $s_deg_ht = 0.002; ###$s_width = 600; # pixel width ###$s_height = 400; # pixel height $s_width = 600; # pixel width $s_height = 300; # pixel height $s_dpp_x = ($s_width / $s_deg_wid); $s_dpp_y = ($s_height / $s_deg_ht ); $s_white = 'white.gif'; $s_black = 'black.gif'; ###$s_white = 'black.gif'; ###$s_black = 'white.gif'; 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 ...gps03.pl ...\n"; @gp_list = ( "GPRMC,,,,,,,,", "GPGGA,,,,,,,,,,", "GPGSA,,,,,,,,,,,,,,,,,", "GPGSV,,,+4*maxtrack", "GPVTG,,,,", "GPGLL,,,,", "PGRME,,,", "PGRMZ,,", "PGRMM" ); # from : http://www.gpsinformation.org/dale/nmea.htm # There are many sentences in the NMEA standard for all kinds of devices that # may be used in a Marine environment. Some of the ones that have applicability # to gps receivers are listed below: (all message start with GP.) my %GP_sentences = ( 'AAM' => 'Waypoint Arrival Alarm', 'ALM' => 'Almanac data', 'APA' => 'Auto Pilot A sentence', 'APB' => 'Auto Pilot B sentence', 'BOD' => 'Bearing Origin to Destination', 'BWC' => 'Bearing using Great Circle route', 'DTM' => 'Datum being used.', 'GGA' => 'Fix information', 'GLL' => 'Lat/Lon data', 'GRS' => 'GPS Range Residuals', 'GSA' => 'Overall Satellite data', 'GST' => 'GPS Pseudorange Noise Statistics', 'GSV' => 'Detailed Satellite data', 'MSK' => 'send control for a beacon receiver', 'MSS' => 'Beacon receiver status information.', 'RMA' => 'recommended Loran data', 'RMB' => 'recommended navigation data for gps', 'RMC' => 'recommended minimum data for gps', 'RTE' => 'route message', 'TRF' => 'Transit Fix Data', 'STN' => 'Multiple Data ID', 'VBW' => 'dual Ground / Water Spped', 'VTG' => 'Vector track an Speed over the Ground', 'WCV' => 'Waypoint closure velocity (Velocity Made Good)', 'WPL' => 'Waypoint Location information', 'XTC' => 'cross track error', 'XTE' => 'measured cross track error', 'ZTG' => 'Zulu (UTC) time and time to go (to destination)', 'ZDA' => 'Date and Time' ); ##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; $currx = 0; # get_p_x ( $long_rmc ); $curry = 0; # get_p_y ( $lat_rmc ); ### if ($field[0] eq '$GPRMC') { # Each time we see this sentence ### print out the accumulated information from the previous burst. ## gps_output() unless ($gps_first) ; $gps_first = 1; # reset, after FIRST GPS $GPRMC senetence ### $GPRMC,130210.030,A,3352.3955,S,15113.3678,E,0.257256,125.59,260605,,*10 $gps_time_rmc = 0; # join ':', unpack "a2" x 3, $field[1]; $gps_ok_rmc = 'V'; # $field[2]; $gps_lat_rmc = 0.0; # latitude(@field[3..4]); $gps_long_rmc = 0.0; # longitude(@field[5..6]); $gps_speed = 0; # $field[7]; $gps_cmg = 0; # $field[8]; $gps_date = '00/00/0000'; # join '/', unpack "a2" x 3, $field[9]; $gps_mvar = 0; # $field[10] . $field[11]; # field[12] is checksum ###last SWITCH; ##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"; ##} sub output { $outgpsline++; ### 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 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 ); } # pre-calculate (x,y) position $currx = get_p_x ( $long_rmc ); $curry = get_p_y ( $lat_rmc ); my (@ll) = ($lat_rmc, $long_rmc, $alt_gga, $currx, $curry); # note $ll_grp = ?? above push(@ll_list, @ll); #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; } $first = 1; $linecnt = 0; ### always have this set of CURRENT variables ### filled with the MOST recent date,time,lat,lon,... sub gps_output { if ($gps_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 ':', $gps_time_rmc; # split TIME # $date = 26/07/05 # split DATE my (@dte) = split '/', $gps_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) ) ) { # got a VALID time $currtimed = "$currhr:$currmin:$currsec"; $currdated = "$curryr/$currmth/$currday"; $dispdt = "$currdated $currtimed"; $gps_lat_curr = $gps_lat_rmc; # latitude(@field[3..4]); $gps_long_curr = $gps_long_rmc; # longitude(@field[5..6]); } } } sub process_GPRMC_msg { my ($line) = @_; @field = split /[,*]/, $line; # split on comma, and * (checksum) # recommended minimum specific GPS/Transit data if ($field[0] eq '$GPRMC') { # Each time we see this # sentence, print out the accumulated information # from the previous burst. gps_output() unless ($gps_first) ; $gps_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 $gps_time_rmc = join ':', unpack "a2" x 3, $field[1]; $gps_ok_rmc = $field[2]; $gps_lat_rmc = latitude(@field[3..4]); $gps_long_rmc = longitude(@field[5..6]); $gps_speed = $field[7]; $gps_cmg = $field[8]; $gps_date = join '/', unpack "a2" x 3, $field[9]; $gps_mvar = $field[10] . $field[11]; # field[12] is checksum ###last SWITCH; } } ####while ($line = <INFILE>) { sub process_sentence { ($line) = @_; $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 process_GPRMC_msg ( $line ); $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; } } } ### process the LINE OF GPS MESSAGES while ($line = <INFILE>) { process_sentence ($line); } # add the LAST GPS message output() unless ($first) ; 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"; 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 get_p_x { ($lon) = @_; # get logitude, in degrees $x = ($lon - $s_min_lon) * $s_dpp_x; ###$x = eval(($lon - $s_min_lon) * ($s_deg_wid / $s_width)); #print "s_min_lon = $s_min_lon ...\n"; #print "px = $x ( $lon - $s_min_lon ) * $s_dpp_x ...\n"; print "px=" . int($x) . " " . ( $lon - $s_min_lon ) . " * $s_dpp_x ...\n"; return $x; } sub get_p_y { ($lat) = @_; # get latitude, in degrees $y = ($lat - $s_max_lat) * $s_dpp_y; #print "py = $y = ($lat - $s_max_lat) * $s_dpp_y ...\n"; print "py=" . int($y) . " " . ($lat - $s_max_lat) . " * $s_dpp_y ...\n"; return $y; } sub get_p_xy { ($lat,$lon) = @_; # get latitude, longitude, in degrees $x = get_p_x($lon); $y = get_p_y($lat); ###return (get_p_x($lon) . get_p_y($lat)) return ($x . ',' . $y) } sub get_graph_msg2 { ($lat, $lon) = @_; $pxy = get_p_xy($lat,$lon); $msg = "<p>From lat,lon ($s_max_lat,$s_min_lon width = ($s_deg_wid x $s_deg_ht) on $s_width,$s_height\n"; $msg .= " to " . ($s_max_lat - $s_deg_ht) . "," . ($s_min_lon + $s_deg_wid) . " ..."; $msg .= "<br>x,y = [$pxy] ...</p>\n"; ###$s_dpp_x = ($s_width / $s_deg_wid); ###$s_dpp_y = ($s_height / $s_deg_ht ); $msg .= "<center><table border=\"1\" width=\"" . $s_width . "\" height=\"" . $s_height . "\">\n"; $msg .= "<tr><td>\n"; $x = get_p_x ( $lon ); $y = get_p_y ( $lat ); $ix = int ($x); $iy = int ($y); print "Got x=$ix $x, y=$iy $y from $lat, $lon ...\n"; for ($i = 0; $i < $s_height; $i++) { ### print "Row " , ($i + 1), "\n"; if ($i == ($iy - 2)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; #$msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix - 1) . ">"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; #$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } elsif ($i == ($iy - 1)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; #$msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix - 1) . ">"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; #$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } elsif ($i == $iy) { ###$msg .= "<img src=\"$s_black\" height=\"1\" width=\"$s_width\">\n"; ###$msg .= "<img src=" . $s_black . " height=1 width=" . $s_width . ">"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 3) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=5>"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix - 2) . ">"; #$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } elsif ($i == ($iy + 1)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; } elsif ($i == ($iy + 2)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; } else { ##$msg .= "<img src=\"$s_white\" height=\"1\" width=\"$s_width\">\n"; ##$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">\n"; $msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } if ($i < ($s_height - 1)) { $msg .= "<br>"; } } $msg .= "</td></tr></table></center>\n"; return $msg; } ### $dispdt = "$currdated $currtimed"; ### $gps_lat_curr = $gps_lat_rmc; # latitude(@field[3..4]); ### $gps_long_curr = $gps_long_rmc; # longitude(@field[5..6]); sub get_graph_msg { ($lat, $lon) = @_; $msg = "<center><table border=\"1\" width=\"" . $s_width . "\" height=\"" . $s_height . "\">\n"; $msg .= "<tr><td>\n"; $x = get_p_x ( $lon ); $y = get_p_y ( $lat ); $ix = int ($x); $iy = int ($y); print "Got x=$ix $x, y=$iy $y from $lat, $lon ...\n"; for ($i = 0; $i < $s_height; $i++) { ### print "Row " , ($i + 1), "\n"; if ($i == ($iy - 1)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; #$msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix - 1) . ">"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; #$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } elsif ($i == $iy) { ###$msg .= "<img src=\"$s_black\" height=\"1\" width=\"$s_width\">\n"; ###$msg .= "<img src=" . $s_black . " height=1 width=" . $s_width . ">"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 2) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=3>"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix - 1) . ">"; #$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } elsif ($i == ($iy + 1)) { $msg .= "<img src=" . $s_white . " height=1 width=" . ($ix - 1) . ">"; $msg .= "<img src=" . $s_black . " height=1 width=1>"; $msg .= "<img src=" . $s_white . " height=1 width=" . ($s_width - $ix) . ">"; } else { ##$msg .= "<img src=\"$s_white\" height=\"1\" width=\"$s_width\">\n"; ##$msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">\n"; $msg .= "<img src=" . $s_white . " height=1 width=" . $s_width . ">"; } if ($i < ($s_height - 1)) { $msg .= "<br>"; } } $msg .= "</td></tr></table></center>\n"; return $msg; } sub do_html_table1 { $ht = ($s_max_lat > 0) ? $s_deg_ht : -$s_deg_ht; ###$mxlat = ($s_max_lat + $s_deg_ht); $mxlat = ($s_max_lat + $ht); $mxlon = ($s_min_lon + $s_deg_wid); $tlx = int(get_p_x($s_min_lon)); $tly = int(get_p_y($s_max_lat)); $brx = int(get_p_x($mxlon)); $bry = int(get_p_y($mxlat)); $tx = "<p>From lat,lon ($s_max_lat,$s_min_lon width ($tlx,$tly) = ($s_deg_wid x $s_deg_ht) on $s_width,$s_height\n"; $tx .= " to " . ($s_max_lat - $s_deg_ht) . "," . ($s_min_lon + $s_deg_wid); $tx .= "<br> to " . $mxlat . "," . $mxlon . " ($brx,$bry)"; $tx .= '<br>' . " pos = " . $gps_lat_curr . ',' . $gps_long_curr . ' x,y ' . int(get_p_y ($gps_lat_curr)) . ',' . int(get_p_x ($gps_long_curr)); $tx .= '<br>' . " dif = " . ($gps_lat_curr - $s_max_lat) . ',' . ($gps_long_curr - $s_min_lon) ; $tx .= "</p>\n"; print $OF $tx; 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) ); print $OF new_row( in_col($clat) . in_col($clon) . in_col($calt) . in_col(get_p_x($clon)) . in_col(get_p_y($clat)) ); $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) ); # if ($currposlaty < $minlat) { $minlat = $currposlaty } # if ($currposlaty > $maxlat) { $maxlat = $currposlaty } # if ($currposlonx < $minlon) { $minlon = $currposlonx } # if ($currposlonx > $maxlon) { $maxlon = $currposlonx } print $OF new_row( in_colb(($maxlat + $minlat) / 2) . in_colb(($maxlon + $minlon) / 2) . in_colb('Aver.') . in_colb($actdist) . in_colb($actazim) ); print $OF new_row( in_colb($maxlat) . in_colb($maxlon) . in_colb('Max') . in_colb($actdist) . in_colb($actazim) ); print $OF new_row( in_colb($minlat) . in_colb($minlon) . in_colb('Min') . in_colb($actdist) . in_colb($actazim) ); print $OF new_row( in_colb($maxlat - $minlat) . in_colb($maxlon - $minlon) . in_colb('Diff') . in_colb($actdist) . in_colb($actazim) ); ######################################################## print $OF "</table>\n"; print $OF "</center>\n"; } sub do_html_file { ## OUPUT HTML open ($OF, ">$outhtml") or die "warning: couldn't open output file [$outhtml]!\n"; html_head ( \*$OF, $outhtml ); do_html_table1 (); ### $gps_lat_curr = $gps_lat_rmc; # latitude(@field[3..4]); ### $gps_long_curr = $gps_long_rmc; # longitude(@field[5..6]); ###$msg = get_graph_msg ( $gps_lat_curr, $gps_long_curr ); $msg = get_graph_msg2 ( $gps_lat_curr, $gps_long_curr ); ###($lat, $lon) = @_; print $OF "$msg\n"; ###print "$msg\n"; print "Added ", length($msg), " written to $outhtml ...\n"; print $OF "</body>\n"; print $OF "</html>\n"; close $OF; } ## done HTML file #eof