From 5a5ef74606d4eef4b364105af801cdf86060074b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jun 2021 11:00:37 -0400 Subject: [PATCH] drivers/imu: use math::negate on int16 data - this handles the size difference between INT16_MIN (-32768) and INT16_MAX (32767) --- .../imu/analog_devices/adis16448/ADIS16448.cpp | 12 ++++++------ .../imu/analog_devices/adis16470/ADIS16470.cpp | 8 ++++---- .../imu/bosch/bmi055/BMI055_Accelerometer.cpp | 4 ++-- src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp | 4 ++-- .../imu/bosch/bmi088/BMI088_Accelerometer.cpp | 4 ++-- src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp | 4 ++-- .../imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp | 4 ++-- .../imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp | 12 ++++++------ src/drivers/imu/invensense/icm20602/ICM20602.cpp | 8 ++++---- src/drivers/imu/invensense/icm20608g/ICM20608G.cpp | 8 ++++---- src/drivers/imu/invensense/icm20649/ICM20649.cpp | 8 ++++---- src/drivers/imu/invensense/icm20689/ICM20689.cpp | 8 ++++---- src/drivers/imu/invensense/icm20948/ICM20948.cpp | 8 ++++---- src/drivers/imu/invensense/icm40609d/ICM40609D.cpp | 8 ++++---- src/drivers/imu/invensense/icm42605/ICM42605.cpp | 8 ++++---- src/drivers/imu/invensense/icm42670p/ICM42670P.cpp | 8 ++++---- src/drivers/imu/invensense/icm42688p/ICM42688P.cpp | 8 ++++---- src/drivers/imu/invensense/mpu6000/MPU6000.cpp | 8 ++++---- src/drivers/imu/invensense/mpu6500/MPU6500.cpp | 8 ++++---- src/drivers/imu/invensense/mpu9250/MPU9250.cpp | 8 ++++---- src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp | 8 ++++---- src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp | 4 ++-- src/drivers/magnetometer/isentek/ist8308/IST8308.cpp | 2 +- src/drivers/magnetometer/isentek/ist8310/IST8310.cpp | 2 +- src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp | 4 ++-- src/drivers/magnetometer/qmc5883l/QMC5883L.cpp | 4 ++-- .../magnetometer/vtrantech/vcm1193l/VCM1193L.cpp | 2 +- 27 files changed, 87 insertions(+), 87 deletions(-) diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index f5e92604c7..f4dd61c68f 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -407,8 +407,8 @@ void ADIS16448::RunImpl() // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) const int16_t accel_x = buffer.XACCL_OUT; - const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; - const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; + const int16_t accel_y = math::negate(buffer.YACCL_OUT); + const int16_t accel_z = math::negate(buffer.ZACCL_OUT); if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) { imu_updated = true; @@ -419,8 +419,8 @@ void ADIS16448::RunImpl() } const int16_t gyro_x = buffer.XGYRO_OUT; - const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; - const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; + const int16_t gyro_y = math::negate(buffer.YGYRO_OUT); + const int16_t gyro_z = math::negate(buffer.ZGYRO_OUT); if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) { imu_updated = true; @@ -441,8 +441,8 @@ void ADIS16448::RunImpl() _px4_mag.set_temperature(temperature); const int16_t mag_x = buffer.XMAGN_OUT; - const int16_t mag_y = (buffer.YMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.YMAGN_OUT; - const int16_t mag_z = (buffer.ZMAGN_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZMAGN_OUT; + const int16_t mag_y = math::negate(buffer.YMAGN_OUT); + const int16_t mag_z = math::negate(buffer.ZMAGN_OUT); _px4_mag.update(timestamp_sample, mag_x, mag_y, mag_z); _px4_baro.set_error_count(error_count); diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 7df959803f..634063aa56 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -306,8 +306,8 @@ void ADIS16470::RunImpl() // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel_y = math::negate(accel_y); + accel_z = math::negate(accel_z); _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); @@ -317,8 +317,8 @@ void ADIS16470::RunImpl() int16_t gyro_z = buffer.Z_GYRO_OUT; // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) - gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro_y = math::negate(gyro_y); + gyro_z = math::negate(gyro_z); _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); success = true; diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp index 77cf537302..3f2963b29b 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp @@ -431,8 +431,8 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[i] = accel_x; - accel.y[i] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[i] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[i] = math::negate(accel_y); + accel.z[i] = math::negate(accel_z); } _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index d2ea514530..d9c3113f93 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -428,8 +428,8 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp index 8f0dc0068b..1f69b9d2c8 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp @@ -507,8 +507,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; fifo_buffer_index += 7; // move forward to next record diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 7dac23e578..f4ebfea006 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -429,8 +429,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp index 6d7e4d6739..c7ab0df4f2 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp @@ -410,8 +410,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime ×tamp_sample, uint8_t // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; fifo_buffer_index += 7; // move forward to next record diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp index ad177c68e5..ba64cf1cdd 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp @@ -356,8 +356,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime ×tamp_sample, uint8_t sam // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + @@ -444,8 +444,8 @@ bool BMI088_Gyroscope::NormalRead(const hrt_abstime ×tamp_sample) // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) x = gyro_x; - y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + y = math::negate(gyro_y); + z = math::negate(gyro_z); _px4_gyro.update(timestamp_sample, x, y, z); @@ -494,8 +494,8 @@ bool BMI088_Gyroscope::SimpleFIFORead(const hrt_abstime ×tamp_sample) }; gyro.x[i] = xyz[0]; - gyro.y[i] = (xyz[1] == INT16_MIN) ? INT16_MAX : -xyz[1]; - gyro.z[i] = (xyz[2] == INT16_MIN) ? INT16_MAX : -xyz[2]; + gyro.y[i] = math::negate(xyz[1]); + gyro.z[i] = math::negate(xyz[2]); gyro.samples++; } diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index c13ab0b800..29c8d5af2b 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -637,8 +637,8 @@ bool ICM20602::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -667,8 +667,8 @@ void ICM20602::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 06fe4b048e..5d58955499 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -580,8 +580,8 @@ bool ICM20608G::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -610,8 +610,8 @@ void ICM20608G::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index e38c9381b8..b366eafa80 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -616,8 +616,8 @@ bool ICM20649::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -646,8 +646,8 @@ void ICM20649::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 8337ba2cc4..b06e1ab7b6 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -580,8 +580,8 @@ bool ICM20689::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -610,8 +610,8 @@ void ICM20689::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index d95cb636c3..de091854d9 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -663,8 +663,8 @@ bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -693,8 +693,8 @@ void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 894c9cd7c2..b320f73cfc 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -605,8 +605,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -633,8 +633,8 @@ void ICM40609D::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.cpp b/src/drivers/imu/invensense/icm42605/ICM42605.cpp index 88ddb8b30e..ef5d94fd40 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.cpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.cpp @@ -606,8 +606,8 @@ void ICM42605::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -634,8 +634,8 @@ void ICM42605::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp index f66eb13e2e..2f294cd1f7 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.cpp @@ -599,8 +599,8 @@ void ICM42670P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -627,8 +627,8 @@ void ICM42670P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 0c8e66f862..3755196df5 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -655,8 +655,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[i] = accel.x[i]; - accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i]; - accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i]; + accel.y[i] = math::negate(accel.y[i]); + accel.z[i] = math::negate(accel.z[i]); } _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + @@ -727,8 +727,8 @@ void ICM42688P::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DAT // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro.x[i]; - gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i]; - gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i]; + gyro.y[i] = math::negate(gyro.y[i]); + gyro.z[i] = math::negate(gyro.z[i]); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index ffdb9bafb0..1041af6625 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -540,8 +540,8 @@ bool MPU6000::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } else if (new_sample && (_fifo_accel_samples_count > 1)) { @@ -582,8 +582,8 @@ void MPU6000::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 2f29db0f79..7858ec06a9 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -585,8 +585,8 @@ bool MPU6500::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -615,8 +615,8 @@ void MPU6500::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 501f5f97db..c9d21cfd39 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -630,8 +630,8 @@ void MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -658,8 +658,8 @@ void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp index 73a87a71db..0f73bbd380 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -533,8 +533,8 @@ bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO:: // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; - accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.y[accel.samples] = math::negate(accel_y); + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } @@ -563,8 +563,8 @@ void MPU9250_I2C::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::D // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[i] = gyro_x; - gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; - gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.y[i] = math::negate(gyro_y); + gyro.z[i] = math::negate(gyro_z); } _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + diff --git a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp index b5577eff63..c5ce3afb7f 100644 --- a/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp +++ b/src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp @@ -360,7 +360,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // flip y & z to publish right handed with z down (x forward, y right, z down) gyro.x[gyro.samples] = gyro_x; gyro.y[gyro.samples] = gyro_y; - gyro.z[gyro.samples] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + gyro.z[gyro.samples] = math::negate(gyro_z); gyro.samples++; } else { @@ -388,7 +388,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) // flip y & z to publish right handed with z down (x forward, y right, z down) accel.x[accel.samples] = accel_x; accel.y[accel.samples] = accel_y; - accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.z[accel.samples] = math::negate(accel_z); accel.samples++; } else { diff --git a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp index c65a5c95b7..8d804e6171 100644 --- a/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp +++ b/src/drivers/magnetometer/isentek/ist8308/IST8308.cpp @@ -184,7 +184,7 @@ void IST8308::RunImpl() int16_t z = combine(buffer.DATAZH, buffer.DATAZL); // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + z = math::negate(z); // flip z _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index cc98c5df60..2abb93fdfc 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -185,7 +185,7 @@ void IST8310::RunImpl() int16_t z = combine(buffer.DATAZH, buffer.DATAZL); // sensor's frame is +x forward, +y right, +z up - z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z + z = math::negate(z); // flip z _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp index bd245fa638..2726224e4f 100644 --- a/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp +++ b/src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp @@ -175,8 +175,8 @@ void LSM9DS1_MAG::RunImpl() // sensor Z is up (RHC), flip z for publication // sensor X is aligned with -X of lsm9ds1 accel/gyro - x = (x == INT16_MIN) ? INT16_MAX : -x; - z = (z == INT16_MIN) ? INT16_MAX : -z; + x = math::negate(x); + z = math::negate(z); _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp index 8adb51818a..fcf6d686cd 100644 --- a/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp +++ b/src/drivers/magnetometer/qmc5883l/QMC5883L.cpp @@ -196,8 +196,8 @@ void QMC5883L::RunImpl() // Forward X := +X // Right Y := -Y // Down Z := -Z - y = (y == INT16_MIN) ? INT16_MAX : -y; // -y - z = (z == INT16_MIN) ? INT16_MAX : -z; // -z + y = math::negate(y); // -y + z = math::negate(z); // -z _px4_mag.update(now, x, y, z); diff --git a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp index 24ceb46964..530de2bb2c 100644 --- a/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp +++ b/src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp @@ -190,7 +190,7 @@ void VCM1193L::RunImpl() _prev_data[2] = z; // Data Sheet is incorrect - y = (y == INT16_MIN) ? INT16_MAX : -y; // -y + y = math::negate(y); // -y _px4_mag.update(now, x, y, z);