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