diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp index 5c37547015..675e0f42e0 100644 --- a/libraries/AP_Compass/AP_Compass.cpp +++ b/libraries/AP_Compass/AP_Compass.cpp @@ -520,7 +520,7 @@ void Compass::_detect_backends(void) AP_Compass_AK8963::name, false); } if (AP_BoardConfig::get_board_type() == AP_BoardConfig::PX4_BOARD_TEST_V4) { - _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0), + _add_backend(AP_Compass_AK8963::probe_mpu9250(*this, 0, ROTATION_ROLL_180_YAW_90), AP_Compass_AK8963::name, false); } // also add any px4 level drivers (for canbus magnetometers) diff --git a/libraries/AP_Compass/AP_Compass_AK8963.cpp b/libraries/AP_Compass/AP_Compass_AK8963.cpp index 42a4c6fe94..9631c4123e 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.cpp +++ b/libraries/AP_Compass/AP_Compass_AK8963.cpp @@ -52,9 +52,11 @@ struct PACKED sample_regs { extern const AP_HAL::HAL &hal; -AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus) +AP_Compass_AK8963::AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus, + enum Rotation rotation) : AP_Compass_Backend(compass) , _bus(bus) + , _rotation(rotation) { } @@ -64,14 +66,15 @@ AP_Compass_AK8963::~AP_Compass_AK8963() } AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass, - AP_HAL::OwnPtr dev) + AP_HAL::OwnPtr dev, + enum Rotation rotation) { AP_AK8963_BusDriver *bus = new AP_AK8963_BusDriver_HALDevice(std::move(dev)); if (!bus) { return nullptr; } - AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus); + AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -81,17 +84,19 @@ AP_Compass_Backend *AP_Compass_AK8963::probe(Compass &compass, } AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, - AP_HAL::OwnPtr dev) + AP_HAL::OwnPtr dev, + enum Rotation rotation) { AP_InertialSensor &ins = *AP_InertialSensor::get_instance(); /* Allow MPU9250 to shortcut auxiliary bus and host bus */ ins.detect_backends(); - return probe(compass, std::move(dev)); + return probe(compass, std::move(dev), rotation); } -AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance) +AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t mpu9250_instance, + enum Rotation rotation) { AP_InertialSensor &ins = *AP_InertialSensor::get_instance(); @@ -101,7 +106,7 @@ AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(Compass &compass, uint8_t m return nullptr; } - AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus); + AP_Compass_AK8963 *sensor = new AP_Compass_AK8963(compass, bus, rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -149,6 +154,8 @@ bool AP_Compass_AK8963::init() /* register the compass instance in the frontend */ _compass_instance = register_compass(); + set_rotation(_compass_instance, _rotation); + _bus->set_device_type(DEVTYPE_AK8963); set_dev_id(_compass_instance, _bus->get_bus_id()); diff --git a/libraries/AP_Compass/AP_Compass_AK8963.h b/libraries/AP_Compass/AP_Compass_AK8963.h index 943cb1f863..e8d106bfac 100644 --- a/libraries/AP_Compass/AP_Compass_AK8963.h +++ b/libraries/AP_Compass/AP_Compass_AK8963.h @@ -19,14 +19,17 @@ class AP_Compass_AK8963 : public AP_Compass_Backend public: /* Probe for AK8963 standalone on I2C bus */ static AP_Compass_Backend *probe(Compass &compass, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through I2C */ static AP_Compass_Backend *probe_mpu9250(Compass &compass, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); /* Probe for AK8963 on auxiliary bus of MPU9250, connected through SPI */ - static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance); + static AP_Compass_Backend *probe_mpu9250(Compass &compass, uint8_t mpu9250_instance, + enum Rotation rotation = ROTATION_NONE); static constexpr const char *name = "AK8963"; @@ -36,7 +39,8 @@ public: void read() override; private: - AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus); + AP_Compass_AK8963(Compass &compass, AP_AK8963_BusDriver *bus, + enum Rotation rotation = ROTATION_NONE); void _make_factory_sensitivity_adjustment(Vector3f &field) const; void _make_adc_sensitivity_adjustment(Vector3f &field) const; @@ -61,6 +65,7 @@ private: uint8_t _compass_instance; bool _initialized; bool _timesliced; + enum Rotation _rotation; }; class AP_AK8963_BusDriver