Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
365 | rain-er | 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 | # |
||
43 | ############################################################################### |
||
44 | |||
45 | $Version{'track.pl'} = "0.1.0 - 2009-04-01"; |
||
46 | |||
47 | # Packages |
||
48 | use Time::HiRes qw(usleep gettimeofday); # http://search.cpan.org/~jhi/Time-HiRes-1.9719/HiRes.pm |
||
49 | use threads; |
||
50 | use threads::shared; |
||
51 | use Math::Trig; |
||
52 | use Geo::Ellipsoid; # http://search.cpan.org/dist/Geo-Ellipsoid-1.12/lib/Geo/Ellipsoid.pm |
||
53 | # http://www.kompf.de/gps/distcalc.html |
||
54 | # http://www.herrmann-w.de/Geocaching/Downloads/Richt.XLS |
||
55 | # http://williams.best.vwh.net/avform.htm |
||
56 | # http://williams.best.vwh.net/gccalc.html |
||
57 | # http://de.wikipedia.org/wiki/Orthodrome |
||
58 | if ( $^O =~ /Win32/i ) |
||
59 | { |
||
60 | require Win32::SerialPort; # http://search.cpan.org/dist/Win32-SerialPort |
||
61 | } |
||
62 | else |
||
63 | { |
||
64 | require Device::SerialPort; # http://search.cpan.org/~cook/Device-SerialPort-1.04/SerialPort.pm |
||
65 | } |
||
66 | |||
67 | require "mkcomm.pl"; # MK communication |
||
68 | |||
69 | # Sharing for Threads |
||
70 | share (@ServoPos); |
||
71 | share (%MkTrack); |
||
72 | |||
73 | # |
||
74 | # Parameter |
||
75 | # |
||
76 | |||
77 | # System Timer |
||
78 | $SysTimerResolution = 1000; # Resolution in us |
||
79 | |||
80 | |||
81 | # Com Port for Pololu Mikro-Servoboard |
||
82 | # http://www.shop.robotikhardware.de/shop/catalog/product_info.php?cPath=65&products_id=118 |
||
83 | if ( ! defined $Cfg->{'track'}->{'Port'} ) |
||
84 | { |
||
85 | # set default port |
||
86 | $Cfg->{'track'}->{'Port'} = "COM8"; |
||
87 | } |
||
88 | |||
89 | # Servo parameter |
||
90 | $ServoPan = 0; # Servo channel Pan |
||
91 | $ServoTilt = 1; # Servo channel Tilt |
||
92 | $MkTrack{'ServoPan'} = $ServoPan; |
||
93 | $MkTrack{'ServoTilt'} = $ServoTilt; |
||
94 | @ServoSpeed = (100/40, 100/40, 100/40, 100/40, 100/40, 100/40, 100/40, 100/40); # ms/degree |
||
95 | $ServoConstPpm = 20; # PPM protocol overhead in ms |
||
96 | |||
97 | @ServoTest = ( [ 90, 0 ], # Pan, Tilt in degrees for servo test |
||
98 | [ 0, 0 ], |
||
99 | [ 180, 0 ], |
||
100 | [ 90, 0 ], |
||
101 | [ 45, 0 ], |
||
102 | [ 135, 0 ], |
||
103 | [ 90, 0 ] ); |
||
104 | |||
105 | |||
106 | # Tracking |
||
107 | $TrackInterval = 100; # in ms |
||
108 | |||
109 | # |
||
110 | # Logging |
||
111 | # |
||
112 | |||
113 | sub Log() |
||
114 | { |
||
115 | my ($Message, $Level) = @_; |
||
116 | |||
117 | if ( $LogOn ) |
||
118 | { |
||
119 | print ("$SysTimerCount_ms: $Message\n"); |
||
120 | } |
||
121 | } |
||
122 | |||
123 | # |
||
124 | # Timer |
||
125 | # |
||
126 | |||
127 | sub SetTimer_ms() |
||
128 | { |
||
129 | return $SysTimerCount_ms + $_[0]; |
||
130 | } |
||
131 | |||
132 | sub CheckTimer_ms() |
||
133 | { |
||
134 | my $Diff = $_[0] - $SysTimerCount_ms; |
||
135 | return ($Diff <= 0); |
||
136 | } |
||
137 | |||
138 | # |
||
139 | # Servo |
||
140 | # |
||
141 | |||
142 | sub ServoInit() |
||
143 | { |
||
144 | # open COM-Port |
||
145 | my $ComPort = $Cfg->{'track'}->{'Port'}; |
||
146 | undef $ServoPort; |
||
147 | if ( $^O =~ m/Win32/ ) |
||
148 | { |
||
149 | $ServoPort = Win32::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
||
150 | } |
||
151 | else |
||
152 | { |
||
153 | $ServoPort = Device::SerialPort->new ($ComPort) || die "Error open $ComPort\n"; |
||
154 | } |
||
155 | # Set COM parameters |
||
156 | $ServoPort->baudrate(38400); |
||
157 | $ServoPort->parity("none"); |
||
158 | $ServoPort->databits(8); |
||
159 | $ServoPort->stopbits(1); |
||
160 | $ServoPort->handshake('none'); |
||
161 | $ServoPort->write_settings; |
||
162 | |||
163 | # Byte 1: sync - Pololu Mode |
||
164 | # Byte 2: device |
||
165 | # Byte 3: command |
||
166 | # Byte 4: Servo num |
||
167 | # Byte 5: data |
||
168 | # Bit 6: Servo on/off |
||
169 | # Bit 5: Direction |
||
170 | # Bit 4-0: Servo Range |
||
171 | my $Output = pack('C*', 0x80, 0x01, 0x00, 0, 0x60 | 27 ); |
||
172 | $ServoPort->write($Output); |
||
173 | |||
174 | @ServoStartTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of last ServoMove() call |
||
175 | @ServoEndTime = (0, 0, 0, 0, 0, 0, 0, 0); # Timestamp of estimated arrival at end position |
||
176 | @ServoPos = (0, 0, 0, 0, 0, 0, 0, 0); # Current servo position 0..180 degree |
||
177 | } |
||
178 | |||
179 | |||
180 | sub ServoMove() |
||
181 | { |
||
182 | my ($Num, $Angel, $Time) = @_; |
||
183 | |||
184 | my $Overhead = 0; |
||
185 | |||
186 | if ( $Angel != $ServoPos[$Num] ) |
||
187 | { |
||
188 | if ( $Angel < 0) {$Angel = 0;} |
||
189 | if ( $Angel > 180) {$Angel = 180;} |
||
190 | |||
191 | my $Pos = $Angel * 127/180; # angel 0..180 degree to servo position 0..127 |
||
192 | |||
193 | # output to COM port |
||
194 | # Byte 1: sync - Pololu Mode |
||
195 | # Byte 2: device |
||
196 | # Byte 3: command |
||
197 | # Byte 4: Servo num |
||
198 | # Byte 5: servo position 0..127 |
||
199 | |||
200 | my $Output = pack('C*', 0x80, 0x01, 0x02, $Num, $Pos ); |
||
201 | $ServoPort->write($Output); |
||
202 | |||
203 | $Overhead += $ServoConstPpm; # PPM protocol overhead |
||
204 | } |
||
205 | |||
206 | # set timer stuff for travel time predicion |
||
207 | my $LastAngel = $ServoPos[$Num]; |
||
208 | my $EstimatedTime = abs($Angel - $LastAngel) * $ServoSpeed[$Num] + $Overhead; |
||
209 | if ( $Time > 0 ) |
||
210 | { |
||
211 | # Parameter override |
||
212 | $EstimatedTime = $Time; |
||
213 | } |
||
214 | $ServoStartTime[$Num] = $SysTimerCount_ms; |
||
215 | $ServoEndTime[$Num] = $SysTimerCount_ms + $EstimatedTime; |
||
216 | $ServoPos[$Num] = $Angel; |
||
217 | |||
218 | &Log ("ServoMove: Num: $Num : LastPos: $LastAngel NewPos: $Angel Estimated: $EstimatedTime ms"); |
||
219 | |||
220 | return $ServoEndTime[$Num]; |
||
221 | } |
||
222 | |||
223 | |||
224 | # Check, if servo has reached end position |
||
225 | sub ServoCheck() |
||
226 | { |
||
227 | my $Num = $_[0]; |
||
228 | return &CheckTimer_ms($ServoEndTime[$Num]); |
||
229 | } |
||
230 | |||
231 | |||
232 | sub ServoClose() |
||
233 | { |
||
234 | # close COM-Port |
||
235 | undef $ServoPort; |
||
236 | } |
||
237 | |||
238 | # |
||
239 | # Signal handler |
||
240 | # |
||
241 | |||
242 | $SIG{'INT'} = 'SigHandler'; |
||
243 | $SIG{'KILL'} = 'SigHandler'; |
||
244 | |||
245 | sub SigHandler() |
||
246 | { |
||
247 | # move all Servo to neutral position |
||
248 | &ServoMove ($ServoPan, 90); |
||
249 | &ServoMove ($ServoTilt, 0); |
||
250 | |||
251 | &ServoClose(); |
||
252 | |||
253 | if ( defined threads->self() ) |
||
254 | { |
||
255 | threads->exit(); |
||
256 | } |
||
257 | exit; |
||
258 | } |
||
259 | |||
260 | |||
261 | # |
||
262 | # Track it |
||
263 | # |
||
264 | |||
265 | sub TrackAntennaGps() |
||
266 | { |
||
267 | |||
268 | # |
||
269 | # State maschine |
||
270 | # |
||
271 | |||
272 | my $State = "ColdStart"; |
||
273 | while (1) |
||
274 | { |
||
275 | |||
276 | if ( $State eq "ColdStart" ) |
||
277 | { |
||
278 | &ServoInit(); |
||
279 | |||
280 | # initialize system-timer |
||
281 | $SysTimerCount_ms = 0; |
||
282 | $SysTimerError = 0; |
||
283 | ($t0_s, $t0_us) = gettimeofday; |
||
284 | |||
285 | $ServoTestIndex = 0; |
||
286 | |||
287 | $State = "InitServoTest"; |
||
288 | } |
||
289 | |||
290 | # |
||
291 | # Start servo test |
||
292 | # doesn't really make much sense, but looks cool:-) |
||
293 | # |
||
294 | |||
295 | elsif ( $State eq "InitServoTest") |
||
296 | { |
||
297 | if ( &ServoCheck ($ServoPan) and &ServoCheck ($ServoTilt) ) |
||
298 | { |
||
299 | |||
300 | my $PanPos = $ServoTest[$ServoTestIndex][0]; |
||
301 | my $TiltPos = $ServoTest[$ServoTestIndex][1]; |
||
302 | $ServoTestIndex ++; |
||
303 | |||
304 | if ( defined $PanPos and defined $TiltPos ) |
||
305 | { |
||
306 | &ServoMove ($ServoPan, $PanPos, 1000); # override 1s travel time |
||
307 | &ServoMove ($ServoTilt, $TiltPos, 1000); # override 1s travel time |
||
308 | } |
||
309 | else |
||
310 | { |
||
311 | # complete |
||
312 | $ServoTestIndex = 0; |
||
313 | $State = "WaitGps"; |
||
314 | } |
||
315 | } |
||
316 | } |
||
317 | |||
318 | # |
||
319 | # Servo test finisched |
||
320 | # |
||
321 | # Wait for GPS Home position and compass |
||
322 | # |
||
323 | |||
324 | elsif ( $State eq "WaitGps" ) |
||
325 | { |
||
326 | if ( &ServoCheck ($ServoPan) ) |
||
327 | { |
||
328 | if ( $MkOsd{'_Timestamp'} >= time-2 and |
||
329 | $MkOsd{'SatsInUse'} >= 6 ) |
||
330 | { |
||
331 | # gültige OSD daten vom MK und guter Satellitenempfang |
||
332 | |||
333 | # take GPS and compass from MK as antenna home-position |
||
334 | $MkTrack{'HomePos_Lon'} = $MkOsd{'HomePos_Lon'}; |
||
335 | $MkTrack{'HomePos_Lat'} = $MkOsd{'HomePos_Lat'}; |
||
336 | $MkTrack{'HomePos_Alt'} = $MkOsd{'HomePos_Alt'}; |
||
337 | if ( $MkTrack{'CompassHeading'} eq "" ) |
||
338 | { |
||
339 | # nur beim ersten mal |
||
340 | $MkTrack{'CompassHeading'} = $MkOsd{'CompassHeading'}; |
||
341 | } |
||
342 | |||
343 | $TrackTimer = &SetTimer_ms($TrackInterval); |
||
344 | |||
345 | $State = "TrackGps"; |
||
346 | } |
||
347 | } |
||
348 | } |
||
349 | |||
350 | # |
||
351 | # GPS Fix Home position |
||
352 | # Track now |
||
353 | # |
||
354 | elsif ( $State eq "TrackGps" ) |
||
355 | { |
||
356 | if ( &CheckTimer_ms($TrackTimer) and &ServoCheck($ServoPan) ) |
||
357 | { |
||
358 | $TrackTimer = &SetTimer_ms($TrackInterval); # reload Timer |
||
359 | |||
360 | if ( $MkOsd{'_Timestamp'} >= time -2 and |
||
361 | $MkOsd{'SatsInUse'} >= 4 ) |
||
362 | { |
||
363 | # gültige OSD Daten vom MK und ausreichender Satellitenempfang |
||
364 | lock (%MkOsd); # until end of block |
||
365 | lock (%MkTrack); |
||
366 | |||
367 | my $Track_Geo = Geo::Ellipsoid->new( 'units' => 'degrees', |
||
368 | 'distance_units' => 'meter', |
||
369 | 'ellipsoid' => 'WGS84', |
||
370 | ); |
||
371 | |||
372 | my ($Dist, $Bearing) = $Track_Geo->to($MkTrack{'HomePos_Lat'}, $MkTrack{'HomePos_Lon'}, |
||
373 | $MkOsd{'CurPos_Lat'}, $MkOsd{'CurPos_Lon'}); |
||
374 | my $Dir_h = $MkTrack{'CompassHeading'}; |
||
375 | my $Dir_c = $MkOsd{'CompassHeading'}; |
||
376 | |||
377 | if ( $Dist < 2 ) # meter |
||
378 | { |
||
379 | # zu nahe an der Home-Position. Antenne auf Mitte stellen |
||
380 | $Bearing = $Dir_h; |
||
381 | } |
||
382 | |||
383 | $MkTrack{'Bearing'} = sprintf ("%d", $Bearing); |
||
384 | $MkTrack{'Dist'} = sprintf ("%d", $Dist); |
||
385 | $MkTrack{'CurPos_Lon'} = $MkOsd{'CurPos_Lon'}; |
||
386 | $MkTrack{'CurPos_Lat'} = $MkOsd{'CurPos_Lat'}; |
||
387 | $MkTrack{'CurPos_Alt'} = $MkOsd{'CurPos_Alt'}; |
||
388 | |||
389 | my $AngelPan = $Bearing - $Dir_h + 90; # direction of antenna: 0..180 degree, center = 90 |
||
390 | while ( $AngelPan >= 360 ) |
||
391 | { |
||
392 | $AngelPan -= 360; |
||
393 | } |
||
394 | |||
395 | $MkTrack{'AngelPan'} = $AngelPan; |
||
396 | &ServoMove ($ServoPan, $AngelPan); |
||
397 | |||
398 | # Timestamp, wann der Datensatz geschtieben wurde |
||
399 | $MkTrack{'_Timestamp'} = time; |
||
400 | } |
||
401 | } |
||
402 | } |
||
403 | |||
404 | else |
||
405 | { |
||
406 | # Restart |
||
407 | &Log ("WARNING: Error in state machine - $State - ColdStart"); |
||
408 | |||
409 | $State = "ColdStart"; |
||
410 | } |
||
411 | |||
412 | # |
||
413 | # update system-timer |
||
414 | # |
||
415 | ($t1_s, $t1_us) = gettimeofday; |
||
416 | $SysTimerSleep_us = ($t0_s - $t1_s) * 1000000 + $t0_us - $t1_us + $SysTimerCount_ms * $SysTimerResolution; |
||
417 | |||
418 | #&Log ("SysTimerSleep_us: $SysTimerSleep_us"); |
||
419 | |||
420 | if ($SysTimerSleep_us > 0) |
||
421 | { |
||
422 | usleep ($SysTimerSleep_us); |
||
423 | } |
||
424 | else |
||
425 | { |
||
426 | $SysTimerError ++; |
||
427 | } |
||
428 | |||
429 | $SysTimerCount_ms ++; |
||
430 | } |
||
431 | } |
||
432 | |||
433 | |||
434 | # |
||
435 | # Main Program |
||
436 | # |
||
437 | |||
438 | if ( $0 =~ /track.pl$/i ) |
||
439 | { |
||
440 | # Program wurde direkt aufgerufen |
||
441 | |||
442 | # Kommunikation zum MK herstellen |
||
443 | # Input: %MkOsd, %MkTarget, %MkNcDebug |
||
444 | # Ouput: Thread-Queue: $MkSendQueue |
||
445 | $mk_thr = threads->create (\&MkCommLoop) -> detach(); |
||
446 | |||
447 | &TrackAntennaGps(); |
||
448 | |||
449 | # should never exit |
||
450 | } |
||
451 | |||
452 | 1; |
||
453 | |||
454 | __END__ |