Skip to content

Commit

Permalink
PX4Accelerometer apply sensor rotation before scaling
Browse files Browse the repository at this point in the history
 - prevents loss of numerical precision
 - fixes #11695
  • Loading branch information
dagar authored and LorenzMeier committed Mar 24, 2019
1 parent b35883f commit 5a84176
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 5 deletions.
13 changes: 9 additions & 4 deletions src/lib/drivers/accelerometer/PX4Accelerometer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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)};
Expand Down
2 changes: 1 addition & 1 deletion src/lib/drivers/accelerometer/PX4Accelerometer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
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};
Expand Down

0 comments on commit 5a84176

Please sign in to comment.