From 3d271245a1052ea612b88a4ddf871286356cba94 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 24 Mar 2019 13:52:18 -0400 Subject: [PATCH] PX4Gyroscope apply sensor rotation before scaling - prevents loss of numerical precision --- src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 13 +++++++++---- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 2 +- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 9cac8cfc39..491cfe4ca3 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -40,7 +40,7 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r CDev(nullptr), ModuleParams(nullptr), _sensor_gyro_pub{ORB_ID(sensor_gyro), priority}, - _rotation{get_rot_matrix(rotation)} + _rotation{rotation} { _class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); @@ -103,9 +103,14 @@ void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z sensor_gyro_s &report = _sensor_gyro_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/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 26df883be2..6c878251ae 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -72,7 +72,7 @@ private: math::LowPassFilter2pVector3f _filter{1000, 100}; Integrator _integrator{4000, true}; - 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};