| 0,0 → 1,445 |
|---|
| #!/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__ |