AP_Compass: added support for per-instance rotations

libraries/AP_Compass/AP_Compass.cpp

@@ -503,7 +503,8 @@ void Compass::_detect_backends(void)
         _add_backend(AP_Compass_HMC5843::probe(*this, hal.i2c_mgr->get_device(0, HAL_COMPASS_HMC5843_I2C_ADDR), false),
                          AP_Compass_HMC5843::name, false);
         // try for SPI device too
-        _add_backend(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME), false),
+        _add_backend(AP_Compass_HMC5843::probe(*this, hal.spi->get_device(HAL_COMPASS_HMC5843_NAME),
+                                               false, ROTATION_PITCH_180),
                      AP_Compass_HMC5843::name, false);
     }
     if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V2) {

libraries/AP_Compass/AP_Compass.h

@@ -375,6 +375,9 @@ private:
         // when we last got data
         uint32_t    last_update_ms;
         uint32_t    last_update_usec;
+
+        // board specific orientation
+        enum Rotation rotation;
     } _state[COMPASS_MAX_INSTANCES];
 
     CompassCalibrator _calibrator[COMPASS_MAX_INSTANCES];

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -15,6 +15,7 @@ void AP_Compass_Backend::rotate_field(Vector3f &mag, uint8_t instance)
 {
     Compass::mag_state &state = _compass._state[instance];
     mag.rotate(MAG_BOARD_ORIENTATION);
+    mag.rotate(state.rotation);
 
     if (!state.external) {
         // and add in AHRS_ORIENTATION setting if not an external compass
@@ -122,3 +123,9 @@ bool AP_Compass_Backend::is_external(uint8_t instance)
 {
     return _compass._state[instance].external;
 }
+
+// set rotation of an instance
+void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation)
+{
+    _compass._state[instance].rotation = rotation;
+}

libraries/AP_Compass/AP_Compass_Backend.h

@@ -91,6 +91,9 @@ protected:
     // tell if instance is an external compass
     bool is_external(uint8_t instance);
 
+    // set rotation of an instance
+    void set_rotation(uint8_t instance, enum Rotation rotation);
+    
     // access to frontend
     Compass &_compass;
 

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -89,10 +89,11 @@ extern const AP_HAL::HAL& hal;
 #define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03
 
 AP_Compass_HMC5843::AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus,
-                                       bool force_external)
+                                       bool force_external, enum Rotation rotation)
     : AP_Compass_Backend(compass)
     , _bus(bus)
     , _force_external(force_external)
+    , _rotation(rotation)
 {
 }
 
@@ -103,14 +104,15 @@ AP_Compass_HMC5843::~AP_Compass_HMC5843()
 
 AP_Compass_Backend *AP_Compass_HMC5843::probe(Compass &compass,
                                               AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                              bool force_external)
+                                              bool force_external,
+                                              enum Rotation rotation)
 {
     AP_HMC5843_BusDriver *bus = new AP_HMC5843_BusDriver_HALDevice(std::move(dev));
     if (!bus) {
         return nullptr;
     }
 
-    AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, force_external);
+    AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, force_external, rotation);
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -119,7 +121,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe(Compass &compass,
     return sensor;
 }
 
-AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass)
+AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass, enum Rotation rotation)
 {
     AP_InertialSensor &ins = *AP_InertialSensor::get_instance();
 
@@ -130,7 +132,7 @@ AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(Compass &compass)
         return nullptr;
     }
 
-    AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, false);
+    AP_Compass_HMC5843 *sensor = new AP_Compass_HMC5843(compass, bus, false, rotation);
     if (!sensor || !sensor->init()) {
         delete sensor;
         return nullptr;
@@ -181,6 +183,8 @@ bool AP_Compass_HMC5843::init()
 
     _compass_instance = register_compass();
 
+    set_rotation(_compass_instance, _rotation);
+    
     _bus->set_device_type(DEVTYPE_HMC5883);
     set_dev_id(_compass_instance, _bus->get_bus_id());
 

libraries/AP_Compass/AP_Compass_HMC5843.h

@@ -18,9 +18,11 @@ class AP_Compass_HMC5843 : public AP_Compass_Backend
 public:
     static AP_Compass_Backend *probe(Compass &compass,
                                      AP_HAL::OwnPtr<AP_HAL::Device> dev,
-                                     bool force_external = false);
+                                     bool force_external = false,
+                                     enum Rotation rotation = ROTATION_NONE);
 
-    static AP_Compass_Backend *probe_mpu6000(Compass &compass);
+    static AP_Compass_Backend *probe_mpu6000(Compass &compass,
+                                             enum Rotation rotation = ROTATION_NONE);
 
     static constexpr const char *name = "HMC5843";
 
@@ -31,7 +33,7 @@ public:
 
 private:
     AP_Compass_HMC5843(Compass &compass, AP_HMC5843_BusDriver *bus,
-                       bool force_external);
+                       bool force_external, enum Rotation rotation);
 
     bool _detect_version();
     bool _calibrate();
@@ -62,6 +64,8 @@ private:
     uint8_t _compass_instance;
     uint8_t _gain_config;
 
+    enum Rotation _rotation;
+    
     bool _is_hmc5883L:1;
     bool _initialised:1;
     bool _force_external:1;