diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp index 2e407ec3cc..68337a5f78 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.cpp @@ -57,13 +57,23 @@ struct PACKED sample_regs { extern const AP_HAL::HAL &hal; +AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, + AP_HAL::OwnPtr dev, + enum Rotation rotation) + : AP_Compass_Backend(compass) + , _dev(std::move(dev)) + , _rotation(rotation) +{ +} + AP_Compass_Backend *AP_Compass_LSM9DS1::probe(Compass &compass, - AP_HAL::OwnPtr dev) + AP_HAL::OwnPtr dev, + enum Rotation rotation) { if (!dev) { return nullptr; } - AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev)); + AP_Compass_LSM9DS1 *sensor = new AP_Compass_LSM9DS1(compass, std::move(dev), rotation); if (!sensor || !sensor->init()) { delete sensor; return nullptr; @@ -98,6 +108,8 @@ bool AP_Compass_LSM9DS1::init() _compass_instance = register_compass(); + set_rotation(_compass_instance, _rotation); + _dev->set_device_type(DEVTYPE_LSM9DS1); set_dev_id(_compass_instance, _dev->get_bus_id()); @@ -235,12 +247,6 @@ bool AP_Compass_LSM9DS1::_set_scale(void) return true; } -AP_Compass_LSM9DS1::AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr dev) - : AP_Compass_Backend(compass) - , _dev(std::move(dev)) -{ -} - uint8_t AP_Compass_LSM9DS1::_register_read(uint8_t reg) { uint8_t val = 0; diff --git a/libraries/AP_Compass/AP_Compass_LSM9DS1.h b/libraries/AP_Compass/AP_Compass_LSM9DS1.h index 4b814ee3dd..4a68b06445 100644 --- a/libraries/AP_Compass/AP_Compass_LSM9DS1.h +++ b/libraries/AP_Compass/AP_Compass_LSM9DS1.h @@ -12,7 +12,8 @@ class AP_Compass_LSM9DS1 : public AP_Compass_Backend { public: static AP_Compass_Backend *probe(Compass &compass, - AP_HAL::OwnPtr dev); + AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); static constexpr const char *name = "LSM9DS1"; @@ -21,7 +22,8 @@ public: virtual ~AP_Compass_LSM9DS1() {} private: - AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr dev); + AP_Compass_LSM9DS1(Compass &compass, AP_HAL::OwnPtr dev, + enum Rotation rotation = ROTATION_NONE); bool init(); bool _check_id(void); bool _configure(void); @@ -41,4 +43,5 @@ private: float _mag_y_accum; float _mag_z_accum; uint32_t _accum_count; + enum Rotation _rotation; };