From 2359a5a1ea48d2e61869e5e257779c9e503f3b4a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Jan 2021 12:30:24 +1100 Subject: [PATCH] Tools: fixed stopping of capture on enable change in tempcal --- Tools/scripts/tempcal_IMU.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Tools/scripts/tempcal_IMU.py b/Tools/scripts/tempcal_IMU.py index d63c08e8c2..3299299ba8 100755 --- a/Tools/scripts/tempcal_IMU.py +++ b/Tools/scripts/tempcal_IMU.py @@ -104,8 +104,7 @@ class Coefficients: temperature = constrain(temperature, self.tmin[imu], self.tmax[imu]) cal_temp = constrain(cal_temp, self.tmin[imu], self.tmax[imu]) poly = np.poly1d(coeff[axis]) - ret = poly(cal_temp - TEMP_REF) - poly(temperature - TEMP_REF) - return ret + return poly(cal_temp - TEMP_REF) - poly(temperature - TEMP_REF) def correction_accel(self, imu, temperature): '''calculate accel correction from temperature calibration from @@ -336,6 +335,9 @@ def IMUfit(logfile): if msg.get_type() == 'IMU' and not args.tclr: imu = msg.I + if stop_capture[imu]: + continue + T = msg.T acc = Vector3(msg.AccX, msg.AccY, msg.AccZ) gyr = Vector3(msg.GyrX, msg.GyrY, msg.GyrZ)