|
|
|
@ -4,29 +4,26 @@
@@ -4,29 +4,26 @@
|
|
|
|
|
|
|
|
|
|
namespace PX4 { |
|
|
|
|
|
|
|
|
|
I2CDevice::PX4_I2C::PX4_I2C(uint8_t bus) |
|
|
|
|
: I2C("AP_I2C", "/dev/api2c", bus, 0, 400000UL) |
|
|
|
|
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) |
|
|
|
|
{ |
|
|
|
|
init(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool I2CDevice::PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len, |
|
|
|
|
uint8_t *recv, uint32_t recv_len) |
|
|
|
|
bool 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) |
|
|
|
|
{ |
|
|
|
@ -57,6 +54,10 @@ AP_HAL::Semaphore *I2CDevice::get_semaphore()
@@ -57,6 +54,10 @@ AP_HAL::Semaphore *I2CDevice::get_semaphore()
|
|
|
|
|
return &semaphore; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
I2CDevice::~I2CDevice() |
|
|
|
|
{ |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
I2CDeviceManager::I2CDeviceManager() |
|
|
|
|
{ |
|
|
|
|
} |
|
|
|
@ -64,8 +65,9 @@ I2CDeviceManager::I2CDeviceManager()
@@ -64,8 +65,9 @@ I2CDeviceManager::I2CDeviceManager()
|
|
|
|
|
AP_HAL::OwnPtr<AP_HAL::I2CDevice> |
|
|
|
|
I2CDeviceManager::get_device(uint8_t bus, uint8_t address) |
|
|
|
|
{ |
|
|
|
|
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(bus, address)); |
|
|
|
|
AP_HAL::OwnPtr<PX4_I2C> i2c { new PX4_I2C(bus) }; |
|
|
|
|
auto dev = AP_HAL::OwnPtr<AP_HAL::I2CDevice>(new I2CDevice(*i2c, address)); |
|
|
|
|
|
|
|
|
|
return dev; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|