From be3af59c76ced526651631ffdb722331eec696c2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 9 Jul 2018 03:01:43 -0400 Subject: [PATCH] lsm303agr flip z axis and properly set scaling and range --- src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp | 14 +++++++------- src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp | 4 +++- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp index bcee090037..9db661e55e 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.cpp @@ -463,10 +463,10 @@ LSM303AGR::collect() mag_report mag_report = {}; mag_report.timestamp = hrt_absolute_time(); - // no auto increment for mag + // switch to right hand coordinate system in place mag_report.x_raw = read_reg(OUTX_L_REG_M) + (read_reg(OUTX_H_REG_M) << 8); mag_report.y_raw = read_reg(OUTY_L_REG_M) + (read_reg(OUTY_H_REG_M) << 8); - mag_report.z_raw = read_reg(OUTZ_L_REG_M) + (read_reg(OUTZ_H_REG_M) << 8); + mag_report.z_raw = -(read_reg(OUTZ_L_REG_M) + (read_reg(OUTZ_H_REG_M) << 8)); float xraw_f = mag_report.x_raw; float yraw_f = mag_report.y_raw; @@ -475,11 +475,11 @@ LSM303AGR::collect() /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - mag_report.x = ((xraw_f) - _mag_scale.x_offset) * _mag_scale.x_scale; - mag_report.y = ((yraw_f) - _mag_scale.y_offset) * _mag_scale.y_scale; - mag_report.z = ((zraw_f) - _mag_scale.z_offset) * _mag_scale.z_scale; - mag_report.scaling = 1.0f; - mag_report.range_ga = (float)_mag_range_ga; + mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; + mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; + mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; + mag_report.scaling = _mag_range_scale; + mag_report.range_ga = _mag_range_ga; mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); /* remember the temperature. The datasheet isn't clear, but it diff --git a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp index 92d1b1f65d..89e91e8ec2 100644 --- a/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp +++ b/src/drivers/magnetometer/lsm303agr/LSM303AGR.hpp @@ -115,7 +115,9 @@ private: unsigned _call_mag_interval{0}; mag_calibration_s _mag_scale{}; - unsigned _mag_range_ga{0}; + + static constexpr float _mag_range_scale{1.5f / 1000.0f}; // 1.5 milligauss/LSB + static constexpr float _mag_range_ga{49.152f}; int _class_instance{-1};