From 8ab33c46ce69ccc7f14c9a2b9bd48c14ba87f0f0 Mon Sep 17 00:00:00 2001
From: Staroselskii Georgii <gosha371@gmail.com>
Date: Wed, 22 Jul 2015 14:50:32 +0300
Subject: [PATCH] AP_Compass: revisited AK8963 code

Reduced duplication a bit and revisited the update method which is now
split into several logical hunks.
---
 libraries/AP_Compass/AP_Compass_AK8963.cpp | 34 ++++++++++++++++------
 libraries/AP_Compass/AP_Compass_AK8963.h   |  4 +++
 2 files changed, 29 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp
index 957b7df5d7..29fbaad804 100644
--- a/libraries/AP_Compass/AP_Compass_AK8963.cpp
+++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp
@@ -102,8 +102,7 @@ AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_SerialBus *bus)
     _last_accum_time(0),
     _bus(bus)
 {
-    _mag_x_accum =_mag_y_accum = _mag_z_accum = 0;
-    _accum_count = 0;
+    _reset_filter();
 }
 
 AP_Compass_Backend *AP_Compass_AK8963::detect_mpu9250(Compass &compass)
@@ -213,14 +212,10 @@ void AP_Compass_AK8963::read()
         return;
     }
 
-    /* Update */
-    Vector3f field(_mag_x_accum * _magnetometer_ASA[0],
-                   _mag_y_accum * _magnetometer_ASA[1],
-                   _mag_z_accum * _magnetometer_ASA[2]);
+    auto field = _get_filtered_field();
+    _make_factory_sensitivity_adjustment(field);
 
-    field /= _accum_count;
-    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
-    _accum_count = 0;
+    _reset_filter();
 
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP
     field.rotate(ROTATION_YAW_90);
@@ -229,6 +224,27 @@ void AP_Compass_AK8963::read()
     publish_field(field, _compass_instance);
 }
 
+Vector3f AP_Compass_AK8963::_get_filtered_field() const
+{
+    Vector3f field(_mag_x_accum, _mag_y_accum, _mag_z_accum);
+    field /= _accum_count;
+
+    return field;
+}
+
+void AP_Compass_AK8963::_reset_filter()
+{
+    _mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
+    _accum_count = 0;
+}
+
+void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) const
+{
+    field.x *= _magnetometer_ASA[0];
+    field.y *= _magnetometer_ASA[1];
+    field.z *= _magnetometer_ASA[2];
+}
+
 void AP_Compass_AK8963::_update()
 {
     if (hal.scheduler->micros() - _last_update_timestamp < 10000) {
diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h
index 825277f24c..c52558987b 100644
--- a/libraries/AP_Compass/AP_Compass_AK8963.h
+++ b/libraries/AP_Compass/AP_Compass_AK8963.h
@@ -46,6 +46,10 @@ public:
     void        accumulate(void);
 
 private:
+    void _make_factory_sensitivity_adjustment(Vector3f& field) const;
+    Vector3f _get_filtered_field() const;
+    void _reset_filter();
+
     bool _reset();
     bool _setup_mode();
     bool _check_id();