sensord: use interrupts to improve LSM6DS3 timing accuracy (#24525)
* change LSM6DS3TR(-c) gyroscope and accelerometer to interrupt
* add pthread for linking
* add interrupt collector thread to fetch in parallel to non interrupt based sensors
* change get_event interface to return true on successful read
* fetch sensor interrupts via gpiochip
* avoid sending empty messages (interrupt only, non interupt magn can leave a gap in the orphan block)
* add verifier script to sensor interrupts (sensor_data_to_hist.py)
* add/update sensord testsweet (test_sensord.py)
* add poll timed out check
* unexport interrupt gpio pins
* gpiochip on both edges, but skip falling edge if rising edge is detected, this is handled in the sensor as the status flag is checked if new data is availble
* add test to sensord to verify 100Hz interrupt frequency
* add sensor shutdown/low power mode functionality on sensord exit
* relax test, will be readded in the splitup PR
Co-authored-by: Kurt Nistelberger <kurt.nistelberger@gmail.com>
old-commit-hash: 84a3c355e5
taco
Willem Melching
3 years ago
committed by
GitHub
parent
0c480f7b3a
commit
7451db46cf
31 changed files with 711 additions and 58 deletions
@ -0,0 +1,121 @@ |
||||
#!/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 |
||||
from collections import defaultdict |
||||
|
||||
from tools.lib.logreader import LogReader |
||||
from 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: |
||||
# only sensorEvents |
||||
if m.which() != 'sensorEvents': |
||||
continue |
||||
|
||||
for se in m.sensorEvents: |
||||
# convert data to dictionary |
||||
d = se.to_dict() |
||||
|
||||
if d["timestamp"] == 0: |
||||
continue # empty event? |
||||
|
||||
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() |
||||
prev = data[0] |
||||
diffs = [] |
||||
for v in data[1:]: |
||||
diffs.append(v - prev) |
||||
prev = v |
||||
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() |
||||
|
||||
Loading…
Reference in new issue