Skip to content

Commit 279191a

Browse files
jerry871002jerry871002
committed
Python files
1 parent 64423be commit 279191a

File tree

8 files changed

+252
-11
lines changed

8 files changed

+252
-11
lines changed

‎app.py

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,138 @@
1+
from imu import gy801, tilt_compensated_heading
2+
from math import sin, cos, atan2, degrees, radians
3+
import time
4+
5+
import socket
6+
7+
import dash
8+
import dash_core_components as dcc
9+
import dash_html_components as html
10+
import plotly.express as px
11+
import pandas as pd
12+
13+
external_stylesheets = ['https://codepen.io/chriddyp/pen/bWLwgP.css']
14+
15+
app = dash.Dash(__name__, external_stylesheets=external_stylesheets)
16+
17+
# assume you have a "long-form" data frame
18+
# see https://plotly.com/python/px-arguments/ for more options
19+
df = pd.DataFrame({
20+
"Fruit": ["Apples", "Oranges", "Bananas", "Apples", "Oranges", "Bananas"],
21+
"Amount": [4, 1, 2, 2, 4, 5],
22+
"City": ["SF", "SF", "SF", "Montreal", "Montreal", "Montreal"]
23+
})
24+
25+
fig = px.bar(df, x="Fruit", y="Amount", color="City", barmode="group")
26+
27+
app.layout = html.Div(children=[
28+
html.H1(children='Hello Dash'),
29+
30+
html.Div(children='''
31+
Dash: A web application framework for Python.
32+
'''),
33+
34+
dcc.Graph(
35+
id='example-graph',
36+
figure=fig
37+
)
38+
])
39+
40+
if __name__ == '__main__':
41+
# app.run_server(debug=True, host='192.168.0.19')
42+
43+
44+
try:
45+
while True:
46+
try:
47+
sensors = gy801()
48+
break
49+
except OSError:
50+
print('OSError...')
51+
time.sleep(1)
52+
continue
53+
54+
55+
accel = sensors.accel
56+
gyro = sensors.gyro
57+
compass = sensors.compass
58+
59+
# pitch_pre = accel.getPitch()
60+
# roll_pre = accel.getRoll()
61+
62+
while True:
63+
# try:
64+
# pitch_cur = (pitch_pre + gyro.getXangle()) * 0.98 + adxl345.getPitch() * 0.02
65+
# roll_cur = (roll_pre + gyro.getYangle()) * 0.98 + adxl345.getRoll() * 0.02
66+
# except OSError:
67+
# print('Skip once')
68+
# time.sleep(0.5)
69+
# continue
70+
71+
# print ("ACC: ")
72+
# print ("x = %.3f m/s2" % ( adxl345.getX() ))
73+
# print ("y = %.3f m/s2" % ( adxl345.getY() ))
74+
# print ("z = %.3f m/s2" % ( adxl345.getZ() ))
75+
# print()
76+
# print ("Gyro: ")
77+
# print ("Xangle = %.3f deg" % ( gyro.getXangle() ))
78+
# print ("Yangle = %.3f deg" % ( gyro.getYangle() ))
79+
# print ("Zangle = %.3f deg" % ( gyro.getZangle() ))
80+
# print()
81+
# print(f"Pitch = {pitch_cur}")
82+
# print(f"Roll = {roll_cur}")
83+
# print()
84+
# print(f"Pitch = {adxl345.getPitch()}")
85+
# print(f"Roll = {adxl345.getRoll()}")
86+
87+
try:
88+
magx = compass.getX()
89+
magy = compass.getY()
90+
magz = compass.getZ()
91+
92+
pitch = accel.getPitch()
93+
roll = accel.getRoll()
94+
except OSError:
95+
print('Skip once...')
96+
time.sleep(0.5)
97+
continue
98+
99+
compx = magx * cos(radians(pitch)) + magz * sin(radians(pitch))
100+
compy = magx * sin(radians(roll)) * sin(radians(pitch)) + \
101+
magy * cos(radians(roll)) - \
102+
magz * sin(radians(roll)) * cos(radians(pitch))
103+
104+
bearing1 = degrees(atan2(magy, magx))
105+
if (bearing1 < 0):
106+
bearing1 += 360
107+
if (bearing1 > 360):
108+
bearing1 -= 360
109+
bearing1 = bearing1 + compass.angle_offset
110+
111+
bearing2 = degrees(atan2(compy, compx))
112+
if (bearing2 < 0):
113+
bearing2 += 360
114+
if (bearing2 > 360):
115+
bearing2 -= 360
116+
bearing2 = bearing2 + compass.angle_offset
117+
118+
# print ("X = %d ," % ( magx ))
119+
# print ("Y = %d ," % ( magy ))
120+
# print ("Z = %d (gauss)" % ( magz ))
121+
print ("Pitch = %.3f" % ( pitch ))
122+
print ("Roll = %.3f" % ( roll ))
123+
print ("tiltX = %.3f" % ( compx ))
124+
print ("tiltY = %.3f" % ( compy ))
125+
print ("Angle offset = %.3f deg" % ( compass.angle_offset ))
126+
print ("Original Heading = %.3f deg, " % ( bearing1 ))
127+
print ("Tilt Heading = %.3f deg, " % ( bearing2 ))
128+
# print ("Heading = %.3f" % (tilt_compensated_heading(compy, compx)))
129+
print('===================================')
130+
print()
131+
132+
# pitch_pre = pitch_cur
133+
# roll_pre = roll_cur
134+
135+
time.sleep(1)
136+
137+
except KeyboardInterrupt:
138+
print("Cleanup")

‎calib_acc.py

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
from imu import gy801
2+
import time
3+
4+
sensor = gy801()
5+
6+
while True:
7+
print(f'{sensor.accel.getXg():4.2f}\t{sensor.accel.getYg():4.2f}\t{sensor.accel.getZg():4.2f}')
8+
time.sleep(0.5)

‎imu.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -306,15 +306,15 @@ def getZ(self):
306306
class gy801(object):
307307
def __init__(self):
308308
# accelorator caliberation
309-
ACC_X_OFFSET = 0.047
310-
ACC_Y_OFFSET = -0.015
311-
ACC_Z_OFFSET = 0.099
309+
ACC_X_OFFSET = -0.03
310+
ACC_Y_OFFSET = 0.04
311+
ACC_Z_OFFSET = 0.059
312312

313313
# compass caliberation
314314
COM_X_OFFSET = 326.5
315315
COM_Y_OFFSET = 49.0
316316
COM_Z_OFFSET = -58.0
317-
ANGLE_OFFSET = (4, 29) # (4, 29) for Taichung, (4, 32) for Hsinchu
317+
ANGLE_OFFSET = (4, 32) # (4, 29) for Taichung, (4, 32) for Hsinchu
318318

319319
self.accel = ADXL345(ACC_X_OFFSET, ACC_Y_OFFSET, ACC_Z_OFFSET)
320320
self.gyro = L3G4200D()

‎line_notifier.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
import requests
22

33
def line_message(msg):
4-
token2 = "zle1Bi1MvdQEW390Vf5WqM6tLdb0SaOksYONslBh3tM"
4+
token2 = "fMzINcJVBA2pp9iBqr2a3qPrqlVkWD0w1a0LZgTn1Tg"
55
headers2 = {
66
"Authorization": "Bearer " + token2,
77
"Content-Type" : "application/x-www-form-urlencoded"

‎main.py

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
import time
2+
import json
3+
import paho.mqtt.client as mqtt
4+
import numpy as np
5+
6+
from sensor_fusion import madgwick, kalman, complementary
7+
from plot import plot_trajectory, fft, plot_trajectory_attenuated
8+
import line_notifier as notifier
9+
10+
acc = []
11+
gyr = []
12+
mag = []
13+
timestamp = []
14+
15+
def on_connect(client, userdata, flags, rc):
16+
print("Connected with result code " + str(rc))
17+
client.subscribe("data")
18+
19+
def on_message(client, userdata, msg):
20+
global acc, gyr, mag, timestamp
21+
data = json.loads(msg.payload)
22+
23+
acc.append([float(data['acc_X']), float(data['acc_Y']), float(data['acc_Z'])])
24+
gyr.append([float(data['gyro_X']), float(data['gyro_Y']), float(data['gyro_Z'])])
25+
mag.append([float(data['mag_X']), float(data['mag_Y']), float(data['mag_Z'])])
26+
timestamp.append(float(data['timestamp']))
27+
28+
def setup_mqtt():
29+
client = mqtt.Client()
30+
client.on_connect = on_connect
31+
client.on_message = on_message
32+
client.connect('192.168.1.8', port=1883, keepalive=60)
33+
return client
34+
35+
def get_raw_data(client, duration):
36+
global acc, gyr, mag, timestamp
37+
38+
print('Start measuring...')
39+
notifier.line_message('Start measuring...')
40+
client.loop_start()
41+
time.sleep(duration * 1.1)
42+
client.loop_stop()
43+
print(f'Finish measuring... Duration = {timestamp[-1] - timestamp[0]}')
44+
notifier.line_message('Finish measuring...')
45+
46+
acc = np.array(acc) # unit: m/s2
47+
gyr = np.array(gyr) # unit: dps
48+
gyr = np.radians(gyr) # unit: rad/s
49+
mag = np.array(mag) # unit: gauss
50+
mag = 0.1 * mag # unit: mT
51+
52+
return acc, gyr, mag
53+
54+
55+
if __name__ == '__main__':
56+
client = setup_mqtt()
57+
58+
DURATION = 3 # unit: s
59+
acc, gyr, mag = get_raw_data(client, DURATION)
60+
df = complementary(acc, gyr, mag, len(acc)/DURATION)
61+
print(df)
62+
63+
dt = DURATION / len(df)
64+
65+
# plot_trajectory(df, dt)
66+
df = fft(df, dt)
67+
plot_trajectory_attenuated(df, dt)

‎mqtt_subscibe.py

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
import paho.mqtt.client as mqtt
2+
import json
3+
4+
def on_connect(client, userdata, flags, rc):
5+
print("Connected with result code " + str(rc))
6+
client.subscribe("data")
7+
8+
def on_message(client, userdata, msg):
9+
print(msg.topic + " " + str(msg.payload))
10+
11+
data = json.loads(msg.payload)
12+
print(data)
13+
14+
client = mqtt.Client()
15+
client.on_connect = on_connect
16+
client.on_message = on_message
17+
client.connect("192.168.1.8", 1883, 60)
18+
client.loop_forever()

‎plot.py

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -131,18 +131,18 @@ def fft(df, dt):
131131
df['z_ifft'] = np.fft.irfft(atten_z_fft, n=df.shape[0])
132132

133133
plt.subplot(1, 3, 1)
134-
plt.plot(df['Before'])
135-
plt.plot(df['After'], label='x_ifft')
134+
plt.plot(df['EARTH LINEAR ACCELERATION X'], label='Before')
135+
plt.plot(df['x_ifft'], label='After')
136136
plt.legend()
137137

138138
plt.subplot(1, 3, 2)
139-
plt.plot(df['Before'])
140-
plt.plot(df['After'], label='y_ifft')
139+
plt.plot(df['EARTH LINEAR ACCELERATION Y'], label='Before')
140+
plt.plot(df['y_ifft'], label='After')
141141
plt.legend()
142142

143143
plt.subplot(1, 3, 3)
144-
plt.plot(df['Before'])
145-
plt.plot(df['After'], label='z_ifft')
144+
plt.plot(df['EARTH LINEAR ACCELERATION Z'], label='Before')
145+
plt.plot(df['z_ifft'], label='After')
146146
plt.legend()
147147

148148
plt.savefig('ifft.png')

‎test_measure.py

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
from imu import gy801
2+
import time
3+
4+
sensor = gy801()
5+
6+
while True:
7+
print(f'x = {sensor.accel.getX():5.3f},\ty = {sensor.accel.getY():5.3f},\tz = {sensor.accel.getZ():5.3f} (m/s2)')
8+
print(*sensor.getGravity())
9+
10+
time.sleep(0.2)

0 commit comments

Comments
 (0)