0,0 → 1,722 |
#!/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 |
# 2010-01-02 0.1.5 rw bugfix |
# 2010-03-12 0.1.6 rw Pololu Maestro-6 Controller |
# |
############################################################################### |
|
$Version{'track.pl'} = "0.1.6 - 2010-03-12"; |
|
|
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 |
# |
|
$SysTimerResolution = 1000; # System Timer resolution in us |
$TrackInterval = 50; # Tracking in ms |
|
# 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"; |
|
# 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 |
|
# Servo Parameter defaults |
@ServoPar = ( { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 0 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 1 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 2 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 3 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 4 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 5 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 6 |
{ Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 7 |
); |
|
# Read configuration |
$ServoPar[$ServoPan]{'Min'} = $Cfg->{'track'}->{'ServoPanLeft'} || "1000"; |
$ServoPar[$ServoPan]{'Mid'} = $Cfg->{'track'}->{'ServoPanMiddle'} || "1500"; |
$ServoPar[$ServoPan]{'Max'} = $Cfg->{'track'}->{'ServoPanRight'} || "2000"; |
$ServoPar[$ServoPan]{'Speed'} = $Cfg->{'track'}->{'ServoPanSpeed'} || "0"; |
$ServoPar[$ServoPan]{'Accel'} = $Cfg->{'track'}->{'ServoPanAccel'} || "0"; |
|
$ServoPar[$ServoTilt]{'Min'} = $Cfg->{'track'}->{'ServoTiltFront'} || "1000"; |
$ServoPar[$ServoTilt]{'Mid'} = $Cfg->{'track'}->{'ServoTiltTop'} || "1500"; |
$ServoPar[$ServoTilt]{'Max'} = $Cfg->{'track'}->{'ServoTiltBack'} || "2000"; |
$ServoPar[$ServoTilt]{'Speed'} = $Cfg->{'track'}->{'ServoTiltSpeed'} || "0"; |
$ServoPar[$ServoTilt]{'Accel'} = $Cfg->{'track'}->{'ServoTiltAccel'} || "0"; |
|
my $ServoController = $Cfg->{'track'}->{'ServoController'} || "Pololu Micro Serial"; |
|
# Debug |
$MkTrack{'ServoPanLeft'} = $ServoPar[$ServoPan]{'Min'}; |
$MkTrack{'ServoPanMiddle'} = $ServoPar[$ServoPan]{'Mid'}; |
$MkTrack{'ServoPanRight'} = $ServoPar[$ServoPan]{'Max'}; |
$MkTrack{'ServoPanSpeed'} = $ServoPar[$ServoPan]{'Speed'}; |
$MkTrack{'ServoPanAccel'} = $ServoPar[$ServoPan]{'Accel'}; |
|
$MkTrack{'ServoTiltLeft'} = $ServoPar[$ServoTilt]{'Min'}; |
$MkTrack{'ServoTiltMiddle'} = $ServoPar[$ServoTilt]{'Mid'}; |
$MkTrack{'ServoTiltRight'} = $ServoPar[$ServoTilt]{'Max'}; |
$MkTrack{'ServoTiltSpeed'} = $ServoPar[$ServoTilt]{'Speed'}; |
$MkTrack{'ServoTiltAccel'} = $ServoPar[$ServoTilt]{'Accel'}; |
|
$MkTrack{'ServoController'} = $ServoController; |
|
# Pan, Tilt in degrees for servo test |
@ServoTest = ( [ 90, 0 ], |
[ 180, 0 ], |
[ 180, 90 ], |
[ 90, 0 ], |
[ 0, 0 ], |
[ 0, 90 ], |
[ 90, 0 ], |
[ 90, 180 ], |
[ 90, 0 ], ); |
|
# |
# 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; |
} |
|
if ( $ServoController =~ /Pololu Micro Serial/i ) |
{ |
# 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, $ServoPan, $ServoPar[$ServoPan]{'Speed'} ); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x01, $ServoTilt, $ServoPar[$ServoTilt]{'Speed'} ); |
$ServoPort->write($Output); |
|
# Acceleration not supported |
} |
|
elsif ( $ServoController =~ /Pololu Maestro/i ) |
{ |
# Pololu compact protocoll |
# Byte 1: Command |
# Byte 2: device |
# Byte 3: low bits 0..6 |
# Byte 4: high bits 7..14 |
|
# Speed |
my $Speed = $ServoPar[$ServoPan]{'Speed'}; |
my $Output = pack('C*', 0x87, $ServoPan, $Speed & 0x7f, ($Speed >> 7) & 0x7f ); |
$ServoPort->write($Output); |
|
my $Speed = $ServoPar[$ServoTilt]{'Speed'}; |
my $Output = pack('C*', 0x87, $ServoTilt, $Speed & 0x7f, ($Speed >> 7) & 0x7f ); |
$ServoPort->write($Output); |
|
# Acceleration |
my $Accel = $ServoPar[$ServoPan]{'Accel'}; |
my $Output = pack('C*', 0x89, $ServoPan, $Accel & 0x7f, ($Accel >> 7) & 0x7f ); |
$ServoPort->write($Output); |
|
my $Accel = $ServoPar[$ServoTilt]{'Accel'}; |
my $Output = pack('C*', 0x89, $ServoTilt, $Accel & 0x7f, ($Accel >> 7) & 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 = $Angel - 90; # -90..0..90 |
|
my $Min = $ServoPar[$Num]{'Min'}; |
my $Mid = $ServoPar[$Num]{'Mid'}; |
my $Max = $ServoPar[$Num]{'Max'}; |
|
if ( $Pos >= 0 ) |
{ |
$Pos = int ($Mid + $Pos / 90 * ($Max - $Mid) + 0.5); |
} |
else |
{ |
$Pos = int ($Mid + $Pos / 90 * ($Mid - $Min) + 0.5); |
} |
|
if ( $ServoController =~ /Pololu Micro Serial/i ) |
{ |
# output to COM port |
# Byte 1: sync - Pololu Mode |
# Byte 2: device |
# Byte 3: command |
# Byte 4: Servo num |
# Byte 5: bits 7..14 in 0.5 us |
# Byte 6: bits 0..6 in 0.5 us |
|
$Pos *= 2; # 0.5 us resolution |
|
my $Output = pack('C*', 0x80, 0x01, 0x04, $Num, ($Pos >> 7) & 0x7f, $Pos & 0x7f ); |
$ServoPort->write($Output); |
} |
|
elsif ( $ServoController =~ /Pololu Maestro/i ) |
{ |
# Pololu compact protocoll |
# Byte 1: Command |
# Byte 2: device |
# Byte 3: low bits 0..6 in 0.25 us |
# Byte 4: high bits 7..14 in 0.25 us |
|
$Pos *= 4; # 0.25 us resolution |
|
my $Output = pack('C*', 0x84, $Num, $Pos & 0x7f, ($Pos >> 7) & 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 ) |
{ |
if ( $ServoController =~ /Pololu Micro Serial/i ) |
{ |
|
# 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, $ServoPan, 0x00 | 15 ); |
$ServoPort->write($Output); |
my $Output = pack('C*', 0x80, 0x01, 0x00, $ServoTilt, 0x00 | 15 ); |
$ServoPort->write($Output); |
} |
|
elsif ( $ServoController =~ /Pololu Maestro/i ) |
{ |
# not supported |
} |
} |
} |
|
|
# 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 $ServoPanSpeed = $ServoPar[$ServoPan]{'Speed'}; |
my $ServoTiltSpeed = $ServoPar[$ServoTilt]{'Speed'}; |
|
if ( $ServoController =~ /Pololu Micro Serial/i ) |
{ |
# 50 us/s |
} |
elsif ( $ServoController =~ /Pololu Maestro/i ) |
{ |
# 25 us/s |
$ServoPanSpeed /= 2; |
$ServoTiltSpeed /= 2; |
} |
|
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', |
'longitude' => 1, # Symmetric: -pi..pi |
); |
|
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 $Pan = $CmdLine{'-ServoPan'}; |
if ( $Pan ne "" ) |
{ |
my ($Min, $Mid, $Max) = split ',', $Pan; |
$ServoPar[$ServoPan]{'Min'} = $Min; |
$ServoPar[$ServoPan]{'Mid'} = $Mid; |
$ServoPar[$ServoPan]{'Max'} = $Max; |
} |
|
my $Tilt = $CmdLine{'-ServoTilt'}; |
if ( $Tilt ne "" ) |
{ |
my ($Min, $Mid, $Max) = split ',', $Tilt; |
$ServoPar[$ServoTilt]{'Min'} = $Min; |
$ServoPar[$ServoTilt]{'Mid'} = $Mid; |
$ServoPar[$ServoTilt]{'Max'} = $Max; |
} |
|
my $PanSpeed = $CmdLine{'-PanSpeed'}; |
if ( $PanSpeed ne "" ) |
{ |
$ServoPar[$ServoPan]{'Speed'} = $PanSpeed; |
} |
|
my $TiltSpeed = $CmdLine{'-TiltSpeed'}; |
if ( $TiltSpeed ne "" ) |
{ |
$ServoPar[$ServoTilt]{'Speed'} = $TiltSpeed; |
} |
|
# 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__ |