diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index 813a9397..6fc0dcf0 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -117,54 +117,6 @@ class MPU9250CommandHelper: val = gcmd.get("VAL", minval=0, maxval=255, parser=lambda x: int(x, 0)) 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 BYTES_PER_SAMPLE = 6 @@ -201,7 +153,7 @@ class MPU9250: # Clock tracking self.last_sequence = self.max_query_duration = 0 self.last_limit_count = self.last_error_count = 0 - self.clock_sync = ClockSyncRegression(self.mcu, 640) + self.clock_sync = adxl345.ClockSyncRegression(self.mcu, 640) # API server endpoints self.api_dump = motion_report.APIDumpHelper( self.printer, self._api_update, self._api_startstop, 0.100)