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