fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 21:59:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

L3GD20(H): driver: Rotate before applying offsets.

Browse Source
This commit is contained in:
Lorenz Meier 2015-03-15 17:25:35 +01:00
parent 7e9984b1d2
commit f36f43db6f
1 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/drivers/l3gd20/l3gd20.cpp
View File

@ -1029,9 +1029,16 @@ L3GD20::measure()
report.temperature_raw = raw_report.temp;
report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
float xraw_f = report.x_raw;
float yraw_f = report.y_raw;
float zraw_f = report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x);
report.y = _gyro_filter_y.apply(report.y);
@ -1039,9 +1046,6 @@ L3GD20::measure()
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
// apply user specified rotation
rotate_3f(_rotation, report.x, report.y, report.z);
report.scaling = _gyro_range_scale;
report.range_rad_s = _gyro_range_rad_s;