fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/imu: use math::negate on int16 data

Browse Source 
- this handles the size difference between INT16_MIN (-32768) and INT16_MAX (32767)
This commit is contained in:
Daniel Agar 2021-06-29 11:00:37 -04:00
parent 4cf8eb8226
commit 5a5ef74606
No known key found for this signature in database
GPG Key ID: FD3CBA98017A69DE
27 changed files with 87 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp
View File

@ -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);

8
src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp
View File

@ -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;

4
src/drivers/imu/bosch/bmi055/BMI055_Accelerometer.cpp
View File

@ -431,8 +431,8 @@ bool BMI055_Accelerometer::FIFORead(const hrt_abstime &timestamp_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) +

4
src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp
View File

@ -428,8 +428,8 @@ bool BMI055_Gyroscope::FIFORead(const hrt_abstime &timestamp_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) +

4
src/drivers/imu/bosch/bmi088/BMI088_Accelerometer.cpp
View File

@ -507,8 +507,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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

4
src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp
View File

@ -429,8 +429,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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) +

4
src/drivers/imu/bosch/bmi088_i2c/BMI088_Accelerometer.cpp
View File

@ -410,8 +410,8 @@ bool BMI088_Accelerometer::FIFORead(const hrt_abstime &timestamp_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

12
src/drivers/imu/bosch/bmi088_i2c/BMI088_Gyroscope.cpp
View File

@ -356,8 +356,8 @@ bool BMI088_Gyroscope::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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 &timestamp_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++;
}

8
src/drivers/imu/invensense/icm20602/ICM20602.cpp
View File

@ -637,8 +637,8 @@ bool ICM20602::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
View File

@ -580,8 +580,8 @@ bool ICM20608G::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm20649/ICM20649.cpp
View File

@ -616,8 +616,8 @@ bool ICM20649::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm20689/ICM20689.cpp
View File

@ -580,8 +580,8 @@ bool ICM20689::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

@ -663,8 +663,8 @@ bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
View File

@ -605,8 +605,8 @@ void ICM40609D::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm42605/ICM42605.cpp
View File

@ -606,8 +606,8 @@ void ICM42605::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm42670p/ICM42670P.cpp
View File

@ -599,8 +599,8 @@ void ICM42670P::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
View File

@ -655,8 +655,8 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/mpu6000/MPU6000.cpp
View File

@ -540,8 +540,8 @@ bool MPU6000::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
View File

@ -585,8 +585,8 @@ bool MPU6500::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

@ -630,8 +630,8 @@ void MPU9250::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

8
src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp
View File

@ -533,8 +533,8 @@ bool MPU9250_I2C::ProcessAccel(const hrt_abstime &timestamp_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 &timestamp_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) +

4
src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp
View File

@ -360,7 +360,7 @@ bool LSM9DS1::FIFORead(const hrt_abstime &timestamp_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 &timestamp_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 {

2
src/drivers/magnetometer/isentek/ist8308/IST8308.cpp
View File

@ -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);

2
src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
View File

@ -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);

4
src/drivers/magnetometer/lsm9ds1_mag/LSM9DS1_MAG.cpp
View File

@ -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);

4
src/drivers/magnetometer/qmc5883l/QMC5883L.cpp
View File

@ -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);

2
src/drivers/magnetometer/vtrantech/vcm1193l/VCM1193L.cpp
View File

@ -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);