diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:25:19 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-15 17:25:19 +0100 |
commit | 7e9984b1d26fd0c1ded8fc7dd9165d83e3683316 (patch) | |
tree | baa24322ad30c5cee5f230a616b2701d0fbf4eb7 /src/drivers | |
parent | ad54ff616dc83a703fe51c2a80a0662618116782 (diff) | |
download | px4-firmware-7e9984b1d26fd0c1ded8fc7dd9165d83e3683316.tar.gz px4-firmware-7e9984b1d26fd0c1ded8fc7dd9165d83e3683316.tar.bz2 px4-firmware-7e9984b1d26fd0c1ded8fc7dd9165d83e3683316.zip |
HMC5883 driver: Rotate before applying offsets.
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/hmc5883/hmc5883.cpp | 22 |
1 files changed, 15 insertions, 7 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 7560ef20b..2b3520fc8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -848,6 +848,10 @@ HMC5883::collect() struct mag_report new_report; bool sensor_is_onboard = false; + float xraw_f; + float yraw_f; + float zraw_f; + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); new_report.error_count = perf_event_count(_comms_errors); @@ -907,17 +911,21 @@ HMC5883::collect() report.x = -report.x; } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; - /* flip axes and negate value for y */ - new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale; - /* z remains z */ - new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + xraw_f = -report.y; + yraw_f = report.x; + zraw_f = report.z; // apply user specified rotation - rotate_3f(_rotation, new_report.x, new_report.y, new_report.z); + rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); + + new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* flip axes and negate value for y */ + new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale; + /* z remains z */ + new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale; if (!(_pub_blocked)) { |