From 5a841761cecbcec3b3995aedecb4a540c1237689 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 24 Mar 2019 13:44:01 -0400
Subject: [PATCH] PX4Accelerometer apply sensor rotation before scaling
- prevents loss of numerical precision
- fixes #11695
---
src/lib/drivers/accelerometer/PX4Accelerometer.cpp | 13 +++++++++----
src/lib/drivers/accelerometer/PX4Accelerometer.hpp | 2 +-
2 files changed, 10 insertions(+), 5 deletions(-)
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp
index 4815745529..8f4aabda9e 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 566dd70121..72f9a09329 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};
--
GitLab