diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:25:35 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:25:35 +0100 |
| commit | f36f43db6f2ad545c5b102d6010ecc8176f151a7 (patch) | |
| tree | 4016bba28ac3e4bb140810b442fec75ae9bd3a84 /src/drivers | |
| parent | 7e9984b1d26fd0c1ded8fc7dd9165d83e3683316 (diff) | |
| download | px4-firmware-f36f43db6f2ad545c5b102d6010ecc8176f151a7.tar.gz px4-firmware-f36f43db6f2ad545c5b102d6010ecc8176f151a7.tar.bz2 px4-firmware-f36f43db6f2ad545c5b102d6010ecc8176f151a7.zip | |
L3GD20(H): driver: Rotate before applying offsets.
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/l3gd20/l3gd20.cpp | 16 |
1 files changed, 10 insertions, 6 deletions
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c41491a8..768723640 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -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; |