You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
2.9 KiB
111 lines
2.9 KiB
#!/usr/bin/env python3
|
|
'''
|
|
printing the gap between interrupts in a histogram to check if the
|
|
frequency is what we expect, the bmx is not interrupt driven for as we
|
|
get interrupts in a 2kHz rate.
|
|
'''
|
|
|
|
import argparse
|
|
import sys
|
|
import numpy as np
|
|
from collections import defaultdict
|
|
|
|
from openpilot.tools.lib.logreader import LogReader
|
|
from openpilot.tools.lib.route import Route
|
|
|
|
import matplotlib.pyplot as plt
|
|
|
|
SRC_BMX = "bmx055"
|
|
SRC_LSM = "lsm6ds3"
|
|
|
|
|
|
def parseEvents(log_reader):
|
|
bmx_data = defaultdict(list)
|
|
lsm_data = defaultdict(list)
|
|
|
|
for m in log_reader:
|
|
if m.which() not in ['accelerometer', 'gyroscope']:
|
|
continue
|
|
|
|
d = getattr(m, m.which()).to_dict()
|
|
|
|
if d["source"] == SRC_BMX and "acceleration" in d:
|
|
bmx_data["accel"].append(d["timestamp"] / 1e9)
|
|
|
|
if d["source"] == SRC_BMX and "gyroUncalibrated" in d:
|
|
bmx_data["gyro"].append(d["timestamp"] / 1e9)
|
|
|
|
if d["source"] == SRC_LSM and "acceleration" in d:
|
|
lsm_data["accel"].append(d["timestamp"] / 1e9)
|
|
|
|
if d["source"] == SRC_LSM and "gyroUncalibrated" in d:
|
|
lsm_data["gyro"].append(d["timestamp"] / 1e9)
|
|
|
|
return bmx_data, lsm_data
|
|
|
|
|
|
def cleanData(data):
|
|
if len(data) == 0:
|
|
return [], []
|
|
|
|
data.sort()
|
|
diffs = np.diff(data)
|
|
return data, diffs
|
|
|
|
|
|
def logAvgValues(data, sensor):
|
|
if len(data) == 0:
|
|
print(f"{sensor}: no data to average")
|
|
return
|
|
|
|
avg = sum(data) / len(data)
|
|
hz = 1 / avg
|
|
print(f"{sensor}: data_points: {len(data)} avg [ns]: {avg} avg [Hz]: {hz}")
|
|
|
|
|
|
if __name__ == "__main__":
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("route", type=str, help="route name")
|
|
parser.add_argument("segment", type=int, help="segment number")
|
|
args = parser.parse_args()
|
|
|
|
r = Route(args.route)
|
|
logs = r.log_paths()
|
|
|
|
if len(logs) == 0:
|
|
print("NO data routes")
|
|
sys.exit(0)
|
|
|
|
if args.segment >= len(logs):
|
|
print(f"RouteID: {args.segment} out of range, max: {len(logs) -1}")
|
|
sys.exit(0)
|
|
|
|
lr = LogReader(logs[args.segment])
|
|
bmx_data, lsm_data = parseEvents(lr)
|
|
|
|
# sort bmx accel data, and then cal all the diffs, and to a histogram of those
|
|
bmx_accel, bmx_accel_diffs = cleanData(bmx_data["accel"])
|
|
bmx_gyro, bmx_gyro_diffs = cleanData(bmx_data["gyro"])
|
|
lsm_accel, lsm_accel_diffs = cleanData(lsm_data["accel"])
|
|
lsm_gyro, lsm_gyro_diffs = cleanData(lsm_data["gyro"])
|
|
|
|
# get out the averages
|
|
logAvgValues(bmx_accel_diffs, "bmx accel")
|
|
logAvgValues(bmx_gyro_diffs, "bmx gyro ")
|
|
logAvgValues(lsm_accel_diffs, "lsm accel")
|
|
logAvgValues(lsm_gyro_diffs, "lsm gyro ")
|
|
|
|
fig, axs = plt.subplots(1, 2, tight_layout=True)
|
|
axs[0].hist(bmx_accel_diffs, bins=50)
|
|
axs[0].set_title("bmx_accel")
|
|
axs[1].hist(bmx_gyro_diffs, bins=50)
|
|
axs[1].set_title("bmx_gyro")
|
|
|
|
figl, axsl = plt.subplots(1, 2, tight_layout=True)
|
|
axsl[0].hist(lsm_accel_diffs, bins=50)
|
|
axsl[0].set_title("lsm_accel")
|
|
axsl[1].hist(lsm_gyro_diffs, bins=50)
|
|
axsl[1].set_title("lsm_gyro")
|
|
|
|
print("check plot...")
|
|
plt.show()
|
|
|