diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 48157455296e54f9739ab61286be3bac014b7418..8f4aabda9e226c876b34d6193dee4a31e6b6402f 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -40,7 +40,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro CDev(nullptr), ModuleParams(nullptr), _sensor_accel_pub{ORB_ID(sensor_accel), priority}, - _rotation{get_rot_matrix(rotation)} + _rotation{rotation} { _class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); @@ -103,9 +103,14 @@ void PX4Accelerometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16 sensor_accel_s &report = _sensor_accel_pub.get(); report.timestamp = timestamp; - // Apply rotation, range scale, and the calibrating offset/scale - const matrix::Vector3f val_raw{(float)x, (float)y, (float)z}; - const matrix::Vector3f val_calibrated{ _rotation *(((val_raw * report.scaling) - _calibration_offset).emult(_calibration_scale))}; + // Apply rotation (before scaling) + float xraw_f = x; + float yraw_f = y; + float zraw_f = z; + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + // Apply range scale and the calibrating offset/scale + const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))}; // Filtered values const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)}; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 566dd7012173f5f11f68cdac6fba928d19121f95..72f9a09329478de8ebf89731774891a3cb410597 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -72,7 +72,7 @@ private: math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, false}; - const matrix::Dcmf _rotation; + const enum Rotation _rotation; matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f}; matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};