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