From 3d5c57c5dc72d949f779d9e32c64560cfc94b48d Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 9 Nov 2016 22:34:19 +1100
Subject: [PATCH] AP_Compass: fixed orientation of PH2 in-tree compass drivers

---
 libraries/AP_Compass/AP_Compass.cpp         | 9 +++------
 libraries/AP_Compass/AP_Compass_LSM303D.cpp | 9 ++++++---
 libraries/AP_Compass/AP_Compass_LSM303D.h   | 5 +++--
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index 7ba1e33861..29c73c6a6f 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -514,14 +514,11 @@ void Compass::_detect_backends(void)
                      AP_Compass_LSM303D::name, false);
     }
     if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V3) {
-        _add_backend(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
-                                               false, ROTATION_PITCH_180),
-                     AP_Compass_HMC5843::name, false);
-        _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0),
+        _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_PITCH_180),
                      AP_Compass_AK8963::name, false);
-        _add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME)),
+        _add_backend(AP_Compass_LSM303D::probe(*this, hal.spi->get_device(HAL_INS_LSM9DS0_EXT_A_NAME), ROTATION_YAW_270),
                      AP_Compass_LSM303D::name, false);
-        _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 1),
+        _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 1, ROTATION_YAW_270),
                      AP_Compass_AK8963::name, false);
     }
     if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_PIXRACER) {
diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.cpp b/libraries/AP_Compass/AP_Compass_LSM303D.cpp
index 26498a810d..a5c306a00f 100644
--- a/libraries/AP_Compass/AP_Compass_LSM303D.cpp
+++ b/libraries/AP_Compass/AP_Compass_LSM303D.cpp
@@ -160,13 +160,14 @@ AP_Compass_LSM303D::AP_Compass_LSM303D(Compass &compass, AP_HAL::OwnPtr<AP_HAL::
 }
 
 AP_Compass_Backend *AP_Compass_LSM303D::probe(Compass &compass,
-                                              AP_HAL::OwnPtr<AP_HAL::Device> dev)
+                                              AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                              enum Rotation rotation)
 {
     if (!dev) {
         return nullptr;
     }
     AP_Compass_LSM303D *sensor = new AP_Compass_LSM303D(compass, std::move(dev));
-    if (!sensor || !sensor->init()) {
+    if (!sensor || !sensor->init(rotation)) {
         delete sensor;
         return nullptr;
     }
@@ -254,7 +255,7 @@ bool AP_Compass_LSM303D::_read_sample()
     return true;
 }
 
-bool AP_Compass_LSM303D::init()
+bool AP_Compass_LSM303D::init(enum Rotation rotation)
 {
     if (LSM303D_DRDY_M_PIN >= 0) {
         _drdy_pin_m = hal.gpio->channel(LSM303D_DRDY_M_PIN);
@@ -272,6 +273,8 @@ bool AP_Compass_LSM303D::init()
     /* register the compass instance in the frontend */
     _compass_instance = register_compass();
 
+    set_rotation(_compass_instance, rotation);
+
     _dev->set_device_type(DEVTYPE_LSM303D);
     set_dev_id(_compass_instance, _dev->get_bus_id());
 
diff --git a/libraries/AP_Compass/AP_Compass_LSM303D.h b/libraries/AP_Compass/AP_Compass_LSM303D.h
index e82d6c13eb..beb6934175 100644
--- a/libraries/AP_Compass/AP_Compass_LSM303D.h
+++ b/libraries/AP_Compass/AP_Compass_LSM303D.h
@@ -12,11 +12,11 @@ class AP_Compass_LSM303D : public AP_Compass_Backend
 {
 public:
     static AP_Compass_Backend *probe(Compass &compass,
-                                     AP_HAL::OwnPtr<AP_HAL::Device> dev);
+                                     AP_HAL::OwnPtr<AP_HAL::Device> dev,
+                                     enum Rotation = ROTATION_NONE);
 
     static constexpr const char *name = "LSM303D";
 
-    bool init() override;
     void read() override;
 
     virtual ~AP_Compass_LSM303D() { }
@@ -24,6 +24,7 @@ public:
 private:
     AP_Compass_LSM303D(Compass &compass, AP_HAL::OwnPtr<AP_HAL::Device> dev);
 
+    bool init(enum Rotation rotation);
     uint8_t _register_read(uint8_t reg);
     void _register_write(uint8_t reg, uint8_t val);
     void _register_modify(uint8_t reg, uint8_t clearbits, uint8_t setbits);