From 63dc6b5bc95712fd1be82fb905a6d5458b9282c7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 13 Mar 2023 15:03:08 +1300 Subject: [PATCH] ICM45686: fix clipping due to rotation It turns out that when you rotate by 45 degrees, as required on the CubeOrange+, then you can easily get into clipping because the vector components are constrained after the rotation. In order to avoid that, we have to avoid getting close to the int16 range and switch from 20 bit resolution to 16bit resolution earlier. Signed-off-by: Julian Oes --- .../imu/invensense/icm45686/ICM45686.cpp | 29 +++++++++++++++---- 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/src/drivers/imu/invensense/icm45686/ICM45686.cpp b/src/drivers/imu/invensense/icm45686/ICM45686.cpp index e6a2ad7619..07fdf6633a 100644 --- a/src/drivers/imu/invensense/icm45686/ICM45686.cpp +++ b/src/drivers/imu/invensense/icm45686/ICM45686.cpp @@ -543,9 +543,18 @@ void ICM45686::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sample invalid if -524288 if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) { - // check if any values are going to exceed int16 limits - static constexpr int16_t max_accel = INT16_MAX; - static constexpr int16_t min_accel = INT16_MIN; + + // It's not enough to check if any values are exceeding the + // int16 limits because there might be a rotation applied later. + // If a rotation is 45 degrees, the new component can be up to + // sqrt(2) longer than one component. This means the number has + // to be constrained to fit the int16 which then triggers + // clipping. + // + // Therefore, we set the limits at int16_max/min / sqrt(2) plus + // a bit of margin. + static constexpr int16_t max_accel = static_cast(INT16_MAX / sqrt(2.f)) - 100; + static constexpr int16_t min_accel = static_cast(INT16_MIN / sqrt(2.f)) + 100; if (accel_x >= max_accel || accel_x <= min_accel) { scale_20bit = true; @@ -634,9 +643,17 @@ void ICM45686::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_YL, fifo[i].GYRO_DATA_YH, fifo[i].HIGHRES_Y_LSB & 0x0F); int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_ZL, fifo[i].GYRO_DATA_ZH, fifo[i].HIGHRES_Z_LSB & 0x0F); - // check if any values are going to exceed int16 limits - static constexpr int16_t max_gyro = INT16_MAX; - static constexpr int16_t min_gyro = INT16_MIN; + // It's not enough to check if any values are exceeding the + // int16 limits because there might be a rotation applied later. + // If a rotation is 45 degrees, the new component can be up to + // sqrt(2) longer than one component. This means the number has + // to be constrained to fit the int16 which then triggers + // clipping. + // + // Therefore, we set the limits at int16_max/min / sqrt(2) plus + // a bit of margin. + static constexpr int16_t max_gyro = static_cast(INT16_MAX / sqrt(2.f)) - 100; + static constexpr int16_t min_gyro = static_cast(INT16_MIN / sqrt(2.f)) + 100; if (gyro_x >= max_gyro || gyro_x <= min_gyro) { scale_20bit = true;