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

PX4Accelerometer apply sensor rotation before scaling

Browse Source 
- prevents loss of numerical precision
 - fixes #11695
This commit is contained in:
Daniel Agar 2019-03-24 13:44:01 -04:00 committed by Lorenz Meier
parent b35883f5d5
commit 5a841761ce
2 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

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

2
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

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