Subversion Repositories Projects

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

$Version{'track.pl'} = "0.1.5 - 2010-01-02";


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',
                                                         '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 $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__