mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Paul Cox
218 lines
6.8 KiB
Perl
Executable File
218 lines
6.8 KiB
Perl
Executable File
#!/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 interpolation when delta is > 2 sec
|
|
#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-$_[0]);
|
|
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 $!;
|
|
|
|
my $sacc =0;
|
|
my $pacc =0;
|
|
my $pdop =0;
|
|
my $numSV =0;
|
|
|
|
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";
|
|
}
|
|
$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) {
|
|
#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
|
|
|
|
$degrees = abs(int($latitude));
|
|
$minutes = (abs($latitude) - $degrees) * 60;
|
|
$nmealat = sprintf("%02d%.4f",$degrees,$minutes). "," ."N";
|
|
|
|
$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
|
|
}
|
|
|
|
$gprmc = sprintf '%.2f,',$fields[8]*.019438444; #gnd spd in knts from cm/s
|
|
$gprmc .= sprintf '%.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?
|
|
$gprmc .= sprintf "$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
|
|
#$GPGGA,121828.500,4452.7696,N,00049.5712,W,1,8,184,52.65,M,52.65,M,,*55
|
|
#$GPVTG,0.000,T,0,M,0.450,N,0.833,K*59
|
|
for (my $i = int($delta-0.25); $i >= 0 ; $i-- ) {
|
|
$time= getnmeatime($i);
|
|
#if ($i>0) {printf NMEAFILE "-----\n";}
|
|
printf NMEAFILE "\$GPRMC,$time,A,$nmealat,$nmealon,". $gprmc;
|
|
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;
|
|
close NMEAFILE;
|
|
|
|
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;
|