# Declare variables ACCEL_CONSTANT = 9.81 # raw data comes in as G values TIME_CONSTANT = 1/1000 # raw data comes in as milliseconds ts_next = 0 ax = 0 ay = 0 az = 0 vx = 0 vy = 0 vz = 0 rot_matrix = np.matrix([[0,0,0],[0,0,0],[0,0,0]]) accel_matrix = np.matrix([[0],[0],[0]]) # Beginning of Parser code with VMU931Parser(accelerometer=True, euler=True) as vp: vp.set_accelerometer_resolution(16) # Set resolution of accelerometer to 8g while True: pkt = vp.parse() if isinstance(pkt, messages.Status): print(pkt) if isinstance(pkt, messages.Accelerometer): ts_last = ts_next ts_next, ax, ay, az = pkt ax_metric = ax * ACCEL_CONSTANT ay_metric = ay * ACCEL_CONSTANT az_metric = az * ACCEL_CONSTANT
x_line, = acc_axes.plot([], [], label="X")
y_line, = acc_axes.plot([], [], label="Y")
z_line, = acc_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
csv_points = [('Attribute', 'Time stamp', 'x', 'y', 'z')]
#with VMU931Parser(device = "COM3", accelerometer=True, quaternion=True) as vp:
with VMU931Parser(device="COM3", accelerometer=True) as vp:
vp.set_accelerometer_resolution(
16) # Set resolution of accelerometer to 8g.
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Accelerometer):
ts, x, y, z = pkt
ts_points.append(ts)
quat_axes.set_ylim([-1, 1])
w_line, = quat_axes.plot([], [], label="W")
x_line, = quat_axes.plot([], [], label="X")
y_line, = quat_axes.plot([], [], label="Y")
z_line, = quat_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
w_points = []
x_points = []
y_points = []
z_points = []
with VMU931Parser(quaternion=True) as vp:
while True:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Quaternion):
ts, w, x, y, z = pkt
ts_points.append(ts)
w_points.append(w)
x_points.append(x)
y_points.append(y)
z_points.append(z)
mag_axes.set_ylabel("Field Strength (μT)")
x_line, = mag_axes.plot([], [], label="X")
y_line, = mag_axes.plot([], [], label="Y")
z_line, = mag_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
csv_points = [('Attribute', 'Time stamp', 'x', 'y', 'z')]
with VMU931Parser(device = "COM3", magnetometer=True) as vp:
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Magnetometer):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
#!/usr/bin/env python3
from pyvmu.vmu931 import VMU931Parser
from pyvmu import messages
f = open("mag_euler.txt", "w")
f.write("Mag X | Mag Y | Mag Z | Euler X | Euler Y | Euler Z | Time (sec)\n")
mag = [0, 0, 0]
euler = [0, 0, 0]
ts_mag = 0
ts_euler = 0
i = 0
with VMU931Parser(euler=True, magnetometer=True) as vp:
while True:
pkt = vp.parse()
# Print any important status messages (if any)
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Magnetometer):
ts_mag, mag[0], mag[1], mag[2] = pkt
if isinstance(pkt, messages.Euler):
i = i + 1
ts_euler, euler[0], euler[1], euler[2] = pkt
figure = plt.figure()
head_axes = figure.add_subplot(111)
head_axes.set_title("Compass Heading")
head_axes.set_xlabel("Timestamp (ms)")
head_axes.set_ylabel("Heading (°)")
head_axes.set_ylim([0, 360])
head_line, = head_axes.plot([], [], label="Heading (°)")
plt.legend()
ts_points = []
h_points = []
with VMU931Parser(heading=True) as vp:
while True:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Heading):
ts, h = pkt
ts_points.append(ts)
h_points.append(h)
# Only plot every 100 points (still quite smooth), otherwise we're bottle-necked by matplotlib.
# Note that when angular data is not being streamed, the faster update rate is assumed.
if len(ts_points) % 10 == 0:
gyro_axes.set_title("Gyroscope")
gyro_axes.set_xlabel("Timestamp (ms)")
gyro_axes.set_ylabel("Velocity (°/s)")
x_line, = gyro_axes.plot([], [], label="X")
y_line, = gyro_axes.plot([], [], label="Y")
z_line, = gyro_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
with VMU931Parser(gyroscope=True) as vp:
while True:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Gyroscope):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
# Only plot every 10 points (still quite smooth), otherwise we risk being bottle-necked by matplotlib.
w_line, = quat_axes.plot([], [], label="W")
x_line, = quat_axes.plot([], [], label="X")
y_line, = quat_axes.plot([], [], label="Y")
z_line, = quat_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
w_points = []
x_points = []
y_points = []
z_points = []
csv_points = [('Attribute', 'Time stamp', 'w', 'x', 'y', 'z')]
with VMU931Parser(device="COM3", quaternion=True) as vp:
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Quaternion):
ts, w, x, y, z = pkt
ts_points.append(ts)
w_points.append(w)
x_points.append(x)
y_points.append(y)
fnameG = 'gyroscope_' + timestr + '.csv'
fnameE = 'euler_' + timestr + '.csv'
fnameQ = 'quaternion_' + timestr + '.csv'
# write csv function
def write_csv(data, csvfilename):
with open(csvfilename, 'a') as csvfile:
csvwriter = csv.writer(csvfile)
csvwriter.writerows(data)
#with VMU931Parser(device = "COM5", accelerometer=True, gyroscope=True, euler=True, quaternion=True) as vp:
with VMU931Parser(device="/dev/ttyACM0",
accelerometer=True,
gyroscope=True,
euler=True,
quaternion=True) as vp:
vp.set_accelerometer_resolution(
16) # Set resolution of accelerometer to 8g.
vp.set_gyroscope_resolution(250)
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 600
while time.time() - start_time <= seconds_to_capture:
# while True:
# for n in range(20):
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
euler_axes.set_xlabel("Timestamp (ms)")
euler_axes.set_ylabel("Angle (°)")
euler_axes.set_ylim([-180, 180]) # -8 <= g <= 8
x_line, = euler_axes.plot([], [], label="X")
y_line, = euler_axes.plot([], [], label="Y")
z_line, = euler_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
with VMU931Parser(euler=True) as vp:
while True:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Euler):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
# Only plot every 10 points (still quite smooth), otherwise we risk being bottle-necked by matplotlib.
euler_axes.set_ylim([-180, 180]) # -8 <= g <= 8
x_line, = euler_axes.plot([], [], label="X")
y_line, = euler_axes.plot([], [], label="Y")
z_line, = euler_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
csv_points = [('Attribute', 'Time stamp', 'x', 'y', 'z')]
with VMU931Parser(device="COM3", euler=True) as vp:
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
# while True:
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Euler):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
gyro_axes.set_ylabel("Velocity (°/s)")
x_line, = gyro_axes.plot([], [], label="X")
y_line, = gyro_axes.plot([], [], label="Y")
z_line, = gyro_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
csv_points = [('Attribute', 'Time stamp', 'x', 'y', 'z')]
with VMU931Parser(device="COM3", gyroscope=True) as vp:
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Gyroscope):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
head_axes = figure.add_subplot(111)
head_axes.set_title("Compass Heading")
head_axes.set_xlabel("Timestamp (ms)")
head_axes.set_ylabel("Heading (°)")
head_axes.set_ylim([0, 360])
head_line, = head_axes.plot([], [], label="Heading (°)")
plt.legend()
ts_points = []
h_points = []
csv_points = [('Attribute', 'Time stamp', 'heading')]
with VMU931Parser(device="COM3", heading=True) as vp:
start_time = time.time() # Time capture started, in seconds
seconds_to_capture = 30
while time.time() - start_time <= seconds_to_capture:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Heading):
ts, h = pkt
ts_points.append(ts)
h_points.append(h)
csv_points.append(('Heading', ts, h))
has_ran_mag = 0
vel = [0, 0, 0]
f = open("speed_imu.txt", "w")
f.write("Speed (m/s) | Heading (deg) | Time (sec)\n")
prevSec = 0
# Variables to calculate dynamic moving average for error
lstError = []
lstIndex = 0
lstSize = 300
with VMU931Parser(euler=True,
accelerometer=True,
magnetometer=True,
heading=True) as vp:
while True:
pkt = vp.parse()
# Print any important status messages (if any)
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Accelerometer):
ts_acc, acc[0], acc[1], acc[2] = pkt
if isinstance(pkt, messages.Magnetometer):
ts_mag, mag[0], mag[1], mag[2] = pkt
mag_axes.set_title("Magnetometer")
mag_axes.set_xlabel("Timestamp (ms)")
mag_axes.set_ylabel("Field Strength (μT)")
x_line, = mag_axes.plot([], [], label="X")
y_line, = mag_axes.plot([], [], label="Y")
z_line, = mag_axes.plot([], [], label="Z")
plt.legend()
ts_points = []
x_points = []
y_points = []
z_points = []
with VMU931Parser(magnetometer=True) as vp:
while True:
pkt = vp.parse()
if isinstance(pkt, messages.Status):
print(pkt)
if isinstance(pkt, messages.Magnetometer):
ts, x, y, z = pkt
ts_points.append(ts)
x_points.append(x)
y_points.append(y)
z_points.append(z)
# Only plot every 10 points (still quite smooth), otherwise we risk being bottle-necked by matplotlib.