Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 779 | - | 1 | #!/usr/bin/perl |
| 2 | #!/usr/bin/perl -d:ptkdb |
||
| 3 | |||
| 4 | ############################################################################### |
||
| 5 | # |
||
| 6 | # mktrack.pl - Tracking Antenne |
||
| 7 | # |
||
| 8 | # Copyright (C) 2009 Rainer Walther (rainerwalther-mail@web.de) |
||
| 9 | # |
||
| 10 | # Creative Commons Lizenz mit den Zusaetzen (by, nc, sa) |
||
| 11 | # |
||
| 12 | # Es ist Ihnen gestattet: |
||
| 13 | # * das Werk vervielfältigen, verbreiten und öffentlich zugänglich machen |
||
| 14 | # * Abwandlungen bzw. Bearbeitungen des Inhaltes anfertigen |
||
| 15 | # |
||
| 16 | # Zu den folgenden Bedingungen: |
||
| 17 | # * Namensnennung. |
||
| 18 | # Sie müssen den Namen des Autors/Rechteinhabers in der von ihm festgelegten Weise nennen. |
||
| 19 | # * Keine kommerzielle Nutzung. |
||
| 20 | # Dieses Werk darf nicht für kommerzielle Zwecke verwendet werden. |
||
| 21 | # * Weitergabe unter gleichen Bedingungen. |
||
| 22 | # Wenn Sie den lizenzierten Inhalt bearbeiten oder in anderer Weise umgestalten, |
||
| 23 | # verändern oder als Grundlage für einen anderen Inhalt verwenden, |
||
| 24 | # dürfen Sie den neu entstandenen Inhalt nur unter Verwendung von Lizenzbedingungen |
||
| 25 | # weitergeben, die mit denen dieses Lizenzvertrages identisch oder vergleichbar sind. |
||
| 26 | # |
||
| 27 | # Im Falle einer Verbreitung müssen Sie anderen die Lizenzbedingungen, unter welche dieses |
||
| 28 | # Werk fällt, mitteilen. Am Einfachsten ist es, einen Link auf diese Seite einzubinden. |
||
| 29 | # |
||
| 30 | # Jede der vorgenannten Bedingungen kann aufgehoben werden, sofern Sie die Einwilligung |
||
| 31 | # des Rechteinhabers dazu erhalten. |
||
| 32 | # |
||
| 33 | # Diese Lizenz lässt die Urheberpersönlichkeitsrechte unberührt. |
||
| 34 | # |
||
| 35 | # Weitere Details zur Lizenzbestimmung gibt es hier: |
||
| 36 | # Kurzform: http://creativecommons.org/licenses/by-nc-sa/3.0/de/ |
||
| 37 | # Komplett: http://creativecommons.org/licenses/by-nc-sa/3.0/de/legalcode |
||
| 38 | # |
||
| 39 | ############################################################################### |
||
| 40 | # 2009-02-14 0.0.1 rw created |
||
| 41 | # 2009-04-01 0.1.0 rw RC1 |
||
| 42 | # 2009-06-14 0.1.1 rw Tilt-Servo added |
||
| 43 | # 2009-08-15 0.1.2 rw Flip Pan/Tilt servo for >180 degree |
||
| 44 | # config servo direction and range |
||
| 45 | # optional get antenna home position from Map-config |
||
| 46 | # Signal handler replaced by command-queue |
||
| 47 | # 2009-09-30 0.1.3 rw Commandline parameter added |
||
| 48 | # 2009-10-07 0.1.4 rw COM-Port > 9 |
||
| 49 | # Servo Speed, neutral position and 8 bit position |
||
| 50 | # Add Coldstart command |
||
| 51 | # Relax servo at shutdown |
||
| 52 | # PortSetSkip config |
||
| 53 | # 2010-01-02 0.1.5 rw bugfix |
||
| 54 | # 2010-03-12 0.1.6 rw Pololu Maestro-6 Controller |
||
| 55 | # |
||
| 56 | ############################################################################### |
||
| 57 | |||
| 58 | $Version{'track.pl'} = "0.1.6 - 2010-03-12"; |
||
| 59 | |||
| 60 | |||
| 61 | if ( $0 =~ /track.pl$/i ) |
||
| 62 | { |
||
| 63 | # Program wurde direkt aufgerufen |
||
| 64 | |||
| 65 | # change working directory to program path |
||
| 66 | my $Cwd = substr ($0, 0, rindex ($0, "track.pl")); |
||
| 67 | chdir $Cwd; |
||
| 68 | |||
| 69 | # set path for local Perl libs |
||
| 70 | push @INC, $Cwd . "perl/lib"; |
||
| 71 | } |
||
| 72 | |||
| 73 | |||
| 74 | # Packages |
||
| 75 | use Time::HiRes qw(usleep gettimeofday); # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm |
||
| 76 | use threads; # http://search.cpan.org/~jdhedden/threads-1.72/threads.pm |
||
| 77 | # http://perldoc.perl.org/threads.html |
||
| 78 | use threads::shared; # http://search.cpan.org/~jdhedden/threads-shared-1.28/shared.pm |
||
| 79 | use Thread::Queue; # http://search.cpan.org/dist/Thread-Queue-2.11/lib/Thread/Queue.pm |
||
| 80 | use Math::Trig; |
||
| 81 | use Geo::Ellipsoid; # http://search.cpan.org/dist/Geo-Ellipsoid-1.12/lib/Geo/Ellipsoid.pm |
||
| 82 | # http://www.kompf.de/gps/distcalc.html |
||
| 83 | # http://www.herrmann-w.de/Geocaching/Downloads/Richt.XLS |
||
| 84 | # http://williams.best.vwh.net/avform.htm |
||
| 85 | # http://williams.best.vwh.net/gccalc.html |
||
| 86 | # http://de.wikipedia.org/wiki/Orthodrome |
||
| 87 | if ( $^O =~ /Win32/i ) |
||
| 88 | { |
||
| 89 | require Win32::SerialPort; # http://search.cpan.org/dist/Win32-SerialPort |
||
| 90 | } |
||
| 91 | else |
||
| 92 | { |
||
| 93 | require Device::SerialPort; # http://search.cpan.org/~cook/Device-SerialPort-1.04/SerialPort.pm |
||
| 94 | } |
||
| 95 | |||
| 96 | require "mkcomm.pl"; # MK communication |
||
| 97 | |||
| 98 | # Sharing for Threads |
||
| 99 | share (@ServoPos); |
||
| 100 | share (%MkTrack); |
||
| 101 | |||
| 102 | # Queue for receiving commands |
||
| 103 | $TrackQueue = Thread::Queue->new(); |
||
| 104 | |||
| 105 | # Commandline |
||
| 106 | my %CmdLine = @ARGV; |
||
| 107 | |||
| 108 | # |
||
| 109 | # Parameter |
||
| 110 | # |
||
| 111 | |||
| 112 | $SysTimerResolution = 1000; # System Timer resolution in us |
||
| 113 | $TrackInterval = 50; # Tracking in ms |
||
| 114 | |||
| 115 | # Com Port for Pololu Mikro-Servoboard |
||
| 116 | # http://www.shop.robotikhardware.de/shop/catalog/product_info.php?cPath=65&products_id=118 |
||
| 117 | my $ComPort = $Cfg->{'track'}->{'Port'} || "COM8"; |
||
| 118 | |||
| 119 | # Servo parameter |
||
| 120 | $ServoPan = 0; # Servo channel Pan |
||
| 121 | $ServoTilt = 1; # Servo channel Tilt |
||
| 122 | $MkTrack{'ServoPan'} = $ServoPan; |
||
| 123 | $MkTrack{'ServoTilt'} = $ServoTilt; |
||
| 124 | @ServoSpeed = (200/40, 200/40, 200/40, 200/40, 200/40, 200/40, 200/40, 200/40); # ms/degree |
||
| 125 | $ServoConstPpm = 20; # PPM protocol overhead in ms |
||
| 126 | |||
| 127 | # Servo Parameter defaults |
||
| 128 | @ServoPar = ( { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 0 |
||
| 129 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 1 |
||
| 130 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 2 |
||
| 131 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 3 |
||
| 132 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 4 |
||
| 133 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 5 |
||
| 134 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 6 |
||
| 135 | { Min => 1000, Mid => 1500, Max => 2000, Speed => 0, Accel => 0, }, # Servo 7 |
||
| 136 | ); |
||
| 137 | |||
| 138 | # Read configuration |
||
| 139 | $ServoPar[$ServoPan]{'Min'} = $Cfg->{'track'}->{'ServoPanLeft'} || "1000"; |
||
| 140 | $ServoPar[$ServoPan]{'Mid'} = $Cfg->{'track'}->{'ServoPanMiddle'} || "1500"; |
||
| 141 | $ServoPar[$ServoPan]{'Max'} = $Cfg->{'track'}->{'ServoPanRight'} || "2000"; |
||
| 142 | $ServoPar[$ServoPan]{'Speed'} = $Cfg->{'track'}->{'ServoPanSpeed'} || "0"; |
||
| 143 | $ServoPar[$ServoPan]{'Accel'} = $Cfg->{'track'}->{'ServoPanAccel'} || "0"; |
||
| 144 | |||
| 145 | $ServoPar[$ServoTilt]{'Min'} = $Cfg->{'track'}->{'ServoTiltFront'} || "1000"; |
||
| 146 | $ServoPar[$ServoTilt]{'Mid'} = $Cfg->{'track'}->{'ServoTiltTop'} || "1500"; |
||
| 147 | $ServoPar[$ServoTilt]{'Max'} = $Cfg->{'track'}->{'ServoTiltBack'} || "2000"; |
||
| 148 | $ServoPar[$ServoTilt]{'Speed'} = $Cfg->{'track'}->{'ServoTiltSpeed'} || "0"; |
||
| 149 | $ServoPar[$ServoTilt]{'Accel'} = $Cfg->{'track'}->{'ServoTiltAccel'} || "0"; |
||
| 150 | |||
| 151 | my $ServoController = $Cfg->{'track'}->{'ServoController'} || "Pololu Micro Serial"; |
||
| 152 | |||
| 153 | # Debug |
||
| 154 | $MkTrack{'ServoPanLeft'} = $ServoPar[$ServoPan]{'Min'}; |
||
| 155 | $MkTrack{'ServoPanMiddle'} = $ServoPar[$ServoPan]{'Mid'}; |
||
| 156 | $MkTrack{'ServoPanRight'} = $ServoPar[$ServoPan]{'Max'}; |
||
| 157 | $MkTrack{'ServoPanSpeed'} = $ServoPar[$ServoPan]{'Speed'}; |
||
| 158 | $MkTrack{'ServoPanAccel'} = $ServoPar[$ServoPan]{'Accel'}; |
||
| 159 | |||
| 160 | $MkTrack{'ServoTiltLeft'} = $ServoPar[$ServoTilt]{'Min'}; |
||
| 161 | $MkTrack{'ServoTiltMiddle'} = $ServoPar[$ServoTilt]{'Mid'}; |
||
| 162 | $MkTrack{'ServoTiltRight'} = $ServoPar[$ServoTilt]{'Max'}; |
||
| 163 | $MkTrack{'ServoTiltSpeed'} = $ServoPar[$ServoTilt]{'Speed'}; |
||
| 164 | $MkTrack{'ServoTiltAccel'} = $ServoPar[$ServoTilt]{'Accel'}; |
||
| 165 | |||
| 166 | $MkTrack{'ServoController'} = $ServoController; |
||
| 167 | |||
| 168 | # Pan, Tilt in degrees for servo test |
||
| 169 | @ServoTest = ( [ 90, 0 ], |
||
| 170 | [ 180, 0 ], |
||
| 171 | [ 180, 90 ], |
||
| 172 | [ 90, 0 ], |
||
| 173 | [ 0, 0 ], |
||
| 174 | [ 0, 90 ], |
||
| 175 | [ 90, 0 ], |
||
| 176 | [ 90, 180 ], |
||
| 177 | [ 90, 0 ], ); |
||
| 178 | |||
| 179 | # |
||
| 180 | # Timer |
||
| 181 | # |
||
| 182 | |||
| 183 | sub SetTimer_ms() |
||
| 184 | { |
||
| 185 | return $SysTimerCount_ms + $_[0]; |
||
| 186 | } |
||
| 187 | |||
| 188 | sub CheckTimer_ms() |
||
| 189 | { |
||
| 190 | my $Diff = $_[0] - $SysTimerCount_ms; |
||
| 191 | return ($Diff <= 0); |
||
| 192 | } |
||
| 193 | |||
| 194 | # |
||
| 195 | # Servo |
||
| 196 | # |
||
| 197 | |||
| 198 | sub ServoInit() |
||
| 199 | { |
||
| 200 | # open COM-Port |
||
| 201 | undef $ServoPort; |
||
| 202 | |||
| 203 | if ( $ComPort =~ /^COM/i ) |
||
| 204 | { |
||
| 205 | $ComPort = "\\\\.\\" . $ComPort; # for Port > 9 required |
||
| 206 | } |
||
| 207 | |||
| 208 | if ( $^O =~ m/Win32/ ) |
||
| 209 | { |
||
| 210 | $ServoPort = Win32::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
||
| 211 | } |
||
| 212 | else |
||
| 213 | { |
||
| 214 | $ServoPort = Device::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
||
| 215 | } |
||
| 216 | |||
| 217 | if ( ! ($Cfg->{'track'}->{'PortSetSkip'} =~ /y/i) ) |
||
| 218 | { |
||
| 219 | # Set COM parameters, don't set for Bluetooth device |
||
| 220 | $ServoPort->baudrate(38400); |
||
| 221 | $ServoPort->parity("none"); |
||
| 222 | $ServoPort->databits(8); |
||
| 223 | $ServoPort->stopbits(1); |
||
| 224 | $ServoPort->handshake('none'); |
||
| 225 | $ServoPort->write_settings; |
||
| 226 | } |
||
| 227 | |||
| 228 | if ( $ServoController =~ /Pololu Micro Serial/i ) |
||
| 229 | { |
||
| 230 | # Byte 1: sync - Pololu Mode |
||
| 231 | # Byte 2: device |
||
| 232 | # Byte 3: command |
||
| 233 | # Byte 4: Servo num |
||
| 234 | |||
| 235 | # Byte 5: Set Speed 0, 1..127, 0=full speed |
||
| 236 | # Speed Pan/Tilt servo #0/#1 |
||
| 237 | my $Output = pack('C*', 0x80, 0x01, 0x01, $ServoPan, $ServoPar[$ServoPan]{'Speed'} ); |
||
| 238 | $ServoPort->write($Output); |
||
| 239 | my $Output = pack('C*', 0x80, 0x01, 0x01, $ServoTilt, $ServoPar[$ServoTilt]{'Speed'} ); |
||
| 240 | $ServoPort->write($Output); |
||
| 241 | |||
| 242 | # Acceleration not supported |
||
| 243 | } |
||
| 244 | |||
| 245 | elsif ( $ServoController =~ /Pololu Maestro/i ) |
||
| 246 | { |
||
| 247 | # Pololu compact protocoll |
||
| 248 | # Byte 1: Command |
||
| 249 | # Byte 2: device |
||
| 250 | # Byte 3: low bits 0..6 |
||
| 251 | # Byte 4: high bits 7..14 |
||
| 252 | |||
| 253 | # Speed |
||
| 254 | my $Speed = $ServoPar[$ServoPan]{'Speed'}; |
||
| 255 | my $Output = pack('C*', 0x87, $ServoPan, $Speed & 0x7f, ($Speed >> 7) & 0x7f ); |
||
| 256 | $ServoPort->write($Output); |
||
| 257 | |||
| 258 | my $Speed = $ServoPar[$ServoTilt]{'Speed'}; |
||
| 259 | my $Output = pack('C*', 0x87, $ServoTilt, $Speed & 0x7f, ($Speed >> 7) & 0x7f ); |
||
| 260 | $ServoPort->write($Output); |
||
| 261 | |||
| 262 | # Acceleration |
||
| 263 | my $Accel = $ServoPar[$ServoPan]{'Accel'}; |
||
| 264 | my $Output = pack('C*', 0x89, $ServoPan, $Accel & 0x7f, ($Accel >> 7) & 0x7f ); |
||
| 265 | $ServoPort->write($Output); |
||
| 266 | |||
| 267 | my $Accel = $ServoPar[$ServoTilt]{'Accel'}; |
||
| 268 | my $Output = pack('C*', 0x89, $ServoTilt, $Accel & 0x7f, ($Accel >> 7) & 0x7f ); |
||
| 269 | $ServoPort->write($Output); |
||
| 270 | } |
||
| 271 | |||
| 272 | @ServoStartTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of last ServoMove() call |
||
| 273 | @ServoEndTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of estimated arrival at end position |
||
| 274 | @ServoPos = (90, 90, 90, 90, 90, 90, 90, 90); # Current servo position 0..180 degree |
||
| 275 | } |
||
| 276 | |||
| 277 | |||
| 278 | sub ServoMove() |
||
| 279 | { |
||
| 280 | my ($Num, $Angel, $Time) = @_; |
||
| 281 | |||
| 282 | my $Overhead = 0; |
||
| 283 | |||
| 284 | if ( $Angel != $ServoPos[$Num] ) |
||
| 285 | { |
||
| 286 | if ( $Angel < 0) {$Angel = 0;} |
||
| 287 | if ( $Angel > 180) {$Angel = 180;} |
||
| 288 | |||
| 289 | my $Pos = $Angel - 90; # -90..0..90 |
||
| 290 | |||
| 291 | my $Min = $ServoPar[$Num]{'Min'}; |
||
| 292 | my $Mid = $ServoPar[$Num]{'Mid'}; |
||
| 293 | my $Max = $ServoPar[$Num]{'Max'}; |
||
| 294 | |||
| 295 | if ( $Pos >= 0 ) |
||
| 296 | { |
||
| 297 | $Pos = int ($Mid + $Pos / 90 * ($Max - $Mid) + 0.5); |
||
| 298 | } |
||
| 299 | else |
||
| 300 | { |
||
| 301 | $Pos = int ($Mid + $Pos / 90 * ($Mid - $Min) + 0.5); |
||
| 302 | } |
||
| 303 | |||
| 304 | if ( $ServoController =~ /Pololu Micro Serial/i ) |
||
| 305 | { |
||
| 306 | # output to COM port |
||
| 307 | # Byte 1: sync - Pololu Mode |
||
| 308 | # Byte 2: device |
||
| 309 | # Byte 3: command |
||
| 310 | # Byte 4: Servo num |
||
| 311 | # Byte 5: bits 7..14 in 0.5 us |
||
| 312 | # Byte 6: bits 0..6 in 0.5 us |
||
| 313 | |||
| 314 | $Pos *= 2; # 0.5 us resolution |
||
| 315 | |||
| 316 | my $Output = pack('C*', 0x80, 0x01, 0x04, $Num, ($Pos >> 7) & 0x7f, $Pos & 0x7f ); |
||
| 317 | $ServoPort->write($Output); |
||
| 318 | } |
||
| 319 | |||
| 320 | elsif ( $ServoController =~ /Pololu Maestro/i ) |
||
| 321 | { |
||
| 322 | # Pololu compact protocoll |
||
| 323 | # Byte 1: Command |
||
| 324 | # Byte 2: device |
||
| 325 | # Byte 3: low bits 0..6 in 0.25 us |
||
| 326 | # Byte 4: high bits 7..14 in 0.25 us |
||
| 327 | |||
| 328 | $Pos *= 4; # 0.25 us resolution |
||
| 329 | |||
| 330 | my $Output = pack('C*', 0x84, $Num, $Pos & 0x7f, ($Pos >> 7) & 0x7f ); |
||
| 331 | $ServoPort->write($Output); |
||
| 332 | } |
||
| 333 | |||
| 334 | $Overhead += $ServoConstPpm; # PPM protocol overhead |
||
| 335 | } |
||
| 336 | |||
| 337 | # set timer stuff for travel time predicion |
||
| 338 | my $LastAngel = $ServoPos[$Num]; |
||
| 339 | my $EstimatedTime = abs($Angel - $LastAngel) * $ServoSpeed[$Num] + $Overhead; |
||
| 340 | if ( $Time > 0 ) |
||
| 341 | { |
||
| 342 | # Parameter override |
||
| 343 | $EstimatedTime = $Time; |
||
| 344 | } |
||
| 345 | $ServoStartTime[$Num] = $SysTimerCount_ms; |
||
| 346 | $ServoEndTime[$Num] = $SysTimerCount_ms + $EstimatedTime; |
||
| 347 | $ServoPos[$Num] = $Angel; |
||
| 348 | |||
| 349 | return $ServoEndTime[$Num]; |
||
| 350 | } |
||
| 351 | |||
| 352 | |||
| 353 | # switch off servo |
||
| 354 | sub ServoRelax() |
||
| 355 | { |
||
| 356 | if ( defined $ServoPort ) |
||
| 357 | { |
||
| 358 | if ( $ServoController =~ /Pololu Micro Serial/i ) |
||
| 359 | { |
||
| 360 | |||
| 361 | # Byte 1: sync - Pololu Mode |
||
| 362 | # Byte 2: device |
||
| 363 | # Byte 3: command |
||
| 364 | # Byte 4: Servo num |
||
| 365 | |||
| 366 | # Byte 5: Set Parameter |
||
| 367 | # Bit 6: Servo on/off |
||
| 368 | # Bit 5: Direction |
||
| 369 | # Bit 4-0: Servo Range |
||
| 370 | # Set Pan/Tilt servo #0/#1 |
||
| 371 | my $Output = pack('C*', 0x80, 0x01, 0x00, $ServoPan, 0x00 | 15 ); |
||
| 372 | $ServoPort->write($Output); |
||
| 373 | my $Output = pack('C*', 0x80, 0x01, 0x00, $ServoTilt, 0x00 | 15 ); |
||
| 374 | $ServoPort->write($Output); |
||
| 375 | } |
||
| 376 | |||
| 377 | elsif ( $ServoController =~ /Pololu Maestro/i ) |
||
| 378 | { |
||
| 379 | # not supported |
||
| 380 | } |
||
| 381 | } |
||
| 382 | } |
||
| 383 | |||
| 384 | |||
| 385 | # Check, if servo has reached end position |
||
| 386 | sub ServoCheck() |
||
| 387 | { |
||
| 388 | my $Num = $_[0]; |
||
| 389 | return &CheckTimer_ms($ServoEndTime[$Num]); |
||
| 390 | } |
||
| 391 | |||
| 392 | |||
| 393 | sub ServoClose() |
||
| 394 | { |
||
| 395 | # close COM-Port |
||
| 396 | undef $ServoPort; |
||
| 397 | } |
||
| 398 | |||
| 399 | # |
||
| 400 | # Track it |
||
| 401 | # |
||
| 402 | |||
| 403 | sub TrackAntennaGps() |
||
| 404 | { |
||
| 405 | |||
| 406 | # initialize system-timer |
||
| 407 | $SysTimerCount_ms = 0; |
||
| 408 | $SysTimerError = 0; |
||
| 409 | ($t0_s, $t0_us) = gettimeofday; |
||
| 410 | |||
| 411 | # |
||
| 412 | # State maschine |
||
| 413 | # |
||
| 414 | |||
| 415 | my $State = "Idle"; |
||
| 416 | while (1) |
||
| 417 | { |
||
| 418 | |||
| 419 | $MkTrack{'State'} = $State; # export state |
||
| 420 | |||
| 421 | # |
||
| 422 | # Idle |
||
| 423 | # |
||
| 424 | if ( $State eq "Idle" ) |
||
| 425 | { |
||
| 426 | # nothing to do. Wait for commands in TrackQueue |
||
| 427 | } |
||
| 428 | |||
| 429 | # |
||
| 430 | # ColdStart |
||
| 431 | # |
||
| 432 | elsif ( $State eq "ColdStart" ) |
||
| 433 | { |
||
| 434 | &ServoInit(); |
||
| 435 | |||
| 436 | |||
| 437 | $ServoTestIndex = 0; |
||
| 438 | |||
| 439 | $State = "InitServoTest"; |
||
| 440 | } |
||
| 441 | |||
| 442 | # |
||
| 443 | # Start servo test |
||
| 444 | # doesn't really make much sense, but looks cool:-) |
||
| 445 | # |
||
| 446 | |||
| 447 | elsif ( $State eq "InitServoTest") |
||
| 448 | { |
||
| 449 | if ( &ServoCheck ($ServoPan) and &ServoCheck ($ServoTilt) ) |
||
| 450 | { |
||
| 451 | |||
| 452 | my $PanPos = $ServoTest[$ServoTestIndex][0]; |
||
| 453 | my $TiltPos = $ServoTest[$ServoTestIndex][1]; |
||
| 454 | $ServoTestIndex ++; |
||
| 455 | |||
| 456 | if ( defined $PanPos and defined $TiltPos ) |
||
| 457 | { |
||
| 458 | my $Delay = 200; |
||
| 459 | my $LastPan = $ServoPos[$ServoPan]; |
||
| 460 | my $LastTilt = $ServoPos[$ServoTilt]; |
||
| 461 | |||
| 462 | my $ServoPanSpeed = $ServoPar[$ServoPan]{'Speed'}; |
||
| 463 | my $ServoTiltSpeed = $ServoPar[$ServoTilt]{'Speed'}; |
||
| 464 | |||
| 465 | if ( $ServoController =~ /Pololu Micro Serial/i ) |
||
| 466 | { |
||
| 467 | # 50 us/s |
||
| 468 | } |
||
| 469 | elsif ( $ServoController =~ /Pololu Maestro/i ) |
||
| 470 | { |
||
| 471 | # 25 us/s |
||
| 472 | $ServoPanSpeed /= 2; |
||
| 473 | $ServoTiltSpeed /= 2; |
||
| 474 | } |
||
| 475 | |||
| 476 | my $EstimatedPan = 1000; |
||
| 477 | if ( $ServoPanSpeed != 0 ) |
||
| 478 | { |
||
| 479 | # about 2ms/180degree pulse, 50us/s servo pulse change per unit |
||
| 480 | $EstimatedPan = abs($PanPos - $LastPan) * 0.002/180 / ($ServoPanSpeed * 0.000050) * 1000 + $Delay; |
||
| 481 | } |
||
| 482 | |||
| 483 | my $EstimatedTilt = 1000; |
||
| 484 | if ( $ServoTiltSpeed != 0 ) |
||
| 485 | { |
||
| 486 | # about 2ms/180degree pulse, 50us/s servo pulse change per unit |
||
| 487 | $EstimatedTilt = abs($TiltPos - $LastTilt) * 0.002/180 / ($ServoTiltSpeed * 0.000050) * 1000 + $Delay; |
||
| 488 | } |
||
| 489 | |||
| 490 | &ServoMove ($ServoPan, $PanPos, $EstimatedPan); # override travel time |
||
| 491 | &ServoMove ($ServoTilt, $TiltPos, $EstimatedTilt); # override travel time |
||
| 492 | } |
||
| 493 | else |
||
| 494 | { |
||
| 495 | # complete |
||
| 496 | $ServoTestIndex = 0; |
||
| 497 | $State = "WaitGps"; |
||
| 498 | } |
||
| 499 | } |
||
| 500 | } |
||
| 501 | |||
| 502 | # |
||
| 503 | # Servo test finisched |
||
| 504 | # |
||
| 505 | # Wait for GPS Home position and compass |
||
| 506 | # |
||
| 507 | |||
| 508 | elsif ( $State eq "WaitGps" ) |
||
| 509 | { |
||
| 510 | if ( &ServoCheck ($ServoPan) ) |
||
| 511 | { |
||
| 512 | lock (%MkOsd); # until end of block |
||
| 513 | lock (%MkTrack); |
||
| 514 | |||
| 515 | if ( $MkOsd{'_Timestamp'} >= time-2 and |
||
| 516 | $MkOsd{'SatsInUse'} >= 6 ) |
||
| 517 | { |
||
| 518 | # gültige OSD daten vom MK und guter Satellitenempfang |
||
| 519 | |||
| 520 | # take GPS and compass from map definition or MK as antenna home-position |
||
| 521 | $MkTrack{'HomePos_Lon'} = $Map{'Track_Lon'} || $MkOsd{'HomePos_Lon'}; |
||
| 522 | $MkTrack{'HomePos_Lat'} = $Map{'Track_Lat'} || $MkOsd{'HomePos_Lat'}; |
||
| 523 | $MkTrack{'HomePos_Alt'} = $Map{'Track_Alt'} || $MkOsd{'HomePos_Alt'}; |
||
| 524 | $MkTrack{'CompassHeading'} = $Map{'Track_Bearing'} || $MkOsd{'CompassHeading'}; |
||
| 525 | |||
| 526 | $TrackTimer = &SetTimer_ms($TrackInterval); |
||
| 527 | |||
| 528 | $State = "TrackGps"; |
||
| 529 | } |
||
| 530 | } |
||
| 531 | } |
||
| 532 | |||
| 533 | # |
||
| 534 | # GPS Fix Home position |
||
| 535 | # Track now |
||
| 536 | # |
||
| 537 | elsif ( $State eq "TrackGps" ) |
||
| 538 | { |
||
| 539 | if ( &CheckTimer_ms($TrackTimer) and &ServoCheck($ServoPan) ) |
||
| 540 | { |
||
| 541 | $TrackTimer = &SetTimer_ms($TrackInterval); # reload Timer |
||
| 542 | |||
| 543 | lock (%MkOsd); # until end of block |
||
| 544 | lock (%MkTrack); |
||
| 545 | |||
| 546 | if ( $MkOsd{'_Timestamp'} >= time -2 and |
||
| 547 | $MkOsd{'SatsInUse'} >= 4 ) |
||
| 548 | { |
||
| 549 | # valid OSD data from the MK and sufficient satellites |
||
| 550 | |||
| 551 | my $Track_Geo = Geo::Ellipsoid->new( 'units' => 'degrees', |
||
| 552 | 'distance_units' => 'meter', |
||
| 553 | 'ellipsoid' => 'WGS84', |
||
| 554 | 'longitude' => 1, # Symmetric: -pi..pi |
||
| 555 | ); |
||
| 556 | |||
| 557 | my ($Dist, $Bearing) = $Track_Geo->to($MkTrack{'HomePos_Lat'}, $MkTrack{'HomePos_Lon'}, |
||
| 558 | $MkOsd{'CurPos_Lat'}, $MkOsd{'CurPos_Lon'}); |
||
| 559 | my $Dir_h = $MkTrack{'CompassHeading'}; |
||
| 560 | my $Dir_c = $MkOsd{'CompassHeading'}; |
||
| 561 | |||
| 562 | if ( $Dist < 4 ) # meter |
||
| 563 | { |
||
| 564 | # Too close to Home-Position. Set antenna to middle position |
||
| 565 | $Bearing = $Dir_h; |
||
| 566 | } |
||
| 567 | |||
| 568 | $MkTrack{'Bearing'} = sprintf ("%d", $Bearing); |
||
| 569 | $MkTrack{'Dist'} = sprintf ("%d", $Dist); |
||
| 570 | $MkTrack{'CurPos_Lon'} = $MkOsd{'CurPos_Lon'}; |
||
| 571 | $MkTrack{'CurPos_Lat'} = $MkOsd{'CurPos_Lat'}; |
||
| 572 | $MkTrack{'CurPos_Alt'} = $MkOsd{'CurPos_Alt'}; |
||
| 573 | |||
| 574 | # antenna pan direction: 0..180 degree, centre = 90 |
||
| 575 | my $AngelPan = $Bearing - $Dir_h + 90; |
||
| 576 | $AngelPan = $AngelPan % 360; |
||
| 577 | |||
| 578 | # antenna tilt direction: 0..180 degree, centre is up, 0 is front |
||
| 579 | my $AngelTilt = rad2deg(atan2(($MkOsd{'CurPos_Alt'} - $MkTrack{'HomePos_Alt'}), $Dist)); |
||
| 580 | if ( $AngelTilt < 0 ) { $AngelTilt = 0; } |
||
| 581 | if ( $AngelTilt > 180 ) { $AngelTilt = 180; } |
||
| 582 | |||
| 583 | if ( $AngelPan >= 180 ) |
||
| 584 | { |
||
| 585 | # Flip Pan/Tilt |
||
| 586 | $AngelPan = $AngelPan - 180; |
||
| 587 | $AngelTilt = 180 - $AngelTilt; |
||
| 588 | } |
||
| 589 | |||
| 590 | $MkTrack{'AngelPan'} = $AngelPan; |
||
| 591 | $MkTrack{'AngelTilt'} = $AngelTilt; |
||
| 592 | |||
| 593 | &ServoMove ($ServoPan, $AngelPan); |
||
| 594 | &ServoMove ($ServoTilt, $AngelTilt); |
||
| 595 | |||
| 596 | # Timestamp, wann der Datensatz geschtieben wurde |
||
| 597 | $MkTrack{'_Timestamp'} = time; |
||
| 598 | } |
||
| 599 | } |
||
| 600 | } |
||
| 601 | |||
| 602 | else |
||
| 603 | { |
||
| 604 | # Restart |
||
| 605 | $State = "ColdStart"; |
||
| 606 | } |
||
| 607 | |||
| 608 | # |
||
| 609 | # check command queue |
||
| 610 | # |
||
| 611 | while ( $TrackQueue->pending() > 0 ) |
||
| 612 | { |
||
| 613 | my $Cmd = $TrackQueue->dequeue(1); |
||
| 614 | |||
| 615 | if ( $Cmd =~ /COLDSTART/i ) |
||
| 616 | { |
||
| 617 | $State = "ColdStart"; |
||
| 618 | } |
||
| 619 | |||
| 620 | if ( $Cmd =~ /IDLE/i ) |
||
| 621 | { |
||
| 622 | if ( defined $ServoPort ) |
||
| 623 | { |
||
| 624 | # move all Servo to neutral position |
||
| 625 | &ServoMove ($ServoPan, 90); |
||
| 626 | &ServoMove ($ServoTilt, 0); |
||
| 627 | |||
| 628 | sleep 1; |
||
| 629 | &ServoRelax(); # swith off servo |
||
| 630 | } |
||
| 631 | |||
| 632 | $State = "Idle"; |
||
| 633 | } |
||
| 634 | } |
||
| 635 | |||
| 636 | # |
||
| 637 | # update system-timer |
||
| 638 | # |
||
| 639 | ($t1_s, $t1_us) = gettimeofday; |
||
| 640 | $SysTimerSleep_us = ($t0_s - $t1_s) * 1000000 + $t0_us - $t1_us + $SysTimerCount_ms * $SysTimerResolution; |
||
| 641 | |||
| 642 | if ($SysTimerSleep_us > 0) |
||
| 643 | { |
||
| 644 | usleep ($SysTimerSleep_us); |
||
| 645 | } |
||
| 646 | else |
||
| 647 | { |
||
| 648 | $SysTimerError ++; |
||
| 649 | } |
||
| 650 | |||
| 651 | $SysTimerCount_ms ++; |
||
| 652 | } |
||
| 653 | } |
||
| 654 | |||
| 655 | |||
| 656 | # |
||
| 657 | # Main Program |
||
| 658 | # |
||
| 659 | |||
| 660 | if ( $0 =~ /track.pl$/i ) |
||
| 661 | { |
||
| 662 | # Program wurde direkt aufgerufen |
||
| 663 | |||
| 664 | # |
||
| 665 | # Commandline Parameter |
||
| 666 | # |
||
| 667 | my $TrackPort = $CmdLine{'-TrackPort'}; |
||
| 668 | if ( $TrackPort ne "" ) |
||
| 669 | { |
||
| 670 | $ComPort = $TrackPort; |
||
| 671 | } |
||
| 672 | |||
| 673 | my $MkPort = $CmdLine{'-MkPort'}; |
||
| 674 | if ( $MkPort ne "" ) |
||
| 675 | { |
||
| 676 | $Cfg->{'mkcomm'}->{'Port'} = $MkPort; |
||
| 677 | } |
||
| 678 | |||
| 679 | my $Pan = $CmdLine{'-ServoPan'}; |
||
| 680 | if ( $Pan ne "" ) |
||
| 681 | { |
||
| 682 | my ($Min, $Mid, $Max) = split ',', $Pan; |
||
| 683 | $ServoPar[$ServoPan]{'Min'} = $Min; |
||
| 684 | $ServoPar[$ServoPan]{'Mid'} = $Mid; |
||
| 685 | $ServoPar[$ServoPan]{'Max'} = $Max; |
||
| 686 | } |
||
| 687 | |||
| 688 | my $Tilt = $CmdLine{'-ServoTilt'}; |
||
| 689 | if ( $Tilt ne "" ) |
||
| 690 | { |
||
| 691 | my ($Min, $Mid, $Max) = split ',', $Tilt; |
||
| 692 | $ServoPar[$ServoTilt]{'Min'} = $Min; |
||
| 693 | $ServoPar[$ServoTilt]{'Mid'} = $Mid; |
||
| 694 | $ServoPar[$ServoTilt]{'Max'} = $Max; |
||
| 695 | } |
||
| 696 | |||
| 697 | my $PanSpeed = $CmdLine{'-PanSpeed'}; |
||
| 698 | if ( $PanSpeed ne "" ) |
||
| 699 | { |
||
| 700 | $ServoPar[$ServoPan]{'Speed'} = $PanSpeed; |
||
| 701 | } |
||
| 702 | |||
| 703 | my $TiltSpeed = $CmdLine{'-TiltSpeed'}; |
||
| 704 | if ( $TiltSpeed ne "" ) |
||
| 705 | { |
||
| 706 | $ServoPar[$ServoTilt]{'Speed'} = $TiltSpeed; |
||
| 707 | } |
||
| 708 | |||
| 709 | # Kommunikation zum MK herstellen |
||
| 710 | # Input: %MkOsd, %MkTarget, %MkNcDebug |
||
| 711 | # Ouput: Thread-Queue: $MkSendQueue |
||
| 712 | $mk_thr = threads->create (\&MkCommLoop) -> detach(); |
||
| 713 | |||
| 714 | $TrackQueue->enqueue("COLDSTART"); # start command |
||
| 715 | &TrackAntennaGps(); |
||
| 716 | |||
| 717 | # should never exit |
||
| 718 | } |
||
| 719 | |||
| 720 | 1; |
||
| 721 | |||
| 722 | __END__ |