0,0 → 1,631 |
#!/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 |
# 2009-06-14 0.1.1 rw Tilt-Servo added |
# 2009-08-15 0.1.2 rw Flip Pan/Tilt servo for >180 degree |
# config servo direction and range |
# optional get antenna home position from Map-config |
# Signal handler replaced by command-queue |
# 2009-09-30 0.1.3 rw Commandline parameter added |
# 2009-10-07 0.1.4 rw COM-Port > 9 |
# Servo Speed, neutral position and 8 bit position |
# Add Coldstart command |
# Relax servo at shutdown |
# PortSetSkip config |
# |
############################################################################### |
|
$Version{'track.pl'} = "0.1.4 - 2009-10-05"; |
|
|
if ( $0 =~ /track.pl$/i ) |
{ |
# Program wurde direkt aufgerufen |
|
# change working directory to program path |
my $Cwd = substr ($0, 0, rindex ($0, "track.pl")); |
chdir $Cwd; |
|
# set path for local Perl libs |
push @INC, $Cwd . "perl/lib"; |
} |
|
|
# Packages |
use Time::HiRes qw(usleep gettimeofday); # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm |
use threads; # http://search.cpan.org/~jdhedden/threads-1.72/threads.pm |
# http://perldoc.perl.org/threads.html |
use threads::shared; # http://search.cpan.org/~jdhedden/threads-shared-1.28/shared.pm |
use Thread::Queue; # http://search.cpan.org/dist/Thread-Queue-2.11/lib/Thread/Queue.pm |
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); |
|
# Queue for receiving commands |
$TrackQueue = Thread::Queue->new(); |
|
# Commandline |
my %CmdLine = @ARGV; |
|
|
# |
# 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 |
my $ComPort = $Cfg->{'track'}->{'Port'} || "COM8"; |
|
my $ServoPanCtrl = $Cfg->{'track'}->{'ServoPanCtrl'} || (0x00 | 15); |
my $ServoTiltCtrl = $Cfg->{'track'}->{'ServoTiltCtrl'} || (0x20 | 15 ); |
my $ServoPanSpeed = $Cfg->{'track'}->{'ServoPanSpeed'} || 0; |
my $ServoTiltSpeed = $Cfg->{'track'}->{'ServoTiltSpeed'} || 0; |
my $ServoPanNeutral = $Cfg->{'track'}->{'ServoPanNeutral'} || 3000; |
my $ServoTiltNeutral = $Cfg->{'track'}->{'ServoTiltNeutral'} || 3000; |
|
$MkTrack{'ServoPanCtrl'} = $ServoPanCtrl; |
$MkTrack{'ServoTiltCtrl'} = $ServoTiltCtrl; |
$MkTrack{'ServoPanSpeed'} = $ServoPanSpeed; |
$MkTrack{'ServoTiltSpeed'} = $ServoTiltSpeed; |
$MkTrack{'ServoPanNeutral'} = $ServoPanNeutral; |
$MkTrack{'ServoTiltNeutral'} = $ServoTiltNeutral; |
|
# Servo parameter |
$ServoPan = 0; # Servo channel Pan |
$ServoTilt = 1; # Servo channel Tilt |
$MkTrack{'ServoPan'} = $ServoPan; |
$MkTrack{'ServoTilt'} = $ServoTilt; |
@ServoSpeed = (200/40, 200/40, 200/40, 200/40, 200/40, 200/40, 200/40, 200/40); # ms/degree |
$ServoConstPpm = 20; # PPM protocol overhead in ms |
|
@ServoTest = ( [ 90, 0 ], # Pan, Tilt in degrees for servo test |
[ 180, 0 ], |
[ 180, 90 ], |
[ 90, 0 ], |
[ 0, 0 ], |
[ 0, 90 ], |
[ 90, 0 ], |
[ 90, 180 ], |
[ 90, 0 ], ); |
|
# Tracking |
$TrackInterval = 50; # in ms |
|
# |
# 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 |
undef $ServoPort; |
|
if ( $ComPort =~ /^COM/i ) |
{ |
$ComPort = "\\\\.\\" . $ComPort; # for Port > 9 required |
} |
|
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"; |
} |
|
if ( ! ($Cfg->{'track'}->{'PortSetSkip'} =~ /y/i) ) |
{ |
# Set COM parameters, don't set for Bluetooth device |
$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: Set Speed 0, 1..127, 0=full speed |
# Speed Pan/Tilt servo #0/#1 |
my $Output = pack('C*', 0x80, 0x01, 0x01, 0, $ServoPanSpeed ); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x01, 1, $ServoTiltSpeed ); |
$ServoPort->write($Output); |
|
# Byte 5: Set Parameter |
# Bit 6: Servo on/off |
# Bit 5: Direction |
# Bit 4-0: Servo Range |
# Set Pan/Tilt servo #0/#1 |
my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x40 | $ServoPanCtrl ); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x00, 1, 0x40 | $ServoTiltCtrl ); |
$ServoPort->write($Output); |
|
# Byte 5/6: Neutral in 0.5us |
# Neutral Pan/Tilt servo #0/#1 |
my $Output = pack('C*', 0x80, 0x01, 0x05, 0, $ServoPanNeutral >> 7, $ServoPanNeutral & 0x7f); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x05, 1, $ServoTiltNeutral >> 7, $ServoTiltNeutral & 0x7f); |
$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 = (90, 90, 90, 90, 90, 90, 90, 90); # 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 = int ($Angel * 255/180 + 0.5); # angel 0..180 degree to servo position 0..255 |
|
# output to COM port |
# Byte 1: sync - Pololu Mode |
# Byte 2: device |
# Byte 3: command |
# Byte 4: Servo num |
# Byte 5/6: servo position 0..255 |
|
my $Output = pack('C*', 0x80, 0x01, 0x03, $Num, $Pos >> 7, $Pos & 0x7f); |
$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; |
|
return $ServoEndTime[$Num]; |
} |
|
|
# switch off servo |
sub ServoRelax() |
{ |
if ( defined $ServoPort ) |
{ |
# Byte 1: sync - Pololu Mode |
# Byte 2: device |
# Byte 3: command |
# Byte 4: Servo num |
|
# Byte 5: Set Parameter |
# Bit 6: Servo on/off |
# Bit 5: Direction |
# Bit 4-0: Servo Range |
# Set Pan/Tilt servo #0/#1 |
my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x00 | $ServoPanCtrl ); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x00, 1, 0x00 | $ServoTiltCtrl ); |
$ServoPort->write($Output); |
} |
} |
|
|
# Check, if servo has reached end position |
sub ServoCheck() |
{ |
my $Num = $_[0]; |
return &CheckTimer_ms($ServoEndTime[$Num]); |
} |
|
|
sub ServoClose() |
{ |
# close COM-Port |
undef $ServoPort; |
} |
|
|
|
|
# |
# Track it |
# |
|
sub TrackAntennaGps() |
{ |
|
# initialize system-timer |
$SysTimerCount_ms = 0; |
$SysTimerError = 0; |
($t0_s, $t0_us) = gettimeofday; |
|
# |
# State maschine |
# |
|
my $State = "Idle"; |
while (1) |
{ |
|
$MkTrack{'State'} = $State; # export state |
|
# |
# Idle |
# |
if ( $State eq "Idle" ) |
{ |
# nothing to do. Wait for commands in TrackQueue |
} |
|
# |
# ColdStart |
# |
elsif ( $State eq "ColdStart" ) |
{ |
&ServoInit(); |
|
|
$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 ) |
{ |
my $Delay = 200; |
my $LastPan = $ServoPos[$ServoPan]; |
my $LastTilt = $ServoPos[$ServoTilt]; |
my $EstimatedPan = 1000; |
if ( $ServoPanSpeed != 0 ) |
{ |
# about 2ms/180degree pulse, 50us/s servo pulse change per unit |
$EstimatedPan = abs($PanPos - $LastPan) * 0.002/180 / ($ServoPanSpeed * 0.000050) * 1000 + $Delay; |
} |
my $EstimatedTilt = 1000; |
if ( $ServoTiltSpeed != 0 ) |
{ |
# about 2ms/180degree pulse, 50us/s servo pulse change per unit |
$EstimatedTilt = abs($TiltPos - $LastTilt) * 0.002/180 / ($ServoTiltSpeed * 0.000050) * 1000 + $Delay; |
} |
|
&ServoMove ($ServoPan, $PanPos, $EstimatedPan); # override travel time |
&ServoMove ($ServoTilt, $TiltPos, $EstimatedTilt); # override 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) ) |
{ |
lock (%MkOsd); # until end of block |
lock (%MkTrack); |
|
if ( $MkOsd{'_Timestamp'} >= time-2 and |
$MkOsd{'SatsInUse'} >= 6 ) |
{ |
# gültige OSD daten vom MK und guter Satellitenempfang |
|
# take GPS and compass from map definition or MK as antenna home-position |
$MkTrack{'HomePos_Lon'} = $Map{'Track_Lon'} || $MkOsd{'HomePos_Lon'}; |
$MkTrack{'HomePos_Lat'} = $Map{'Track_Lat'} || $MkOsd{'HomePos_Lat'}; |
$MkTrack{'HomePos_Alt'} = $Map{'Track_Alt'} || $MkOsd{'HomePos_Alt'}; |
$MkTrack{'CompassHeading'} = $Map{'Track_Bearing'} || $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 |
|
lock (%MkOsd); # until end of block |
lock (%MkTrack); |
|
if ( $MkOsd{'_Timestamp'} >= time -2 and |
$MkOsd{'SatsInUse'} >= 4 ) |
{ |
# valid OSD data from the MK and sufficient satellites |
|
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 < 4 ) # meter |
{ |
# Too close to Home-Position. Set antenna to middle position |
$Bearing = $Dir_h; |
} |
|
$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'}; |
|
# antenna pan direction: 0..180 degree, centre = 90 |
my $AngelPan = $Bearing - $Dir_h + 90; |
$AngelPan = $AngelPan % 360; |
|
# antenna tilt direction: 0..180 degree, centre is up, 0 is front |
my $AngelTilt = rad2deg(atan2(($MkOsd{'CurPos_Alt'} - $MkTrack{'HomePos_Alt'}), $Dist)); |
if ( $AngelTilt < 0 ) { $AngelTilt = 0; } |
if ( $AngelTilt > 180 ) { $AngelTilt = 180; } |
|
if ( $AngelPan >= 180 ) |
{ |
# Flip Pan/Tilt |
$AngelPan = $AngelPan - 180; |
$AngelTilt = 180 - $AngelTilt; |
} |
|
$MkTrack{'AngelPan'} = $AngelPan; |
$MkTrack{'AngelTilt'} = $AngelTilt; |
|
&ServoMove ($ServoPan, $AngelPan); |
&ServoMove ($ServoTilt, $AngelTilt); |
|
# Timestamp, wann der Datensatz geschtieben wurde |
$MkTrack{'_Timestamp'} = time; |
} |
} |
} |
|
else |
{ |
# Restart |
$State = "ColdStart"; |
} |
|
# |
# check command queue |
# |
while ( $TrackQueue->pending() > 0 ) |
{ |
my $Cmd = $TrackQueue->dequeue(1); |
|
if ( $Cmd =~ /COLDSTART/i ) |
{ |
$State = "ColdStart"; |
} |
|
if ( $Cmd =~ /IDLE/i ) |
{ |
if ( defined $ServoPort ) |
{ |
# move all Servo to neutral position |
&ServoMove ($ServoPan, 90); |
&ServoMove ($ServoTilt, 0); |
|
sleep 1; |
&ServoRelax(); # swith off servo |
} |
|
$State = "Idle"; |
} |
} |
|
# |
# update system-timer |
# |
($t1_s, $t1_us) = gettimeofday; |
$SysTimerSleep_us = ($t0_s - $t1_s) * 1000000 + $t0_us - $t1_us + $SysTimerCount_ms * $SysTimerResolution; |
|
if ($SysTimerSleep_us > 0) |
{ |
usleep ($SysTimerSleep_us); |
} |
else |
{ |
$SysTimerError ++; |
} |
|
$SysTimerCount_ms ++; |
} |
} |
|
|
# |
# Main Program |
# |
|
if ( $0 =~ /track.pl$/i ) |
{ |
# Program wurde direkt aufgerufen |
|
# |
# Commandline Parameter |
# |
my $TrackPort = $CmdLine{'-TrackPort'}; |
if ( $TrackPort ne "" ) |
{ |
$ComPort = $TrackPort; |
} |
|
my $MkPort = $CmdLine{'-MkPort'}; |
if ( $MkPort ne "" ) |
{ |
$Cfg->{'mkcomm'}->{'Port'} = $MkPort; |
} |
|
my $PanCtrl = $CmdLine{'-PanCtrl'}; |
if ( $PanCtrl ne "" ) |
{ |
$ServoPanCtrl = $PanCtrl; |
} |
|
my $TiltCtrl = $CmdLine{'-TiltCtrl'}; |
if ( $TiltCtrl ne "" ) |
{ |
$ServoTiltCtrl = $TiltCtrl; |
} |
|
my $PanSpeed = $CmdLine{'-PanSpeed'}; |
if ( $PanSpeed ne "" ) |
{ |
$ServoPanSpeed = $PanSpeed; |
} |
|
my $TiltSpeed = $CmdLine{'-TiltSpeed'}; |
if ( $TiltSpeed ne "" ) |
{ |
$ServoTiltSpeed = $TiltSpeed; |
} |
|
my $PanNeutral = $CmdLine{'-PanNeutral'}; |
if ( $PanNeutral ne "" ) |
{ |
$ServoPanNeutral = $PanNeutral; |
} |
|
my $TiltNeutral = $CmdLine{'-TiltNeutral'}; |
if ( $TiltNeutral ne "" ) |
{ |
$ServoTiltNeutral = $TiltNeutral; |
} |
|
# Kommunikation zum MK herstellen |
# Input: %MkOsd, %MkTarget, %MkNcDebug |
# Ouput: Thread-Queue: $MkSendQueue |
$mk_thr = threads->create (\&MkCommLoop) -> detach(); |
|
$TrackQueue->enqueue("COLDSTART"); # start command |
&TrackAntennaGps(); |
|
# should never exit |
} |
|
1; |
|
__END__ |