Rev 4429 Rev 4666
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
-   16 import numpy as np
12 from pymlab import config 17 from pymlab import config
13   18  
14 #### Script Arguments ############################################### 19 #### Script Arguments ###############################################
15   20  
16 if len(sys.argv) != 2: 21 if len(sys.argv) != 2:
17 sys.stderr.write("Invalid number of arguments.\n") 22 sys.stderr.write("Invalid number of arguments.\n")
18 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], )) 23 sys.stderr.write("Usage: %s PORT ADDRESS\n" % (sys.argv[0], ))
19 sys.exit(1) 24 sys.exit(1)
20   25  
21 port = eval(sys.argv[1]) 26 port = eval(sys.argv[1])
22 #### Sensor Configuration ########################################### 27 #### Sensor Configuration ###########################################
23   28  
24 '''' 29 ''''
25 cfg = config.Config( 30 cfg = config.Config(
26 i2c = { 31 i2c = {
27 "port": port, 32 "port": port,
28 }, 33 },
29   34  
30 bus = [ 35 bus = [
31 { 36 {
32 "type": "i2chub", 37 "type": "i2chub",
33 "address": 0x72, 38 "address": 0x72,
34 39
35 "children": [ 40 "children": [
36 {"name": "encoder", "type": "rps01", "channel": 1, } 41 {"name": "encoder", "type": "rps01", "channel": 1, }
37 ], 42 ],
38 }, 43 },
39 ], 44 ],
40 ) 45 )
41   46  
42 ''' 47 '''
43 cfg = config.Config( 48 cfg = config.Config(
44 i2c = { 49 i2c = {
45 "port": port, 50 "port": port,
46 }, 51 },
47 bus = [ 52 bus = [
48 { 53 {
49 "name": "encoder", 54 "name": "encoder",
50 "type": "rps01", 55 "type": "rps01",
51 }, 56 },
52 ], 57 ],
53 ) 58 )
54   59  
55   60  
56 cfg.initialize() 61 cfg.initialize()
57   62  
58 print "RPS01A magnetic position sensor RPS01 readout example \r\n" 63 print "RPS01A magnetic position sensor RPS01 readout example \r\n"
59 sensor = cfg.get_device("encoder") 64 sensor = cfg.get_device("encoder")
60   65  
61 print sensor.get_address() 66 print sensor.get_address()
62 print sensor.get_zero_position() 67 print sensor.get_zero_position()
63   68  
64 #### Data Logging ################################################### 69 #### Data Logging ###################################################
65   70  
66 try: 71 try:
-   72 angles = np.zeros(5)
-   73 angles[4] = sensor.get_angle(verify = False)
-   74 time.sleep(0.01)
-   75 angles[3] = sensor.get_angle(verify = False)
-   76 time.sleep(0.01)
-   77 angles[2] = sensor.get_angle(verify = False)
-   78 time.sleep(0.01)
-   79 angles[1] = sensor.get_angle(verify = False)
-   80 n = 0
-   81 speed = 0
-   82 AVERAGING = 50
-   83  
67 while True: 84 while True:
68 # for i in range(10): 85 for i in range(AVERAGING):
-   86 time.sleep(0.01)
69 angle1 = sensor.get_angle(verify = False) 87 angles[0] = sensor.get_angle(verify = False)
70 time.sleep(0.1) 88
71 angle2 = sensor.get_angle(verify = False) 89 if (angles[0] + n*360 - angles[1]) > 300:
72 time.sleep(0.1) 90 n -= 1
73 angle3 = sensor.get_angle(verify = False) 91 angles[0] = angles[0] + n*360
74 92  
-   93 elif (angles[0] + n*360 - angles[1]) < -300: # compute angular speed in backward direction.
75 if (angle1 < angle2): 94 n += 1
76 speed = (angle2 - angle1)/0.01 95 angles[0] = angles[0] + n*360
-   96  
77 else: 97 else:
78 speed = (360 - angle1 + angle2)/0.01 98 angles[0] = angles[0] + n*360
79 99
80 -  
81 -  
82 sys.stdout.write("Speed: " + str(speed) +"\t"+ str(angle1) +"\t"+ str(angle2) + "\t\tMagnitude: " + str(sensor.get_magnitude()) -  
83 + "\tAGC Value: " + str(sensor.get_agc_value()) + "\tDiagnostics: " + str(sensor.get_diagnostics()) + "\r\n") 100 speed += (-angles[4] + 8*angles[3] - 8*angles[1] + angles[0])/12
84 sys.stdout.flush() 101 angles = np.roll(angles, 1)
-   102  
-   103 speed = speed/AVERAGING # apply averaging on acummulated value.
85 time.sleep(0.01) 104 print "Speed: %0.2f \t Total Angle: %0.2f \r\n" % (speed, angles[0])
-   105  
86 except KeyboardInterrupt: 106 except KeyboardInterrupt:
87 sys.exit(0) 107 sys.exit(0)