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 .
*
* Driver by RadioLink LjWang, Jun 2017
+ * GPS compass module See
*/
#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 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 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 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 dev,
+ bool force_external,
enum Rotation rotation);
void timer();
@@ -54,4 +56,5 @@ private:
enum Rotation _rotation;
uint8_t _instance;
+ bool _force_external:1;
};