diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp index 636b9fe66f..a1c6d18923 100644 --- a/libraries/AP_HAL_PX4/I2CDevice.cpp +++ b/libraries/AP_HAL_PX4/I2CDevice.cpp @@ -4,26 +4,29 @@ namespace PX4 { -class PX4_I2C : public device::I2C { -public: - PX4_I2C(uint8_t bus); - bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, - uint8_t *recv, uint32_t recv_len); -}; - -PX4_I2C::PX4_I2C(uint8_t bus) : - I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL) +I2CDevice::PX4_I2C::PX4_I2C(uint8_t bus) + : I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL) { init(); } -bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, - uint8_t *recv, uint32_t recv_len) +bool I2CDevice::PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) { set_address(address); return transfer(send, send_len, recv, recv_len) == OK; } +I2CDevice::I2CDevice(uint8_t bus, uint8_t address) + : _device(bus) + , _address(address) +{ +} + +I2CDevice::~I2CDevice() +{ +} + bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) { @@ -54,10 +57,6 @@ AP_HAL::Semaphore *I2CDevice::get_semaphore() return &semaphore; } -I2CDevice::~I2CDevice() -{ -} - I2CDeviceManager::I2CDeviceManager() { } @@ -65,9 +64,8 @@ I2CDeviceManager::I2CDeviceManager() AP_HAL::OwnPtr I2CDeviceManager::get_device(uint8_t bus, uint8_t address) { - AP_HAL::OwnPtr i2c { new PX4_I2C(bus) }; - auto dev = AP_HAL::OwnPtr(new I2CDevice(*i2c, address)); - + auto dev = AP_HAL::OwnPtr(new I2CDevice(bus, address)); return dev; } + } diff --git a/libraries/AP_HAL_PX4/I2CDevice.h b/libraries/AP_HAL_PX4/I2CDevice.h index 9bad240e68..819f5f41ef 100644 --- a/libraries/AP_HAL_PX4/I2CDevice.h +++ b/libraries/AP_HAL_PX4/I2CDevice.h @@ -21,11 +21,7 @@ public: } /* AP_HAL::I2CDevice implementation */ - I2CDevice(PX4_I2C &device, uint8_t address) - : _device(device) - , _address(address) - { - } + I2CDevice(uint8_t bus, uint8_t address); ~I2CDevice(); @@ -61,7 +57,14 @@ public: int get_fd() override; protected: - PX4_I2C &_device; + class PX4_I2C : public device::I2C { + public: + PX4_I2C(uint8_t bus); + bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len); + }; + + PX4_I2C _device; uint8_t _address; uint8_t _retries = 0;