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

#

###############################################################################

$Version{'track.pl'} = "0.1.1 - 2009-08-15";

# 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();

#

# 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 | 29);

my $ServoTiltCtrl = $Cfg->{'track'}->{'ServoTiltCtrl'} || (0x20 | 27);

$MkTrack{'ServoPanCtrl'} = $ServoPanCtrl;

$MkTrack{'ServoTiltCtrl'} = $ServoPanCtrl;

# 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 = ( [  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 ( $^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

    # Pan servo #0

    my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x40 | $ServoPanCtrl );

    $ServoPort->write($Output);

    # Tilt servo #1

    my $Output = pack('C*', 0x80, 0x01, 0x00, 1, 0x40 | $ServoTiltCtrl );

    $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 = int ($Angel * 127/180 + 0.5);   # 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;

    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;

    }

#

# Track it

#

sub TrackAntennaGps()

    {

    #

    # State maschine

    #

    $MkTrack{'IsRunning'} = "Running";

    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) )

                {

                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'};

                    if ( $MkTrack{'CompassHeading'} eq "" )

                        {

                        # nur beim ersten mal

                        $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 =~ /EXIT/i )

                {

                &TrackExit();

                }

            }

        #

        # 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 ++;

        }

    }

sub TrackExit()

    {

    $MkTrack{'IsRunning'} = "";

    $State = "ColdStart";

    # move all Servo to neutral position

    &ServoMove ($ServoPan, 90);

    &ServoMove ($ServoTilt, 0);

    sleep 2;

    # &ServoClose();

    # if ( defined threads->self() )

    #     {

    #     threads->exit();

    #     }

    # exit;

    }

#

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