-
Notifications
You must be signed in to change notification settings - Fork 10
/
imu-blender.py
executable file
·83 lines (65 loc) · 2.09 KB
/
imu-blender.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#!/usr/bin/python
import os
import sys
sys.path.append(os.getcwd() + "/MPU6050")
import mpu6050
import time
import math
import csv
import socket
import smbus
from pycomms import PyComms
# Sensor initialization
mpu = mpu6050.MPU6050()
mpu.dmpInitialize()
mpu.setDMPEnabled(True)
# Send UDP Data
def send_data(msg):
try:
sock.sendto(msg, (REMOTE_IP, DST_PORT))
except socket.error as err:
sock.close()
print "Connection err!"
# get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize()
# UDP socket instance
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Read configuration from file (csv file format)
with open("imu-blender.cfg", 'rb') as f:
file = csv.DictReader(f)
for rows in file:
REMOTE_IP = rows['remote_ip']
DST_PORT = int(rows['dst_port'])
DEBUG = rows['debug']
print ("IMU Sensor started (CTRL-C to stop)!")
while True:
# Get INT_STATUS byte
mpuIntStatus = mpu.getIntStatus()
# check for DMP data ready interrupt (this should happen frequently)
if mpuIntStatus >= 2:
# get current FIFO count
fifoCount = mpu.getFIFOCount()
# check for overflow (this should never happen unless our code is too inefficient)
if fifoCount == 1024:
# reset so we can continue cleanly
mpu.resetFIFO()
print('FIFO overflow!')
# wait for correct available data length, should be a VERY short wait
fifoCount = mpu.getFIFOCount()
while fifoCount < packetSize:
fifoCount = mpu.getFIFOCount()
result = mpu.getFIFOBytes(packetSize)
# Get quaternio, q return y, x, z, w
q = mpu.dmpGetQuaternion(result)
x = "{0:.6f}".format(q['x'])
y = "{0:.6f}".format(q['y'])
z = "{0:.6f}".format(q['z'])
w = "{0:.6f}".format(q['w'])
if DEBUG == "1":
print (x),
print (y),
print (z),
print (w)
# Sends quaternion through UDP
send_data(str(x) + "," + str(y) + "," + str(z) + "," + str(w))
fifoCount -= packetSize