bulk_sensor: New file with helper code for reading bulk sensors
Move the ClockSyncRegression class from adxl345.py to a new bulk_sensors.py file. Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
This commit is contained in:
parent
644f7e0872
commit
978c294741
|
@ -4,7 +4,7 @@
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import logging, time, collections, threading, multiprocessing, os
|
import logging, time, collections, threading, multiprocessing, os
|
||||||
from . import bus, motion_report
|
from . import bus, motion_report, bulk_sensor
|
||||||
|
|
||||||
# ADXL345 registers
|
# ADXL345 registers
|
||||||
REG_DEVID = 0x00
|
REG_DEVID = 0x00
|
||||||
|
@ -173,59 +173,13 @@ class AccelCommandHelper:
|
||||||
val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
|
val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0))
|
||||||
self.chip.set_reg(reg, val)
|
self.chip.set_reg(reg, val)
|
||||||
|
|
||||||
# Helper class for chip clock synchronization via linear regression
|
|
||||||
class ClockSyncRegression:
|
|
||||||
def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
|
|
||||||
self.mcu = mcu
|
|
||||||
self.chip_clock_smooth = chip_clock_smooth
|
|
||||||
self.decay = decay
|
|
||||||
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
|
||||||
self.mcu_clock_avg = self.mcu_clock_variance = 0.
|
|
||||||
self.chip_clock_avg = self.chip_clock_covariance = 0.
|
|
||||||
def reset(self, mcu_clock, chip_clock):
|
|
||||||
self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
|
|
||||||
self.chip_clock_avg = chip_clock
|
|
||||||
self.mcu_clock_variance = self.chip_clock_covariance = 0.
|
|
||||||
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
|
||||||
def update(self, mcu_clock, chip_clock):
|
|
||||||
# Update linear regression
|
|
||||||
decay = self.decay
|
|
||||||
diff_mcu_clock = mcu_clock - self.mcu_clock_avg
|
|
||||||
self.mcu_clock_avg += decay * diff_mcu_clock
|
|
||||||
self.mcu_clock_variance = (1. - decay) * (
|
|
||||||
self.mcu_clock_variance + diff_mcu_clock**2 * decay)
|
|
||||||
diff_chip_clock = chip_clock - self.chip_clock_avg
|
|
||||||
self.chip_clock_avg += decay * diff_chip_clock
|
|
||||||
self.chip_clock_covariance = (1. - decay) * (
|
|
||||||
self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
|
|
||||||
def set_last_chip_clock(self, chip_clock):
|
|
||||||
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
|
||||||
self.last_chip_clock = chip_clock
|
|
||||||
self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
|
|
||||||
def get_clock_translation(self):
|
|
||||||
inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
|
|
||||||
if not self.last_chip_clock:
|
|
||||||
return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
|
|
||||||
# Find mcu clock associated with future chip_clock
|
|
||||||
s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
|
|
||||||
scdiff = s_chip_clock - self.chip_clock_avg
|
|
||||||
s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
|
|
||||||
# Calculate frequency to converge at future point
|
|
||||||
mdiff = s_mcu_clock - self.last_exp_mcu_clock
|
|
||||||
s_inv_chip_freq = mdiff / self.chip_clock_smooth
|
|
||||||
return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
|
|
||||||
def get_time_translation(self):
|
|
||||||
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
|
||||||
clock_to_print_time = self.mcu.clock_to_print_time
|
|
||||||
base_time = clock_to_print_time(base_mcu)
|
|
||||||
inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
|
|
||||||
return base_time, base_chip, inv_freq
|
|
||||||
|
|
||||||
MIN_MSG_TIME = 0.100
|
MIN_MSG_TIME = 0.100
|
||||||
|
|
||||||
BYTES_PER_SAMPLE = 5
|
BYTES_PER_SAMPLE = 5
|
||||||
SAMPLES_PER_BLOCK = 10
|
SAMPLES_PER_BLOCK = 10
|
||||||
|
|
||||||
|
API_UPDATES = 0.100
|
||||||
|
|
||||||
# Printer class that controls ADXL345 chip
|
# Printer class that controls ADXL345 chip
|
||||||
class ADXL345:
|
class ADXL345:
|
||||||
def __init__(self, config):
|
def __init__(self, config):
|
||||||
|
@ -259,10 +213,11 @@ class ADXL345:
|
||||||
# Clock tracking
|
# Clock tracking
|
||||||
self.last_sequence = self.max_query_duration = 0
|
self.last_sequence = self.max_query_duration = 0
|
||||||
self.last_limit_count = self.last_error_count = 0
|
self.last_limit_count = self.last_error_count = 0
|
||||||
self.clock_sync = ClockSyncRegression(self.mcu, 640)
|
chip_smooth = self.data_rate * API_UPDATES * 2
|
||||||
|
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
|
||||||
# API server endpoints
|
# API server endpoints
|
||||||
self.api_dump = motion_report.APIDumpHelper(
|
self.api_dump = motion_report.APIDumpHelper(
|
||||||
self.printer, self._api_update, self._api_startstop, 0.100)
|
self.printer, self._api_update, self._api_startstop, API_UPDATES)
|
||||||
self.name = config.get_name().split()[-1]
|
self.name = config.get_name().split()[-1]
|
||||||
wh = self.printer.lookup_object('webhooks')
|
wh = self.printer.lookup_object('webhooks')
|
||||||
wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
|
wh.register_mux_endpoint("adxl345/dump_adxl345", "sensor", self.name,
|
||||||
|
|
|
@ -0,0 +1,53 @@
|
||||||
|
# Tools for reading bulk sensor data from the mcu
|
||||||
|
#
|
||||||
|
# Copyright (C) 2020-2023 Kevin O'Connor <kevin@koconnor.net>
|
||||||
|
#
|
||||||
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
|
|
||||||
|
# Helper class for chip clock synchronization via linear regression
|
||||||
|
class ClockSyncRegression:
|
||||||
|
def __init__(self, mcu, chip_clock_smooth, decay = 1. / 20.):
|
||||||
|
self.mcu = mcu
|
||||||
|
self.chip_clock_smooth = chip_clock_smooth
|
||||||
|
self.decay = decay
|
||||||
|
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
||||||
|
self.mcu_clock_avg = self.mcu_clock_variance = 0.
|
||||||
|
self.chip_clock_avg = self.chip_clock_covariance = 0.
|
||||||
|
def reset(self, mcu_clock, chip_clock):
|
||||||
|
self.mcu_clock_avg = self.last_mcu_clock = mcu_clock
|
||||||
|
self.chip_clock_avg = chip_clock
|
||||||
|
self.mcu_clock_variance = self.chip_clock_covariance = 0.
|
||||||
|
self.last_chip_clock = self.last_exp_mcu_clock = 0.
|
||||||
|
def update(self, mcu_clock, chip_clock):
|
||||||
|
# Update linear regression
|
||||||
|
decay = self.decay
|
||||||
|
diff_mcu_clock = mcu_clock - self.mcu_clock_avg
|
||||||
|
self.mcu_clock_avg += decay * diff_mcu_clock
|
||||||
|
self.mcu_clock_variance = (1. - decay) * (
|
||||||
|
self.mcu_clock_variance + diff_mcu_clock**2 * decay)
|
||||||
|
diff_chip_clock = chip_clock - self.chip_clock_avg
|
||||||
|
self.chip_clock_avg += decay * diff_chip_clock
|
||||||
|
self.chip_clock_covariance = (1. - decay) * (
|
||||||
|
self.chip_clock_covariance + diff_mcu_clock*diff_chip_clock*decay)
|
||||||
|
def set_last_chip_clock(self, chip_clock):
|
||||||
|
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
||||||
|
self.last_chip_clock = chip_clock
|
||||||
|
self.last_exp_mcu_clock = base_mcu + (chip_clock-base_chip) * inv_cfreq
|
||||||
|
def get_clock_translation(self):
|
||||||
|
inv_chip_freq = self.mcu_clock_variance / self.chip_clock_covariance
|
||||||
|
if not self.last_chip_clock:
|
||||||
|
return self.mcu_clock_avg, self.chip_clock_avg, inv_chip_freq
|
||||||
|
# Find mcu clock associated with future chip_clock
|
||||||
|
s_chip_clock = self.last_chip_clock + self.chip_clock_smooth
|
||||||
|
scdiff = s_chip_clock - self.chip_clock_avg
|
||||||
|
s_mcu_clock = self.mcu_clock_avg + scdiff * inv_chip_freq
|
||||||
|
# Calculate frequency to converge at future point
|
||||||
|
mdiff = s_mcu_clock - self.last_exp_mcu_clock
|
||||||
|
s_inv_chip_freq = mdiff / self.chip_clock_smooth
|
||||||
|
return self.last_exp_mcu_clock, self.last_chip_clock, s_inv_chip_freq
|
||||||
|
def get_time_translation(self):
|
||||||
|
base_mcu, base_chip, inv_cfreq = self.get_clock_translation()
|
||||||
|
clock_to_print_time = self.mcu.clock_to_print_time
|
||||||
|
base_time = clock_to_print_time(base_mcu)
|
||||||
|
inv_freq = clock_to_print_time(base_mcu + inv_cfreq) - base_time
|
||||||
|
return base_time, base_chip, inv_freq
|
|
@ -4,8 +4,8 @@
|
||||||
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
|
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import logging, time, collections, threading, multiprocessing, os
|
import logging, time, threading, multiprocessing, os
|
||||||
from . import bus, motion_report, adxl345
|
from . import bus, motion_report, adxl345, bulk_sensor
|
||||||
|
|
||||||
# LIS2DW registers
|
# LIS2DW registers
|
||||||
REG_LIS2DW_WHO_AM_I_ADDR = 0x0F
|
REG_LIS2DW_WHO_AM_I_ADDR = 0x0F
|
||||||
|
@ -30,14 +30,13 @@ LIS2DW_DEV_ID = 0x44
|
||||||
FREEFALL_ACCEL = 9.80665
|
FREEFALL_ACCEL = 9.80665
|
||||||
SCALE = FREEFALL_ACCEL * 1.952 / 4
|
SCALE = FREEFALL_ACCEL * 1.952 / 4
|
||||||
|
|
||||||
Accel_Measurement = collections.namedtuple(
|
|
||||||
'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
|
|
||||||
|
|
||||||
MIN_MSG_TIME = 0.100
|
MIN_MSG_TIME = 0.100
|
||||||
|
|
||||||
BYTES_PER_SAMPLE = 6
|
BYTES_PER_SAMPLE = 6
|
||||||
SAMPLES_PER_BLOCK = 8
|
SAMPLES_PER_BLOCK = 8
|
||||||
|
|
||||||
|
API_UPDATES = 0.100
|
||||||
|
|
||||||
# Printer class that controls LIS2DW chip
|
# Printer class that controls LIS2DW chip
|
||||||
class LIS2DW:
|
class LIS2DW:
|
||||||
def __init__(self, config):
|
def __init__(self, config):
|
||||||
|
@ -69,10 +68,11 @@ class LIS2DW:
|
||||||
# Clock tracking
|
# Clock tracking
|
||||||
self.last_sequence = self.max_query_duration = 0
|
self.last_sequence = self.max_query_duration = 0
|
||||||
self.last_limit_count = self.last_error_count = 0
|
self.last_limit_count = self.last_error_count = 0
|
||||||
self.clock_sync = adxl345.ClockSyncRegression(self.mcu, 640)
|
chip_smooth = self.data_rate * API_UPDATES * 2
|
||||||
|
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
|
||||||
# API server endpoints
|
# API server endpoints
|
||||||
self.api_dump = motion_report.APIDumpHelper(
|
self.api_dump = motion_report.APIDumpHelper(
|
||||||
self.printer, self._api_update, self._api_startstop, 0.100)
|
self.printer, self._api_update, self._api_startstop, API_UPDATES)
|
||||||
self.name = config.get_name().split()[-1]
|
self.name = config.get_name().split()[-1]
|
||||||
wh = self.printer.lookup_object('webhooks')
|
wh = self.printer.lookup_object('webhooks')
|
||||||
wh.register_mux_endpoint("lis2dw/dump_lis2dw", "sensor", self.name,
|
wh.register_mux_endpoint("lis2dw/dump_lis2dw", "sensor", self.name,
|
||||||
|
|
|
@ -4,8 +4,8 @@
|
||||||
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
|
# Copyright (C) 2020-2021 Kevin O'Connor <kevin@koconnor.net>
|
||||||
#
|
#
|
||||||
# This file may be distributed under the terms of the GNU GPLv3 license.
|
# This file may be distributed under the terms of the GNU GPLv3 license.
|
||||||
import logging, time, collections, threading, multiprocessing, os
|
import logging, time, threading, multiprocessing, os
|
||||||
from . import bus, motion_report, adxl345
|
from . import bus, motion_report, adxl345, bulk_sensor
|
||||||
|
|
||||||
MPU9250_ADDR = 0x68
|
MPU9250_ADDR = 0x68
|
||||||
|
|
||||||
|
@ -47,14 +47,13 @@ SCALE = 0.000244140625 * FREEFALL_ACCEL
|
||||||
|
|
||||||
FIFO_SIZE = 512
|
FIFO_SIZE = 512
|
||||||
|
|
||||||
Accel_Measurement = collections.namedtuple(
|
|
||||||
'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z'))
|
|
||||||
|
|
||||||
MIN_MSG_TIME = 0.100
|
MIN_MSG_TIME = 0.100
|
||||||
|
|
||||||
BYTES_PER_SAMPLE = 6
|
BYTES_PER_SAMPLE = 6
|
||||||
SAMPLES_PER_BLOCK = 8
|
SAMPLES_PER_BLOCK = 8
|
||||||
|
|
||||||
|
API_UPDATES = 0.100
|
||||||
|
|
||||||
# Printer class that controls MPU9250 chip
|
# Printer class that controls MPU9250 chip
|
||||||
class MPU9250:
|
class MPU9250:
|
||||||
def __init__(self, config):
|
def __init__(self, config):
|
||||||
|
@ -86,10 +85,11 @@ class MPU9250:
|
||||||
# Clock tracking
|
# Clock tracking
|
||||||
self.last_sequence = self.max_query_duration = 0
|
self.last_sequence = self.max_query_duration = 0
|
||||||
self.last_limit_count = self.last_error_count = 0
|
self.last_limit_count = self.last_error_count = 0
|
||||||
self.clock_sync = adxl345.ClockSyncRegression(self.mcu, 640)
|
chip_smooth = self.data_rate * API_UPDATES * 2
|
||||||
|
self.clock_sync = bulk_sensor.ClockSyncRegression(mcu, chip_smooth)
|
||||||
# API server endpoints
|
# API server endpoints
|
||||||
self.api_dump = motion_report.APIDumpHelper(
|
self.api_dump = motion_report.APIDumpHelper(
|
||||||
self.printer, self._api_update, self._api_startstop, 0.100)
|
self.printer, self._api_update, self._api_startstop, API_UPDATES)
|
||||||
self.name = config.get_name().split()[-1]
|
self.name = config.get_name().split()[-1]
|
||||||
wh = self.printer.lookup_object('webhooks')
|
wh = self.printer.lookup_object('webhooks')
|
||||||
wh.register_mux_endpoint("mpu9250/dump_mpu9250", "sensor", self.name,
|
wh.register_mux_endpoint("mpu9250/dump_mpu9250", "sensor", self.name,
|
||||||
|
|
Loading…
Reference in New Issue