Rev 4451 Rev 4452
1 #!/usr/bin/python 1 #!/usr/bin/python
2   2  
3 # Python library for RPS01A MLAB module with AS5048B I2C Magnetic position sensor. 3 # MLAB meteostation wind speed gauge with magnetic rotation sensor.
-   4 # This simple algorithm calculate difference between five time equidistant points during the rotation. The result is angular speed per time step.
-   5 # Size of time-step could be varied depending on expected wind speed range to measure.
-   6 # Algorithm should be expanded by Kalman filtering to minimize dependence on fast reading.
-   7 # The measuring principle could introduce time-stamped reading to increase precision of measurement. It could be possible because the readings are not exactly time equidistant in real Linux word.
4   8  
5 #uncomment for debbug purposes 9 #uncomment for debbug purposes
6 #import logging 10 #import logging
7 #logging.basicConfig(level=logging.DEBUG) 11 #logging.basicConfig(level=logging.DEBUG)
8   12  
9 import time 13 import time
10 import datetime 14 import datetime
11 import sys 15 import sys
12 import numpy as np 16 import numpy as np
13 from pymlab import config 17 from pymlab import config
14   18  
15 #### Script Arguments ############################################### 19 #### Script Arguments ###############################################
16   20  
17 if len(sys.argv) != 2: 21 if len(sys.argv) != 2:
18 sys.stderr.write("Invalid number of arguments.\n") 22 sys.stderr.write("Invalid number of arguments.\n")
19 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], )) 23 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], ))
20 sys.exit(1) 24 sys.exit(1)
21   25  
22 port = eval(sys.argv[1]) 26 port = eval(sys.argv[1])
23 #### Sensor Configuration ########################################### 27 #### Sensor Configuration ###########################################
24   28  
25 '''' 29 ''''
26 cfg = config.Config( 30 cfg = config.Config(
27 i2c = { 31 i2c = {
28 "port": port, 32 "port": port,
29 }, 33 },
30   34  
31 bus = [ 35 bus = [
32 { 36 {
33 "type": "i2chub", 37 "type": "i2chub",
34 "address": 0x72, 38 "address": 0x72,
35 39
36 "children": [ 40 "children": [
37 {"name": "encoder", "type": "rps01", "channel": 1, } 41 {"name": "encoder", "type": "rps01", "channel": 1, }
38 ], 42 ],
39 }, 43 },
40 ], 44 ],
41 ) 45 )
42   46  
43 ''' 47 '''
44 cfg = config.Config( 48 cfg = config.Config(
45 i2c = { 49 i2c = {
46 "port": port, 50 "port": port,
47 }, 51 },
48 bus = [ 52 bus = [
49 { 53 {
50 "name": "encoder", 54 "name": "encoder",
51 "type": "rps01", 55 "type": "rps01",
52 }, 56 },
53 ], 57 ],
54 ) 58 )
55   59  
56   60  
57 cfg.initialize() 61 cfg.initialize()
58   62  
59 print "RPS01A magnetic position sensor RPS01 readout example \r\n" 63 print "RPS01A magnetic position sensor RPS01 readout example \r\n"
60 sensor = cfg.get_device("encoder") 64 sensor = cfg.get_device("encoder")
61   65  
62 print sensor.get_address() 66 print sensor.get_address()
63 print sensor.get_zero_position() 67 print sensor.get_zero_position()
64   68  
65 #### Data Logging ################################################### 69 #### Data Logging ###################################################
66   70  
67 try: 71 try:
68 angles = np.zeros(5) 72 angles = np.zeros(5)
69 angles[4] = sensor.get_angle(verify = False) 73 angles[4] = sensor.get_angle(verify = False)
70 time.sleep(0.01) 74 time.sleep(0.01)
71 angles[3] = sensor.get_angle(verify = False) 75 angles[3] = sensor.get_angle(verify = False)
72 time.sleep(0.01) 76 time.sleep(0.01)
73 angles[2] = sensor.get_angle(verify = False) 77 angles[2] = sensor.get_angle(verify = False)
74 time.sleep(0.01) 78 time.sleep(0.01)
75 angles[1] = sensor.get_angle(verify = False) 79 angles[1] = sensor.get_angle(verify = False)
76 n = 0 80 n = 0
77 speed = 0 81 speed = 0
78 AVERAGING = 50 82 AVERAGING = 50
79   83  
80 while True: 84 while True:
81 for i in range(AVERAGING): 85 for i in range(AVERAGING):
82 time.sleep(0.01) 86 time.sleep(0.01)
83 angles[0] = sensor.get_angle(verify = False) 87 angles[0] = sensor.get_angle(verify = False)
84 88
85 if (angles[0] + n*360 - angles[1]) > 300: 89 if (angles[0] + n*360 - angles[1]) > 300:
86 n -= 1 90 n -= 1
87 angles[0] = angles[0] + n*360 91 angles[0] = angles[0] + n*360
88   92  
89 elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction. 93 elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction.
90 n += 1 94 n += 1
91 angles[0] = angles[0] + n*360 95 angles[0] = angles[0] + n*360
92   96  
93 else: 97 else:
94 angles[0] = angles[0] + n*360 98 angles[0] = angles[0] + n*360
95 99
96 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12 100 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12
97 angles = np.roll(angles, 1) 101 angles = np.roll(angles, 1)
98   102  
99 speed = speed/AVERAGING # apply averaging on acummulated value. 103 speed = speed/AVERAGING # apply averaging on acummulated value.
100 print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0]) 104 print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0])
101   105  
102 except KeyboardInterrupt: 106 except KeyboardInterrupt:
103 sys.exit(0) 107 sys.exit(0)