From 693613aa0f970586ab07f71741699cdc17ea8ed4 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Fri, 24 Jul 2015 01:50:45 +0300 Subject: [PATCH] AP_Compass: convert AK8963 measurements to uT AK8963 is configured in 16-bit ADC mode which implies sensitivity of 0.15 uT/LSb. Knowing this fact we can convert the measurements to the proper units. The change will make users recalibrate their compasses. --- libraries/AP_Compass/AP_Compass_AK8963.cpp | 9 +++++++++ libraries/AP_Compass/AP_Compass_AK8963.h | 1 + 2 files changed, 10 insertions(+) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index c9d50c028e..42d236e0f7 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -204,9 +204,11 @@ void AP_Compass_AK8963::read() hal.scheduler->suspend_timer_procs(); auto field = _get_filtered_field(); + _reset_filter(); hal.scheduler->resume_timer_procs(); _make_factory_sensitivity_adjustment(field); + _make_adc_sensitivity_adjustment(field); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP field.rotate(ROTATION_YAW_90); @@ -234,6 +236,13 @@ void AP_Compass_AK8963::_reset_filter() _accum_count = 0; } +void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const +{ + static const float ADC_16BIT_RESOLUTION = 0.15f; + + field *= ADC_16BIT_RESOLUTION; +} + void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) const { field.x *= _magnetometer_ASA[0]; diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index abae432010..0794177bc9 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -52,6 +52,7 @@ private: static AP_Compass_Backend *_detect(Compass &compass, AP_AK8963_SerialBus *bus); void _make_factory_sensitivity_adjustment(Vector3f& field) const; + void _make_adc_sensitivity_adjustment(Vector3f& field) const; Vector3f _get_filtered_field() const; void _reset_filter();