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