Rev 4666 Rev 4702
1 #!/usr/bin/python 1 #!/usr/bin/python
2   2  
3 # plot with >> plot 'last.txt' u 1:2 w l axes x1y1, 'last.txt' u 1:4 w l axes x1y2, 'last.txt' u 1:3 3 # plot with >> plot 'last.txt' u 1:2 w l axes x1y1, 'last.txt' u 1:4 w l axes x1y2, 'last.txt' u 1:3
4   4  
5 import os 5 import os
6 import time 6 import time
7 import datetime 7 import datetime
8 import sys 8 import sys
9 import numpy as np 9 import numpy as np
10 from gps import * 10 from gps import *
11 from pymlab import config 11 from pymlab import config
12 import threading 12 import threading
13   13  
14 gpsd = None 14 gpsd = None
15   15  
16 class GpsPoller(threading.Thread): 16 class GpsPoller(threading.Thread):
17 def __init__(self): 17 def __init__(self):
18 threading.Thread.__init__(self) 18 threading.Thread.__init__(self)
19 global gpsd #bring it in scope 19 global gpsd #bring it in scope
20 gpsd = gps(mode=WATCH_ENABLE) 20 gpsd = gps(mode=WATCH_ENABLE)
21 self.current_value = None 21 self.current_value = None
22 self.running = True 22 self.running = True
23 23  
24 def run(self): 24 def run(self):
25 global gpsd 25 global gpsd
26 while gpsp.running: 26 while gpsp.running:
27 gpsd.next() 27 gpsd.next()
28 28  
29   29  
30   30  
31   31  
32 cfg = config.Config( 32 cfg = config.Config(
33 i2c = { 33 i2c = {
34 "port": 1, 34 "port": 1,
35 }, 35 },
36 bus = [ 36 bus = [
37 { 37 {
38 "name": "rps", 38 "name": "rps",
39 "type": "rps01", 39 "type": "rps01",
40 }, 40 },
41 ], 41 ],
42 ) 42 )
43   43  
44   44  
45 cfg.initialize() 45 cfg.initialize()
46   46  
47 print "RPS01A logger" 47 print "RPS01A logger"
48 sensor = cfg.get_device("rps") 48 sensor = cfg.get_device("rps")
49   49  
50   50  
51 try: 51 try:
52 angles = np.zeros(5) -  
53 angles[4] = sensor.get_angle(verify = False) -  
54 time.sleep(0.01) -  
55 angles[3] = sensor.get_angle(verify = False) -  
56 time.sleep(0.01) -  
57 angles[2] = sensor.get_angle(verify = False) -  
58 time.sleep(0.01) -  
59 angles[1] = sensor.get_angle(verify = False) -  
60 n = 0 -  
61 speed = 0 -  
62 AVERAGING = 50 -  
63 -  
64 filen = 'log%0.0f.txt'%time.time() 52 filen = 'log%0.0f.txt'%time.time()
65 f = open(filen,'w') 53 f = open(filen,'w')
66 os.remove("last.txt") 54 os.remove("last.txt")
67 os.symlink(filen, "last.txt") 55 os.symlink(filen, "last.txt")
68 56  
69 gpsp = GpsPoller() 57 gpsp = GpsPoller()
70 gpsp.start() 58 gpsp.start()
71   59  
72 while True: 60 while True:
73 for i in range(AVERAGING): -  
74 time.sleep(0.01) 61 w_spd = abs(sensor.get_speed())
75 angles[0] = sensor.get_angle(verify = False) -  
76 -  
77 if (angles[0] + n*360 - angles[1]) > 300: -  
78 n -= 1 -  
79 angles[0] = angles[0] + n*360 -  
80   -  
81 elif (angles[0] + n*360 - angles[1]) < -300: -  
82 n += 1 -  
83 angles[0] = angles[0] + n*360 -  
84   -  
85 else: -  
86 angles[0] = angles[0] + n*360 -  
87 -  
88 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 -  
89 angles = np.roll(angles, 1) 62 g_spd = abs(gpsd.fix.speed())
90 -  
91 speed = speed/AVERAGING -  
92   63  
93 g_spd = gpsd.fix.speed -  
94 print "W_Spd: %0.2f \t Angle: %0.2f \t G_Spd %0.2f" % (speed, angles[0], g_spd) 64 print "W_Spd: %.3f \t G_Spd %.3f \t multiplayer: %.3f \t multiplayer: %.3f" % (w_spd, g_spd, w_spd/g_spd, g_spd/w_spd)
95 f.write("%0.2f %0.2f %0.2f %0.2f\r\n" %(time.time(), abs(speed), angles[0], g_spd)) 65 f.write("%.3f %.3f %.3f\r\n" %(time.time(), w_spd, g_spd))
96 f.flush() 66 f.flush()
97 67  
98   68  
99 except KeyboardInterrupt: 69 except KeyboardInterrupt:
100 gpsp.running = False 70 gpsp.running = False
101 gpsp.join() 71 gpsp.join()
102 sys.exit(0) 72 sys.exit(0)