#!/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__
