From 603e903ccdd8748f8a6a7783e97f451fdd2e3121 Mon Sep 17 00:00:00 2001
From: ljwang <wlj@radiolink.com.cn>
Date: Thu, 22 Jun 2017 14:37:00 +0800
Subject: [PATCH] AP_Compass: QMC5883 Add internal i2c bus and fix rotation
 error

---
 libraries/AP_Compass/AP_Compass.cpp          | 10 +++++++---
 libraries/AP_Compass/AP_Compass_QMC5883L.cpp | 20 ++++++++++++++++----
 libraries/AP_Compass/AP_Compass_QMC5883L.h   |  3 +++
 3 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index c413582a7a..37da3759ff 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -551,10 +551,14 @@ void Compass::_detect_backends(void)
         ADD_BACKEND(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR),
                                               both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
                     AP_Compass_HMC5843::name, both_i2c_external);
-        //external i2c bus for QMC5883L
+        //external i2c bus
         ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(1, HAL_COMPASS_QMC5883L_I2C_ADDR),
-                                 ROTATION_YAW_90), AP_Compass_QMC5883L::name, true);
-
+                								true,ROTATION_ROLL_180),
+        			AP_Compass_QMC5883L::name, true);
+        //internal i2c bus
+        ADD_BACKEND(AP_Compass_QMC5883L::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_QMC5883L_I2C_ADDR),
+                								both_i2c_external, both_i2c_external?ROTATION_ROLL_180:ROTATION_YAW_270),
+        			AP_Compass_QMC5883L::name,both_i2c_external);
         
 #if !HAL_MINIMIZE_FEATURES
         // AK09916 on ICM20948
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp
index b291cdda2d..63e6a16fe3 100644
--- a/libraries/AP_Compass/AP_Compass_QMC5883L.cpp
+++ b/libraries/AP_Compass/AP_Compass_QMC5883L.cpp
@@ -15,6 +15,7 @@
  * with this program.  If not, see <http://www.gnu.org/licenses/>.
  *
  * Driver by RadioLink LjWang, Jun 2017
+ * GPS compass module See<http://www.radiolink.com>
  */
 #include "AP_Compass_QMC5883L.h"
 
@@ -58,13 +59,14 @@ extern const AP_HAL::HAL &hal;
 
 AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
                                               AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
-                                              enum Rotation rotation)
+											  bool force_external,
+											  enum Rotation rotation)
 {
     if (!dev) {
         return nullptr;
     }
 
-    AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),rotation);
+    AP_Compass_QMC5883L *sensor = new AP_Compass_QMC5883L(compass, std::move(dev),force_external,rotation);
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -75,10 +77,12 @@ AP_Compass_Backend *AP_Compass_QMC5883L::probe(Compass &compass,
 
 AP_Compass_QMC5883L::AP_Compass_QMC5883L(Compass &compass,
                                        AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                       enum Rotation rotation)
+									   bool force_external,
+									   enum Rotation rotation)
     : AP_Compass_Backend(compass)
     , _dev(std::move(dev))
     , _rotation(rotation)
+	, _force_external(force_external)
 {
 }
 
@@ -125,6 +129,10 @@ bool AP_Compass_QMC5883L::init()
     _dev->set_device_type(DEVTYPE_QMC5883L);
     set_dev_id(_instance, _dev->get_bus_id());
 
+    if (_force_external) {
+        set_external(_instance, true);
+    }
+
     //Enable 100HZ
     _dev->register_periodic_callback(10000,
         FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void));
@@ -173,6 +181,11 @@ void AP_Compass_QMC5883L::timer()
 
     Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale };
 
+    // rotate to the desired orientation
+    if (is_external(_instance)) {
+        field.rotate(ROTATION_YAW_90);
+    }
+
     /* rotate raw_field from sensor frame to body frame */
     rotate_field(field, _instance);
 
@@ -191,7 +204,6 @@ void AP_Compass_QMC5883L::timer()
         	_accum.z /= 2;
         	_accum_count = 10;
         }
-
         _sem->give();
     }
 }
diff --git a/libraries/AP_Compass/AP_Compass_QMC5883L.h b/libraries/AP_Compass/AP_Compass_QMC5883L.h
index ac70d8537e..d39ed34c64 100644
--- a/libraries/AP_Compass/AP_Compass_QMC5883L.h
+++ b/libraries/AP_Compass/AP_Compass_QMC5883L.h
@@ -33,6 +33,7 @@ class AP_Compass_QMC5883L : public AP_Compass_Backend
 public:
     static AP_Compass_Backend *probe(Compass &compass,
                                      AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
+									 bool force_external,
                                      enum Rotation rotation = ROTATION_NONE);
 
     void read() override;
@@ -42,6 +43,7 @@ public:
 private:
     AP_Compass_QMC5883L(Compass &compass,
                        AP_HAL::OwnPtr<AP_HAL::Device> dev,
+					   bool force_external,
                        enum Rotation rotation);
 
     void timer();
@@ -54,4 +56,5 @@ private:
 
     enum Rotation _rotation;
     uint8_t _instance;
+    bool _force_external:1;
 };