mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-21 05:03:35 +08:00
PX4Accelerometer apply sensor rotation before scaling
- prevents loss of numerical precision - fixes #11695
This commit is contained in:
committed by
Lorenz Meier
parent
b35883f5d5
commit
5a841761ce
@@ -40,7 +40,7 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Ro
|
|||||||
CDev(nullptr),
|
CDev(nullptr),
|
||||||
ModuleParams(nullptr),
|
ModuleParams(nullptr),
|
||||||
_sensor_accel_pub{ORB_ID(sensor_accel), priority},
|
_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);
|
_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();
|
sensor_accel_s &report = _sensor_accel_pub.get();
|
||||||
report.timestamp = timestamp;
|
report.timestamp = timestamp;
|
||||||
|
|
||||||
// Apply rotation, range scale, and the calibrating offset/scale
|
// Apply rotation (before scaling)
|
||||||
const matrix::Vector3f val_raw{(float)x, (float)y, (float)z};
|
float xraw_f = x;
|
||||||
const matrix::Vector3f val_calibrated{ _rotation *(((val_raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
|
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
|
// Filtered values
|
||||||
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
|
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
|
||||||
|
|||||||
@@ -72,7 +72,7 @@ private:
|
|||||||
math::LowPassFilter2pVector3f _filter{1000, 100};
|
math::LowPassFilter2pVector3f _filter{1000, 100};
|
||||||
Integrator _integrator{4000, false};
|
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_scale{1.0f, 1.0f, 1.0f};
|
||||||
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
|
matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
|
||||||
|
|||||||
Reference in New Issue
Block a user