Rev Author Line No. Line
4649 roman 1 #!/usr/bin/python
2  
4650 roman 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
4649 roman 4  
4650 roman 5 import os
4649 roman 6 import time
7 import datetime
8 import sys
9 import numpy as np
10 from gps import *
11 from pymlab import config
12 import threading
13  
14 gpsd = None
15  
16 class GpsPoller(threading.Thread):
17 def __init__(self):
18 threading.Thread.__init__(self)
19 global gpsd #bring it in scope
20 gpsd = gps(mode=WATCH_ENABLE)
21 self.current_value = None
22 self.running = True
23  
24 def run(self):
25 global gpsd
26 while gpsp.running:
27 gpsd.next()
28  
29  
30  
31  
32 cfg = config.Config(
33 i2c = {
34 "port": 1,
35 },
36 bus = [
37 {
38 "name": "rps",
39 "type": "rps01",
40 },
41 ],
42 )
43  
44  
45 cfg.initialize()
46  
47 print "RPS01A logger"
48 sensor = cfg.get_device("rps")
49  
50  
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
4650 roman 62 AVERAGING = 50
4649 roman 63  
4650 roman 64 filen = 'log%0.0f.txt'%time.time()
65 f = open(filen,'w')
66 os.remove("last.txt")
67 os.symlink(filen, "last.txt")
68  
4649 roman 69 gpsp = GpsPoller()
70 gpsp.start()
71  
72 while True:
73 for i in range(AVERAGING):
4650 roman 74 time.sleep(0.01)
4649 roman 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)
4650 roman 90  
91 speed = speed/AVERAGING
4649 roman 92  
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)
4650 roman 95 f.write("%0.2f %0.2f %0.2f %0.2f\r\n" %(time.time(), abs(speed), angles[0], g_spd))
4649 roman 96 f.flush()
97  
98  
99 except KeyboardInterrupt:
100 gpsp.running = False
101 gpsp.join()
102 sys.exit(0)