diff options
author | Lorenz Meier <lorenz@px4.io> | 2015-03-16 00:04:03 +0100 |
---|---|---|
committer | Lorenz Meier <lorenz@px4.io> | 2015-03-16 00:04:03 +0100 |
commit | bba6f0ae1d484bba367121e8d33c60a193420353 (patch) | |
tree | 6aa7e32ae2df9ddfb9536e3e1614be311238d513 /src/drivers/hmc5883/hmc5883.cpp | |
parent | 9935707acdcd47189ae08b9eb8104f1981884d8c (diff) | |
parent | 92a52ea1953e0e17e7a55512b484adf7048e619c (diff) | |
download | px4-firmware-bba6f0ae1d484bba367121e8d33c60a193420353.tar.gz px4-firmware-bba6f0ae1d484bba367121e8d33c60a193420353.tar.bz2 px4-firmware-bba6f0ae1d484bba367121e8d33c60a193420353.zip |
Merge pull request #1919 from PX4/ekf_gyro
EKF gyro offsetfix
Diffstat (limited to 'src/drivers/hmc5883/hmc5883.cpp')
-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)) { |