diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp index 44e5fe9a97..445f1c978b 100644 --- a/libraries/AP_HAL_PX4/I2CDevice.cpp +++ b/libraries/AP_HAL_PX4/I2CDevice.cpp @@ -17,16 +17,54 @@ #include #include "Util.h" +#include "Scheduler.h" + +extern bool _px4_thread_should_exit; namespace PX4 { uint8_t PX4::PX4_I2C::instance; +/* + constructor for I2C wrapper class + */ +PX4_I2C::PX4_I2C(uint8_t bus) : + I2C(devname, devpath, map_bus_number(bus), 0, 400000UL) +{} + +/* + map ArduPilot bus numbers to PX4 bus numbers + */ +uint8_t PX4_I2C::map_bus_number(uint8_t bus) const +{ + switch (bus) { + case 0: + // map to internal bus +#ifdef PX4_I2C_BUS_ONBOARD + return PX4_I2C_BUS_ONBOARD; +#else + return 0; +#endif + + case 1: + // map to expansion bus +#ifdef PX4_I2C_BUS_EXPANSION + return PX4_I2C_BUS_EXPANSION; +#else + return 1; +#endif + + } + // default to bus 1 + return 1; +} + /* implement wrapper for PX4 I2C driver */ 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); if (!init_done) { init_done = true; // we do late init() so we can setup the device paths @@ -40,13 +78,15 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le if (!init_ok) { return false; } - set_address(address); bool ret = (transfer(send, send_len, recv, recv_len) == OK); return ret; } + +PX4::I2CDevice::bus_info PX4::I2CDevice::businfo[num_buses]; I2CDevice::I2CDevice(uint8_t bus, uint8_t address) : - _bus(bus), + _busnum(bus), + _px4dev(_busnum), _address(address) { } @@ -58,7 +98,7 @@ I2CDevice::~I2CDevice() bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len, uint8_t *recv, uint32_t recv_len) { - return _bus.do_transfer(_address, send, send_len, recv, recv_len); + return _px4dev.do_transfer(_address, send, send_len, recv, recv_len); } bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, @@ -67,6 +107,122 @@ bool I2CDevice::read_registers_multiple(uint8_t first_reg, uint8_t *recv, return false; } +/* + per-bus i2c callback thread +*/ +void *I2CDevice::i2c_thread(void *arg) +{ + struct bus_info *binfo = (struct bus_info *)arg; + while (!_px4_thread_should_exit) { + uint64_t now = AP_HAL::micros64(); + uint64_t next_needed = 0; + struct callback_info *callback; + + // find a callback to run + for (callback = binfo->callbacks; callback; callback = callback->next) { + if (now >= callback->next_usec) { + while (now >= callback->next_usec) { + callback->next_usec += callback->period_usec; + } + // call it with semaphore held + if (binfo->semaphore.take(0)) { + callback->cb(); + binfo->semaphore.give(); + } + } + if (next_needed == 0 || + callback->next_usec < next_needed) { + next_needed = callback->next_usec; + } + } + + // delay for at most 50ms, to handle newly added callbacks + now = AP_HAL::micros64(); + uint32_t delay = 50000; + if (next_needed > now && next_needed - now < delay) { + delay = next_needed - now; + } + hal.scheduler->delay_microseconds(delay); + } + return nullptr; +} + + +/* + find or create thread for this bus +*/ +I2CDevice::bus_info *I2CDevice::find_thread(void) +{ + if (_busnum >= num_buses) { + return nullptr; + } + struct bus_info &binfo = businfo[_busnum]; + if (binfo.thread_started) { + return &binfo; + } + + binfo.bus = _busnum; + binfo.thread_started = true; + + pthread_attr_t thread_attr; + struct sched_param param; + + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 1024); + + param.sched_priority = APM_I2C_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&binfo.thread_ctx, &thread_attr, &I2CDevice::i2c_thread, &binfo); + + return &binfo; +} + +/* + register a periodic callback +*/ +AP_HAL::Device::PeriodicHandle I2CDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + struct bus_info *binfo = find_thread(); + if (binfo == nullptr) { + return nullptr; + } + struct callback_info *callback = new callback_info; + if (callback == nullptr) { + return nullptr; + } + callback->cb = cb; + callback->period_usec = period_usec; + callback->next_usec = AP_HAL::micros64() + period_usec; + + // add to linked list of callbacks on thread + callback->next = binfo->callbacks; + binfo->callbacks = callback; + + return callback; +} + + +/* + adjust a periodic callback +*/ +bool I2CDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + struct bus_info *binfo = find_thread(); + if (binfo == nullptr) { + return nullptr; + } + + for (struct callback_info *callback = binfo->callbacks; callback; callback = callback->next) { + if (h == (AP_HAL::Device::PeriodicHandle)callback) { + callback->period_usec = period_usec; + return true; + } + } + return false; +} + AP_HAL::OwnPtr I2CDeviceManager::get_device(uint8_t bus, uint8_t address) { diff --git a/libraries/AP_HAL_PX4/I2CDevice.h b/libraries/AP_HAL_PX4/I2CDevice.h index d256f6c81c..709ded7676 100644 --- a/libraries/AP_HAL_PX4/I2CDevice.h +++ b/libraries/AP_HAL_PX4/I2CDevice.h @@ -21,8 +21,7 @@ #include #include #include -#include -#include +#include "Semaphores.h" #include "I2CWrapper.h" namespace PX4 { @@ -41,7 +40,7 @@ public: void set_address(uint8_t address) override { _address = address; } /* See AP_HAL::I2CDevice::set_retries() */ - void set_retries(uint8_t retries) override { _retries = retries; } + void set_retries(uint8_t retries) override { _px4dev.set_retries(retries); } /* See AP_HAL::Device::set_speed(): Empty implementation, not supported. */ bool set_speed(enum Device::Speed speed) override { return true; } @@ -55,27 +54,43 @@ public: /* See AP_HAL::Device::register_periodic_callback() */ AP_HAL::Device::PeriodicHandle register_periodic_callback( - uint32_t period_usec, AP_HAL::Device::PeriodicCb) override - { - /* Not implemented yet */ - return nullptr; - } + uint32_t period_usec, AP_HAL::Device::PeriodicCb) override; /* See AP_HAL::Device::adjust_periodic_callback() */ - bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override - { - /* Not implemented yet */ - return false; + bool adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) override; + + AP_HAL::Semaphore* get_semaphore() override { + // if asking for invalid bus number use bus 0 semaphore + return &businfo[_busnum