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

$Version{'track.pl'} = "0.1.0 - 2009-04-01";

# Packages
use Time::HiRes qw(usleep gettimeofday);   # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm
use threads;
use threads::shared;
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);

#
# 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
if ( ! defined $Cfg->{'track'}->{'Port'} )
    {
    # set default port
    $Cfg->{'track'}->{'Port'} = "COM8";
    }

# 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 = ( [  45,  45 ],       # Pan, Tilt in degrees for servo test
               [   0,   0 ],
               [  45,  45 ],
               [  90,  90 ],
               [ 135,  45 ],
               [ 180,   0 ],
               [ 135,  45 ],
               [  90,  90 ] );

# Tracking
$TrackInterval = 100;    # in ms
#
# Logging
#
sub Log()
    {
    my ($Message, $Level) = @_;

    if ( $LogOn )
        {
        print ("$SysTimerCount_ms: $Message\n");
        }
    }
#
# 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
    my $ComPort = $Cfg->{'track'}->{'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
    my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x60 | 27 );
    $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 = $Angel * 127/180;       # 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;

    &Log ("ServoMove: Num: $Num : LastPos: $LastAngel NewPos: $Angel Estimated: $EstimatedTime ms");

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

#
# Signal handler
#

$SIG{'INT'}  = 'SigHandler';
$SIG{'KILL'} = 'SigHandler';

sub SigHandler()
    {
    # move all Servo to neutral position
    &ServoMove ($ServoPan, 90);
    &ServoMove ($ServoTilt, 90);
       
    &ServoClose();

    if ( defined threads->self() )
        {
        threads->exit();
        }
    exit;
    }


#
# Track it
#

sub TrackAntennaGps()
    {

    #
    # State maschine
    #

    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) )
                {
                if ( $MkOsd{'_Timestamp'} >= time-2  and
                     $MkOsd{'SatsInUse'} >= 6 )
                    {
                    # gültige OSD daten vom MK und guter Satellitenempfang

                    # take GPS and compass from MK as antenna home-position
                    $MkTrack{'HomePos_Lon'} = $MkOsd{'HomePos_Lon'};
                    $MkTrack{'HomePos_Lat'} = $MkOsd{'HomePos_Lat'};
                    $MkTrack{'HomePos_Alt'} = $MkOsd{'HomePos_Alt'};
                    if ( $MkTrack{'CompassHeading'} eq "" )
                        {
                        # nur beim ersten mal
                        $MkTrack{'CompassHeading'} = $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
       
                if ( $MkOsd{'_Timestamp'} >= time -2  and
                     $MkOsd{'SatsInUse'} >= 4 )
                    {
                    # valid OSD data from the MK and sufficient satellites
                    lock (%MkOsd);     # until end of block
                    lock (%MkTrack);
               
                    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 < 2 )  # meter
                        {
                        # Too close to Home-Position. Set antenna to middle position
                        $Bearing = $Dir_h + 90;                                        
                        }

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

                    my $AngelPan = $Bearing - $Dir_h + 90;   # antenna direction: 0..180 degree, centre = 90
                    while ( $AngelPan >= 360 )
                        {
                        $AngelPan -= 360;
                        }

                    $MkTrack{'AngelPan'} = $AngelPan;
                    &ServoMove ($ServoPan, $AngelPan);
               
                                        # determine tilt angle
                                        my $AngelTilt = rad2deg(atan2(($MkOsd{'CurPos_Alt'} - $MkTrack{'HomePos_Alt'}), $Dist)) + 90; # antenna direction: 0..180 degree, center = 90
                                        #if ( $AngelTilt < 90 ) { $AngelTilt = 90; }
                    $MkTrack{'AngelTilt'} = $AngelTilt;
                    &ServoMove ($ServoTilt, $AngelTilt);

                    # Timestamp, wann der Datensatz geschtieben wurde
                    $MkTrack{'_Timestamp'} = time;
                    }
                }
            }

        else
            {
            # Restart
            &Log ("WARNING: Error in state machine - $State - ColdStart");

            $State = "ColdStart";
            }

        #
        # update system-timer
        #
        ($t1_s, $t1_us) = gettimeofday;
        $SysTimerSleep_us = ($t0_s - $t1_s) * 1000000 + $t0_us - $t1_us + $SysTimerCount_ms * $SysTimerResolution;

        #&Log ("SysTimerSleep_us: $SysTimerSleep_us");

        if ($SysTimerSleep_us > 0)
            {
            usleep ($SysTimerSleep_us);
            }
        else
            {
            $SysTimerError ++;
            }

        $SysTimerCount_ms ++;
        }
    }


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