0,0 → 1,445 |
#!/usr/bin/perl |
#!/usr/bin/perl -d:ptkdb |
|
############################################################################### |
# |
# mktrack.pl - Tracking Antenne |
# |
# Copyright (C) 2009 Rainer Walther (rainerwalther-mail@web.de) |
# |
# Creative Commons Lizenz mit den Zusaetzen (by, nc, sa) |
# |
# Es ist Ihnen gestattet: |
# * das Werk vervielfältigen, verbreiten und öffentlich zugänglich machen |
# * Abwandlungen bzw. Bearbeitungen des Inhaltes anfertigen |
# |
# Zu den folgenden Bedingungen: |
# * Namensnennung. |
# Sie müssen den Namen des Autors/Rechteinhabers in der von ihm festgelegten Weise nennen. |
# * Keine kommerzielle Nutzung. |
# Dieses Werk darf nicht für kommerzielle Zwecke verwendet werden. |
# * Weitergabe unter gleichen Bedingungen. |
# Wenn Sie den lizenzierten Inhalt bearbeiten oder in anderer Weise umgestalten, |
# verändern oder als Grundlage für einen anderen Inhalt verwenden, |
# dürfen Sie den neu entstandenen Inhalt nur unter Verwendung von Lizenzbedingungen |
# weitergeben, die mit denen dieses Lizenzvertrages identisch oder vergleichbar sind. |
# |
# Im Falle einer Verbreitung müssen Sie anderen die Lizenzbedingungen, unter welche dieses |
# Werk fällt, mitteilen. Am Einfachsten ist es, einen Link auf diese Seite einzubinden. |
# |
# Jede der vorgenannten Bedingungen kann aufgehoben werden, sofern Sie die Einwilligung |
# des Rechteinhabers dazu erhalten. |
# |
# Diese Lizenz lässt die Urheberpersönlichkeitsrechte unberührt. |
# |
# Weitere Details zur Lizenzbestimmung gibt es hier: |
# Kurzform: http://creativecommons.org/licenses/by-nc-sa/3.0/de/ |
# Komplett: http://creativecommons.org/licenses/by-nc-sa/3.0/de/legalcode |
# |
############################################################################### |
# 2009-02-14 0.0.1 rw created |
# 2009-04-01 0.1.0 rw RC1 |
# |
############################################################################### |
|
$Version{'track.pl'} = "0.1.0 - 2009-04-01"; |
|
# Packages |
use Time::HiRes qw(usleep gettimeofday); # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm |
use threads; |
use threads::shared; |
use Math::Trig; |
use Geo::Ellipsoid; # http://search.cpan.org/dist/Geo-Ellipsoid-1.12/lib/Geo/Ellipsoid.pm |
# http://www.kompf.de/gps/distcalc.html |
# http://www.herrmann-w.de/Geocaching/Downloads/Richt.XLS |
# http://williams.best.vwh.net/avform.htm |
# http://williams.best.vwh.net/gccalc.html |
# http://de.wikipedia.org/wiki/Orthodrome |
if ( $^O =~ /Win32/i ) |
{ |
require Win32::SerialPort; # http://search.cpan.org/dist/Win32-SerialPort |
} |
else |
{ |
require Device::SerialPort; # http://search.cpan.org/~cook/Device-SerialPort-1.04/SerialPort.pm |
} |
|
require "mkcomm.pl"; # MK communication |
|
# Sharing for Threads |
share (@ServoPos); |
share (%MkTrack); |
|
# |
# Parameter |
# |
|
# System Timer |
$SysTimerResolution = 1000; # Resolution in us |
|
# Com Port for Pololu Mikro-Servoboard |
# http://www.shop.robotikhardware.de/shop/catalog/product_info.php?cPath=65&products_id=118 |
if ( ! defined $Cfg->{'track'}->{'Port'} ) |
{ |
# set default port |
$Cfg->{'track'}->{'Port'} = "COM8"; |
} |
|
# Servo parameter |
$ServoPan = 0; # Servo channel Pan |
$ServoTilt = 1; # Servo channel Tilt |
$MkTrack{'ServoPan'} = $ServoPan; |
$MkTrack{'ServoTilt'} = $ServoTilt; |
@ServoSpeed = (100/40, 100/40, 100/40, 100/40, 100/40, 100/40, 100/40, 100/40); # ms/degree |
$ServoConstPpm = 20; # PPM protocol overhead in ms |
|
@ServoTest = ( [ 45, 45 ], # Pan, Tilt in degrees for servo test |
[ 0, 0 ], |
[ 45, 45 ], |
[ 90, 90 ], |
[ 135, 45 ], |
[ 180, 0 ], |
[ 135, 45 ], |
[ 90, 90 ] ); |
|
# Tracking |
$TrackInterval = 100; # in ms |
# |
# Logging |
# |
sub Log() |
{ |
my ($Message, $Level) = @_; |
|
if ( $LogOn ) |
{ |
print ("$SysTimerCount_ms: $Message\n"); |
} |
} |
# |
# Timer |
# |
sub SetTimer_ms() |
{ |
return $SysTimerCount_ms + $_[0]; |
} |
|
sub CheckTimer_ms() |
{ |
my $Diff = $_[0] - $SysTimerCount_ms; |
return ($Diff <= 0); |
} |
# |
# Servo |
# |
sub ServoInit() |
{ |
# open COM-Port |
my $ComPort = $Cfg->{'track'}->{'Port'}; |
undef $ServoPort; |
if ( $^O =~ m/Win32/ ) |
{ |
$ServoPort = Win32::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
} |
else |
{ |
$ServoPort = Device::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
} |
# Set COM parameters |
$ServoPort->baudrate(38400); |
$ServoPort->parity("none"); |
$ServoPort->databits(8); |
$ServoPort->stopbits(1); |
$ServoPort->handshake('none'); |
$ServoPort->write_settings; |
# Byte 1: sync - Pololu Mode |
# Byte 2: device |
# Byte 3: command |
# Byte 4: Servo num |
# Byte 5: data |
# Bit 6: Servo on/off |
# Bit 5: Direction |
# Bit 4-0: Servo Range |
my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x60 | 27 ); |
$ServoPort->write($Output); |
|
@ServoStartTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of last ServoMove() call |
@ServoEndTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of estimated arrival at end position |
@ServoPos = (0, 0, 0, 0, 0, 0, 0, 0); # Current servo position 0..180 degree |
} |
|
sub ServoMove() |
{ |
my ($Num, $Angel, $Time) = @_; |
|
my $Overhead = 0; |
|
if ( $Angel != $ServoPos[$Num] ) |
{ |
if ( $Angel < 0) {$Angel = 0;} |
if ( $Angel > 180) {$Angel = 180;} |
|
my $Pos = $Angel * 127/180; # angel 0..180 degree to servo position 0..127 |
|
# output to COM port |
# Byte 1: sync - Pololu Mode |
# Byte 2: device |
# Byte 3: command |
# Byte 4: Servo num |
# Byte 5: servo position 0..127 |
|
my $Output = pack('C*', 0x80, 0x01, 0x02, $Num, $Pos ); |
$ServoPort->write($Output); |
|
$Overhead += $ServoConstPpm; # PPM protocol overhead |
} |
|
# set timer stuff for travel time predicion |
my $LastAngel = $ServoPos[$Num]; |
my $EstimatedTime = abs($Angel - $LastAngel) * $ServoSpeed[$Num] + $Overhead; |
if ( $Time > 0 ) |
{ |
# Parameter override |
$EstimatedTime = $Time; |
} |
$ServoStartTime[$Num] = $SysTimerCount_ms; |
$ServoEndTime[$Num] = $SysTimerCount_ms + $EstimatedTime; |
$ServoPos[$Num] = $Angel; |
|
&Log ("ServoMove: Num: $Num : LastPos: $LastAngel NewPos: $Angel Estimated: $EstimatedTime ms"); |
|
return $ServoEndTime[$Num]; |
} |
|
|
# Check, if servo has reached end position |
sub ServoCheck() |
{ |
my $Num = $_[0]; |
return &CheckTimer_ms($ServoEndTime[$Num]); |
} |
|
|
sub ServoClose() |
{ |
# close COM-Port |
undef $ServoPort; |
} |
|
# |
# Signal handler |
# |
|
$SIG{'INT'} = 'SigHandler'; |
$SIG{'KILL'} = 'SigHandler'; |
|
sub SigHandler() |
{ |
# move all Servo to neutral position |
&ServoMove ($ServoPan, 90); |
&ServoMove ($ServoTilt, 90); |
|
&ServoClose(); |
|
if ( defined threads->self() ) |
{ |
threads->exit(); |
} |
exit; |
} |
|
|
# |
# Track it |
# |
|
sub TrackAntennaGps() |
{ |
|
# |
# State maschine |
# |
|
my $State = "ColdStart"; |
while (1) |
{ |
if ( $State eq "ColdStart" ) |
{ |
&ServoInit(); |
|
# initialize system-timer |
$SysTimerCount_ms = 0; |
$SysTimerError = 0; |
($t0_s, $t0_us) = gettimeofday; |
|
$ServoTestIndex = 0; |
|
$State = "InitServoTest"; |
} |
# |
# Start servo test |
# doesn't really make much sense, but looks cool:-) |
# |
elsif ( $State eq "InitServoTest") |
{ |
if ( &ServoCheck ($ServoPan) and &ServoCheck ($ServoTilt) ) |
{ |
|
my $PanPos = $ServoTest[$ServoTestIndex][0]; |
my $TiltPos = $ServoTest[$ServoTestIndex][1]; |
$ServoTestIndex ++; |
|
if ( defined $PanPos and defined $TiltPos ) |
{ |
&ServoMove ($ServoPan, $PanPos, 1000); # override 1s travel time |
&ServoMove ($ServoTilt, $TiltPos, 1000); # override 1s travel time |
} |
else |
{ |
# complete |
$ServoTestIndex = 0; |
$State = "WaitGps"; |
} |
} |
} |
# |
# Servo test finisched |
# |
# Wait for GPS Home position and compass |
# |
elsif ( $State eq "WaitGps" ) |
{ |
if ( &ServoCheck ($ServoPan) ) |
{ |
if ( $MkOsd{'_Timestamp'} >= time-2 and |
$MkOsd{'SatsInUse'} >= 6 ) |
{ |
# gültige OSD daten vom MK und guter Satellitenempfang |
|
# take GPS and compass from MK as antenna home-position |
$MkTrack{'HomePos_Lon'} = $MkOsd{'HomePos_Lon'}; |
$MkTrack{'HomePos_Lat'} = $MkOsd{'HomePos_Lat'}; |
$MkTrack{'HomePos_Alt'} = $MkOsd{'HomePos_Alt'}; |
if ( $MkTrack{'CompassHeading'} eq "" ) |
{ |
# nur beim ersten mal |
$MkTrack{'CompassHeading'} = $MkOsd{'CompassHeading'}; |
} |
|
$TrackTimer = &SetTimer_ms($TrackInterval); |
|
$State = "TrackGps"; |
} |
} |
} |
# |
# GPS Fix Home position |
# Track now |
# |
elsif ( $State eq "TrackGps" ) |
{ |
if ( &CheckTimer_ms($TrackTimer) and &ServoCheck($ServoPan) ) |
{ |
$TrackTimer = &SetTimer_ms($TrackInterval); # reload Timer |
|
if ( $MkOsd{'_Timestamp'} >= time -2 and |
$MkOsd{'SatsInUse'} >= 4 ) |
{ |
# valid OSD data from the MK and sufficient satellites |
lock (%MkOsd); # until end of block |
lock (%MkTrack); |
|
my $Track_Geo = Geo::Ellipsoid->new( 'units' => 'degrees', |
'distance_units' => 'meter', |
'ellipsoid' => 'WGS84', |
); |
|
my ($Dist, $Bearing) = $Track_Geo->to($MkTrack{'HomePos_Lat'}, $MkTrack{'HomePos_Lon'}, |
$MkOsd{'CurPos_Lat'}, $MkOsd{'CurPos_Lon'}); |
my $Dir_h = $MkTrack{'CompassHeading'}; |
my $Dir_c = $MkOsd{'CompassHeading'}; |
|
if ( $Dist < 2 ) # meter |
{ |
# Too close to Home-Position. Set antenna to middle position |
$Bearing = $Dir_h + 90; |
} |
|
$MkTrack{'Bearing'} = sprintf ("%d", $Bearing); |
$MkTrack{'Dist'} = sprintf ("%d", $Dist); |
$MkTrack{'CurPos_Lon'} = $MkOsd{'CurPos_Lon'}; |
$MkTrack{'CurPos_Lat'} = $MkOsd{'CurPos_Lat'}; |
$MkTrack{'CurPos_Alt'} = $MkOsd{'CurPos_Alt'}; |
|
my $AngelPan = $Bearing - $Dir_h + 90; # antenna direction: 0..180 degree, centre = 90 |
while ( $AngelPan >= 360 ) |
{ |
$AngelPan -= 360; |
} |
|
$MkTrack{'AngelPan'} = $AngelPan; |
&ServoMove ($ServoPan, $AngelPan); |
|
# determine tilt angle |
my $AngelTilt = rad2deg(atan2(($MkOsd{'CurPos_Alt'} - $MkTrack{'HomePos_Alt'}), $Dist)) + 90; # antenna direction: 0..180 degree, center = 90 |
#if ( $AngelTilt < 90 ) { $AngelTilt = 90; } |
$MkTrack{'AngelTilt'} = $AngelTilt; |
&ServoMove ($ServoTilt, $AngelTilt); |
|
# Timestamp, wann der Datensatz geschtieben wurde |
$MkTrack{'_Timestamp'} = time; |
} |
} |
} |
|
else |
{ |
# Restart |
&Log ("WARNING: Error in state machine - $State - ColdStart"); |
|
$State = "ColdStart"; |
} |
|
# |
# update system-timer |
# |
($t1_s, $t1_us) = gettimeofday; |
$SysTimerSleep_us = ($t0_s - $t1_s) * 1000000 + $t0_us - $t1_us + $SysTimerCount_ms * $SysTimerResolution; |
|
#&Log ("SysTimerSleep_us: $SysTimerSleep_us"); |
|
if ($SysTimerSleep_us > 0) |
{ |
usleep ($SysTimerSleep_us); |
} |
else |
{ |
$SysTimerError ++; |
} |
|
$SysTimerCount_ms ++; |
} |
} |
|
|
# |
# Main Program |
# |
|
if ( $0 =~ /track.pl$/i ) |
{ |
# Program wurde direkt aufgerufen |
|
# Kommunikation zum MK herstellen |
# Input: %MkOsd, %MkTarget, %MkNcDebug |
# Ouput: Thread-Queue: $MkSendQueue |
$mk_thr = threads->create (\&MkCommLoop) -> detach(); |
|
&TrackAntennaGps(); |
|
# should never exit |
} |
|
1; |
|
__END__ |