From 247a409335ff48134dcb6abc41d2c464b465a6ec Mon Sep 17 00:00:00 2001 From: Kevin O'Connor Date: Thu, 16 Jun 2022 12:13:12 -0400 Subject: [PATCH] mpu9250: Inline twos_complement() code Calling python functions can have high overhead. Inline the twos_complement code in the _extract_samples() inner loop. Signed-off-by: Kevin O'Connor --- klippy/extras/mpu9250.py | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/klippy/extras/mpu9250.py b/klippy/extras/mpu9250.py index bd882945..c588eaf0 100644 --- a/klippy/extras/mpu9250.py +++ b/klippy/extras/mpu9250.py @@ -42,12 +42,6 @@ FIFO_SIZE = 512 Accel_Measurement = collections.namedtuple( 'Accel_Measurement', ('time', 'accel_x', 'accel_y', 'accel_z')) -# Helper method for getting the two's complement value of an unsigned int -def twos_complement(val, nbits): - if (val & (1 << (nbits - 1))) != 0: - val = val - (1 << nbits) - return val - MIN_MSG_TIME = 0.100 BYTES_PER_SAMPLE = 6 @@ -139,11 +133,12 @@ class MPU9250: for i in range(len(d) // BYTES_PER_SAMPLE): d_xyz = d[i*BYTES_PER_SAMPLE:(i+1)*BYTES_PER_SAMPLE] xhigh, xlow, yhigh, ylow, zhigh, zlow = d_xyz - rx = twos_complement(xhigh << 8 | xlow, 16) - ry = twos_complement(yhigh << 8 | ylow, 16) - rz = twos_complement(zhigh << 8 | zlow, 16) - raw_xyz = (rx, ry, rz) + # Merge and perform twos-complement + rx = ((xhigh << 8) | xlow) - ((xhigh & 0x80) << 9) + ry = ((yhigh << 8) | ylow) - ((yhigh & 0x80) << 9) + rz = ((zhigh << 8) | zlow) - ((zhigh & 0x80) << 9) + raw_xyz = (rx, ry, rz) x = round(raw_xyz[x_pos] * x_scale, 6) y = round(raw_xyz[y_pos] * y_scale, 6) z = round(raw_xyz[z_pos] * z_scale, 6)