[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5904] Some cleanup.
|
From: |
Paul Cox |
|
Subject: |
[paparazzi-commits] [5904] Some cleanup. |
|
Date: |
Sun, 19 Sep 2010 22:44:44 +0000 |
Revision: 5904
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5904
Author: paulcox
Date: 2010-09-19 22:44:43 +0000 (Sun, 19 Sep 2010)
Log Message:
-----------
Some cleanup.
Modified Paths:
--------------
paparazzi3/trunk/sw/in_progress/log_parser/nmea2kml.sh
Added Paths:
-----------
paparazzi3/trunk/sw/in_progress/log_parser/distance.pl
paparazzi3/trunk/sw/in_progress/log_parser/log2nmea.pl
Removed Paths:
-------------
paparazzi3/trunk/sw/in_progress/log_parser/get_gps.pl
Added: paparazzi3/trunk/sw/in_progress/log_parser/distance.pl
===================================================================
--- paparazzi3/trunk/sw/in_progress/log_parser/distance.pl
(rev 0)
+++ paparazzi3/trunk/sw/in_progress/log_parser/distance.pl 2010-09-19
22:44:43 UTC (rev 5904)
@@ -0,0 +1,48 @@
+#!/usr/bin/perl
+
+$pi = atan2(1,1) * 4;
+
+sub distance {
+ my ($lat1, $lon1, $lat2, $lon2, $unit) = @_;
+ #printf "$lat1, $lon1, $lat2, $lon2\n";
+ my $theta = $lon1 - $lon2;
+ my $dist = sin(deg2rad($lat1)) * sin(deg2rad($lat2)) +
cos(deg2rad($lat1)) * cos(deg2rad($lat2)) * cos(deg2rad($theta));
+ $dist = acos($dist);
+ $dist = rad2deg($dist);
+ $dist = $dist * 60 * 1.1515;
+ if ($unit eq "K") {
+ $dist = $dist * 1.609344;
+ } elsif ($unit eq "N") {
+ $dist = $dist * 0.8684;
+ }
+ return ($dist);
+}
+
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+#::: This function get the arccos function using arctan function :::
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+sub acos {
+ my ($rad) = @_;
+ my $ret = atan2(sqrt(1 - $rad**2), $rad);
+ return $ret;
+}
+
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+#::: This function converts decimal degrees to radians :::
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+sub deg2rad {
+ my ($deg) = @_;
+ return ($deg * $pi / 180);
+}
+
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+#::: This function converts radians to decimal degrees :::
+#::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::
+sub rad2deg {
+ my ($rad) = @_;
+ return ($rad * 180 / $pi);
+}
+
+#print distance(32.9697, -96.80322, 29.46786, -98.53506, "M") . " Miles\n";
+#print distance(32.9697, -96.80322, 29.46786, -98.53506, "K") . "
Kilometers\n";
+#print distance(32.9697, -96.80322, 29.46786, -98.53506, "N") . " Nautical
Miles\n";
\ No newline at end of file
Property changes on: paparazzi3/trunk/sw/in_progress/log_parser/distance.pl
___________________________________________________________________
Added: svn:executable
+ *
Deleted: paparazzi3/trunk/sw/in_progress/log_parser/get_gps.pl
===================================================================
--- paparazzi3/trunk/sw/in_progress/log_parser/get_gps.pl 2010-09-19
20:09:27 UTC (rev 5903)
+++ paparazzi3/trunk/sw/in_progress/log_parser/get_gps.pl 2010-09-19
22:44:43 UTC (rev 5904)
@@ -1,194 +0,0 @@
-#!/usr/bin/perl
-#Author: Paul Cox
-#This script reads a paparazzi log file and extracts the GPS messages
-#It outputs Time/Lat/Long/Alt
-#TODO: NMEA output, add speed
-#Notes:
-#mode
-#utm_east ALT_UNIT="m" UNIT="cm"
-#utm_north ALT_UNIT="m" UNIT="cm"
-#course ALT_UNIT="deg" UNIT="decideg"
-#alt ALT_UNIT="m" UNIT="cm"
-#speed ALT_UNIT="m/s" UNIT="cm/s"
-#climb ALT_UNIT="m/s" UNIT="cm/s"
-#week weeks
-#itow ms
-#utm_zone
-#gps_nb_err
-#0 1 2 3 4 5 6 7 8 9 10 11 12 13
-#time ID MSG M EAST NORTH C ALT S C W ITOW ZO ERR
-#144.225 20 GPS 3 19779772 497668512 1819 3625 9 -20 1601 303393500 31 0
-
-
-use Geo::Coordinates::UTM;
-
-sub getnmeatime {
- my $utw_h = 0;
- my $utw_d = 0;
- my $foo = $utw/60000/60;
- #calculate days and hours
- while ($foo > 1) {
- if ($utw_h == 23) {$utw_h = 0;$utw_d++;} else {$utw_h = $utw_h + 1;}
- $foo = $foo - 1;
- }
- #ensure proper leading zero for single digits
- $utw_h = sprintf("%02d",$utw_h);
- #foo+1 is the fractional hours
- my $utw_m = int($foo*60);
- #ensure proper leading zero for single digits
- $utw_m = sprintf("%02d",$utw_m);
- #calculate remaining seconds
- my $utw_s =
sprintf("%06.3f",$utw/1000-$utw_m*60-$utw_h*60*60-$utw_d*60*60*24);
- my $time = $utw_h . $utw_m . $utw_s; #Time UTC HHMMSS.mmm
303318000/60000=5055.3/60=84.255/24=3.510625
- return $time;
-}
-
-open INFILE, "<10_09_15__14_13_55.data" or die $!;
-open OUTFILE, ">GPS_data.txt" or die $!;
-open NMEAFILE, ">NMEA.log" or die $!;
-
-$cnt=0;
-$utw=0;
-$toffset=0;
-$solstart=0;
-$totime=0;
-$tdtime=0;
-$hialt=0;
-
-while (my $line = <INFILE>) {
- chomp($line);
- @fields = split(/ /,$line);
-
- #Determine when GPS fix is acquired by looking for PDOP <1000 and numSV>3
- if ($fields[2] eq "GPS_SOL" and $fields[5] < 1000 and $fields[6] > 3) {
- if ($solstart == 0 ) {
- $solstart = $fields[0];
- printf "GPS SOL start time: $solstart\n";
- }
- $pacc = $fields[3];
- $sacc = $fields[4];
- $pdop = $fields[5];
- $numSV = $fields[6];
- }
-
- #We are going to look for GPS messages, in mode 3 (3D fix)
- # Skip messages that have the previous utw (duplicates)
- # Skip any messages with negative altitude (GPS not initialized yet)
- if ($fields[2] eq "GPS" and $fields[3] == "3" and $fields[11] != $utw and
$fields[7] > 0 and $solstart != 0) {
-
- #takeoff is considered to be > 4 m/s on hor and vert
- if ($fields[8] > 400 and $fields[9] > 400 and $totime == 0) {
- $totime = $fields[0];
- my $gndalt = $fields[7];
- printf "Takeoff detected at time : $totime s\n";
- }
-
- if ($fields[7] > $hialt and $totime != 0 ) {
- $hialt = $fields[7];
- $hialtt = $fields[0];
- }
-
- #touchdown is considered when < 1 m/s on hor and vert
- if ($fields[8] < 100 and $fields[9] < 100 and $totime != 0 and $tdtime ==
0) {
- $tdtime = $fields[0];
- printf "Highest Alt : %.2f ($hialtt)\n",($hialt-$gndalt)/100;
- printf "Touchdown detected at time : $tdtime s (flight time: %.2f
min)\n",($tdtime-$totime)/60;
- }
-
- #store begin flight time
- if ($toffset == 0) {
- $toffset = $fields[0];
- printf "GPS Start Time: $toffset\n";
- }
- #Calculate delta and store for averaging at the end
- $delta= $fields[0]-$prevtime;
- $prevtime = $fields[0];
- $sum += $delta;
-
- $utw=$fields[11];
- printf OUTFILE "Time: ";
- printf OUTFILE '%.2f',$fields[0] - $toffset;
-# printf OUTFILE " utme: $fields[4] utmn: $fields[5]";
- printf OUTFILE " Alt: ";
- printf OUTFILE '%.2f',($fields[7]/100);
- my $zone = $fields[12] . "V";
- #divide by 100 as gps provides utm in centimeters
- my
($latitude,$longitude)=utm_to_latlon('wgs84',$zone,$fields[4]/100,$fields[5]/100);
- printf OUTFILE " Lat: ";
- printf OUTFILE '%.6f',$latitude ;
- printf OUTFILE " Lon: \ ";
- printf OUTFILE '%.6f',$longitude;
- printf OUTFILE "\n";
-
- if ($totime == 0) { next;}
- if ($tdtime != 0) { break;}
- #Begin NMEA output
- #RMC,GGA/GSA/VTG/GSV
- printf NMEAFILE "\$GPRMC,";
-
- $time = getnmeatime($utw);
-
- printf NMEAFILE $time . ",A,"; #Active
-
- $degrees = abs(int($latitude));
- $minutes = (abs($latitude) - $degrees) * 60;
- $nmealat = sprintf("%02d%.4f",$degrees,$minutes). "," ."N";
- printf NMEAFILE $nmealat . ","; #LAT
-
- $degrees = abs(int($longitude));
- $minutes = (abs($longitude) - $degrees) * 60;
- $nmealon = sprintf("%03d%.4f",$degrees,$minutes);
- if ($longitude > 0) {
- $nmealon .= ",E" ; #LON
- } else {
- $nmealon .= ",W" ; #LON
- }
- printf NMEAFILE $nmealon . ",";
-
- printf NMEAFILE '%.2f,',$fields[8]*.019438444; #gnd spd in knts from cm/s
- printf NMEAFILE '%.2f,',$fields[6]/10; #trk angle in deg from decideg
-
- # ($year,$month,$day) = Monday_of_Week($week,"2010");
- # print
- printf NMEAFILE "150910" . ",,\n"; #date and mag var TODO:use real date
- $cnt++;
-
- #$GPRMC,121518.000,A,4452.767,N,00049.573,W,0.45,0.00,150910,,*1F
- #$GPGGA,121518.000,4452.767,N,00049.573,W,1,00,0.0,0.000,M,0.0,M,,*7D
- #$GPVTG,0.000,T,0,M,0.450,N,0.833,K*59
- printf NMEAFILE
"\$GPGGA,$time,$nmealat,$nmealon,1,$numSV,$pdop,%.2f,M,%.2f,M,,\n",$fields[7]/100,$fields[7]/100;
- printf NMEAFILE
"\$GPVTG,0.000,T,0,M,%.2f,N,%.2f,K\n",$fields[8]*.019438444,$fields[8]*.03598272;
- }
-}
-
-printf "Number of GPS points: $cnt Avg. delta: ";
-printf '%.2f',$sum/$cnt;
-printf " Duration: ";
-printf '%.2f',$cnt/4/60;
-printf " minutes\n";
-
-close INFILE;
-close OUTFILE;
-
-open NMEAFILE, "<NMEA.log" or die $!;
-open OUTFILE, ">gps.nmea" or die $!;
-
-
-while (my $line = <NMEAFILE>) {
- chomp($line);
- #hack off CR
- $len = length($line);
- $cksum = 0;
- printf OUTFILE $line . "*";
- #hack off $
- $line = substr($line,1,$len);
- for ( split(//, $line) ) { $cksum ^= ord($_); };
-
- printf OUTFILE '%02x',$cksum ;
- printf OUTFILE "\n";
-}
-
-close NMEAFILE;
-close OUTFILE;
-
-
\ No newline at end of file
Added: paparazzi3/trunk/sw/in_progress/log_parser/log2nmea.pl
===================================================================
--- paparazzi3/trunk/sw/in_progress/log_parser/log2nmea.pl
(rev 0)
+++ paparazzi3/trunk/sw/in_progress/log_parser/log2nmea.pl 2010-09-19
22:44:43 UTC (rev 5904)
@@ -0,0 +1,213 @@
+#!/usr/bin/perl
+#Author: Paul Cox
+#This script reads a paparazzi log file and extracts the GPS messages
+#It outputs Time/Lat/Long/Alt
+#TODO: waypoints and remove intermediate file creation
+#Notes:
+#mode
+#utm_east ALT_UNIT="m" UNIT="cm"
+#utm_north ALT_UNIT="m" UNIT="cm"
+#course ALT_UNIT="deg" UNIT="decideg"
+#alt ALT_UNIT="m" UNIT="cm"
+#speed ALT_UNIT="m/s" UNIT="cm/s"
+#climb ALT_UNIT="m/s" UNIT="cm/s"
+#week weeks
+#itow ms
+#utm_zone
+#gps_nb_err
+#0 1 2 3 4 5 6 7 8 9 10 11 12 13
+#time ID MSG M EAST NORTH C ALT S C W ITOW ZO ERR
+#144.225 20 GPS 3 19779772 497668512 1819 3625 9 -20 1601 303393500 31 0
+
+#use strict; fast and loose is nice...
+use Geo::Coordinates::UTM;
+require "distance.pl";
+
+my $utw=0;
+my $cnt=0;
+my $toffset=0;
+my $solstart=0;
+my $totime=0;
+my $tdtime=0;
+my $hialt=0;
+my $maxdist=0;
+my $latitude=0;
+my $longitude=0;
+my $gndalt=0;
+my $delta=0;
+
+sub getnmeatime {
+ my $utw_h = 0;
+ my $utw_d = 0;
+ my $foo = $utw/60000/60;
+ #calculate days and hours
+ while ($foo > 1) {
+ if ($utw_h == 23) {$utw_h = 0;$utw_d++;} else {$utw_h = $utw_h + 1;}
+ $foo = $foo - 1;
+ }
+ #ensure proper leading zero for single digits
+ $utw_h = sprintf("%02d",$utw_h);
+ #foo+1 is the fractional hours
+ my $utw_m = int($foo*60);
+ #ensure proper leading zero for single digits
+ $utw_m = sprintf("%02d",$utw_m);
+ #calculate remaining seconds
+ my $utw_s =
sprintf("%06.3f",$utw/1000-$utw_m*60-$utw_h*60*60-$utw_d*60*60*24);
+ my $time = $utw_h . $utw_m . $utw_s; #Time UTC HHMMSS.mmm
303318000/60000=5055.3/60=84.255/24=3.510625
+ return $time;
+}
+
+#my $filename = '10_09_15__14_13_55';
+my $filename = $ARGV[0];
+my @filepts = split(/\_/,$filename);
+my $date = $filepts[2] . $filepts[1] . $filepts[0];
+printf "NMEA Date: $date\n";
+open DATAFILE, "<$filename.data" or die $!;
+#TODO: open .log file and create nmea waypoints from flightplan waypoints
+#open OUTFILE, ">GPS_data_$date.txt" or die $!;
+open NMEAFILE, ">NMEA_$date.log" or die $!;
+
+while (my $line = <DATAFILE>) {
+ chomp($line);
+ my @fields = split(/ /,$line);
+
+ #Determine when GPS fix is acquired by looking for PDOP <1000 and numSV>3
+ if ($fields[2] eq "GPS_SOL" and $fields[5] < 1000 and $fields[6] > 3) {
+ if ($solstart == 0 ) {
+ $solstart = $fields[0];
+ printf "GPS SOL start time: $solstart\n";
+ }
+ my $pacc = $fields[3];
+ my $sacc = $fields[4];
+ my $pdop = $fields[5];
+ my $numSV = $fields[6];
+ }
+
+ #We are going to look for GPS messages, in mode 3 (3D fix)
+ # Skip messages that have the previous utw (duplicates)
+ # Skip any messages with negative altitude (GPS not initialized yet)
+ if ($fields[2] eq "GPS" and $fields[3] == "3" and $fields[11] != $utw and
$fields[7] > 0 and $solstart != 0) {
+ #store begin flight time
+ if ($toffset == 0) {
+ $toffset = $fields[0];
+ printf "GPS Start Time: $toffset\n";
+ }
+ #Calculate delta and store for averaging at the end
+ if ($prevtime == 0) {
+ $prevtime = $fields[0];
+ } else {
+ $delta= $fields[0]-$prevtime;
+ if ($delta > 2) {printf "warning: delta %.1f at $fields[0] s.\n",$delta;}
+ $prevtime = $fields[0];
+ $sum += $delta;
+ $cnt++;
+ }
+ #takeoff is considered to be > 4 m/s on hor and vert
+ if ($fields[8] > 400 and $fields[9] > 400 and $totime == 0) {
+ $totime = $fields[0];
+ $gndalt = $fields[7];
+ $olat=$latitude; $olon=$longitude;
+ #create waypoint
+ printf "Takeoff detected at time : $totime s\n";
+ }
+
+ if ($fields[7] > $hialt and $totime != 0 ) {
+ $hialt = $fields[7];
+ $hialtt = $fields[0];
+ #store lat/lon for waypoint creation at end of program
+ }
+
+ #touchdown is considered when < 1 m/s on hor and vert
+ if ($fields[8] < 100 and $fields[9] < 100 and $totime != 0 and $tdtime ==
0) {
+ $tdtime = $fields[0];
+ printf "Max Alt : %.2f meters ($hialtt sec)\n",($hialt-$gndalt)/100;
+ printf "Max Dist: %.3f km ($maxdistt sec)\n",$maxdist;
+ printf "Touchdown detected at time : $tdtime s (flight time: %.2f
min)\n",($tdtime-$totime)/60;
+ }
+
+ $utw=$fields[11];
+ #divide by 100 as gps provides utm in centimeters
+ ($latitude,$longitude)=utm_to_latlon('wgs84',($fields[12] .
"V"),$fields[4]/100,$fields[5]/100);
+ #printf OUTFILE "Time: %.2f Alt: %.2f Lat: %.6f Lon: %.6f\n",
+ # $fields[0] -
$toffset,($fields[7]/100),$latitude,$longitude;
+
+ if ($totime == 0) { next;}
+ if ($tdtime != 0) { last;}
+
+ my $dist = distance($latitude, $longitude, $olat, $olon, "K");
+ if ( $dist > $maxdist ) {
+ $maxdist = $dist;
+ $maxdistt = $fields[0];
+ #create waypoint
+ #$GPWPL,4917.16,N,12310.64,W,003*65
+ }
+
+ #Begin NMEA output
+ #RMC,GGA/GSA/VTG/GSV
+ printf NMEAFILE "\$GPRMC,";
+
+ $time = getnmeatime($utw);
+
+ printf NMEAFILE $time . ",A,"; #Active
+
+ $degrees = abs(int($latitude));
+ $minutes = (abs($latitude) - $degrees) * 60;
+ $nmealat = sprintf("%02d%.4f",$degrees,$minutes). "," ."N";
+ printf NMEAFILE $nmealat . ","; #LAT
+
+ $degrees = abs(int($longitude));
+ $minutes = (abs($longitude) - $degrees) * 60;
+ $nmealon = sprintf("%03d%.4f",$degrees,$minutes);
+ if ($longitude > 0) {
+ $nmealon .= ",E" ; #LON
+ } else {
+ $nmealon .= ",W" ; #LON
+ }
+ printf NMEAFILE $nmealon . ",";
+
+ printf NMEAFILE '%.2f,',$fields[8]*.019438444; #gnd spd in knts from cm/s
+ printf NMEAFILE '%.2f,',$fields[6]/10; #trk angle in deg from decideg
+
+ # ($year,$month,$day) = Monday_of_Week($week,"2010"); TODO:use week and
day to calculate date?
+ printf NMEAFILE "$date,,\n"; #date and mag var
+
+
+ #$GPRMC,121518.000,A,4452.767,N,00049.573,W,0.45,0.00,150910,,*1F
+ #$GPGGA,121518.000,4452.767,N,00049.573,W,1,00,0.0,0.000,M,0.0,M,,*7D
+ #$GPVTG,0.000,T,0,M,0.450,N,0.833,K*59
+ printf NMEAFILE
"\$GPGGA,$time,$nmealat,$nmealon,1,$numSV,$pdop,%.2f,M,%.2f,M,,\n",$fields[7]/100,$fields[7]/100;
+ printf NMEAFILE
"\$GPVTG,0.000,T,0,M,%.2f,N,%.2f,K\n",$fields[8]*.019438444,$fields[8]*.03598272;
+ }
+
+}
+
+printf "Number of GPS points: $cnt Avg. delta: ";
+printf '%.2f',$sum/$cnt;
+printf " Duration: ";
+printf '%.2f',$cnt/4/60;
+printf " minutes\n";
+
+close DATAFILE;
+#close OUTFILE;
+
+open NMEAFILE, "<NMEA_$date.log" or die $!;
+$time = substr($time,0,6);
+open OUTFILE, ">gps_$date\_$time.nmea" or die $!;
+
+while (my $line = <NMEAFILE>) {
+ chomp($line);
+ #hack off CR
+ $len = length($line);
+ $cksum = 0;
+ printf OUTFILE $line . "*";
+ #hack off $
+ $line = substr($line,1,$len);
+ for ( split(//, $line) ) { $cksum ^= ord($_); };
+
+ printf OUTFILE '%02x',$cksum ;
+ printf OUTFILE "\n";
+}
+
+close NMEAFILE;
+`rm NMEA_$date.log`;
+close OUTFILE;
Property changes on: paparazzi3/trunk/sw/in_progress/log_parser/log2nmea.pl
___________________________________________________________________
Added: svn:executable
+ *
Modified: paparazzi3/trunk/sw/in_progress/log_parser/nmea2kml.sh
===================================================================
--- paparazzi3/trunk/sw/in_progress/log_parser/nmea2kml.sh 2010-09-19
20:09:27 UTC (rev 5903)
+++ paparazzi3/trunk/sw/in_progress/log_parser/nmea2kml.sh 2010-09-19
22:44:43 UTC (rev 5904)
@@ -1 +1,16 @@
-gpsbabel -i nmea -f gps.nmea -o kml,deficon=funjet,line_width=1,floating=1 -F
souge4.kml
+#!/bin/bash
+# kml Google Earth (Keyhole) Markup Language
+# deficon Default icon name
+# lines (0/1) Export linestrings for tracks and routes
+# points (0/1) Export placemarks for tracks and routes
+# line_width Width of lines, in pixels
+# line_color Line color, specified in hex AABBGGRR
+# floating (0/1) Altitudes are absolute and not clamped to
ground
+# extrude (0/1) Draw extrusion line from trackpoint to
ground
+# trackdata (0/1) Include extended data for trackpoints
(default = 1
+# trackdirection (0/1) Indicate direction of travel in track
icons (defau
+# units Units used when writing comments ('s'tatute or
'm'
+# labels (0/1) Display labels on track and routepoints
(default
+# max_position_point Retain at most this number of position points
(0
+
+gpsbabel -i nmea -f "$@" -o kml,deficon=funjet,line_width=1,floating=1 -F
ppzout.kml
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5904] Some cleanup.,
Paul Cox <=