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) |