From c2c7284a942452a8d10ca43de2dda0d236b2e80d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 1 Nov 2016 19:10:52 +1100 Subject: [PATCH] HAL_PX4: implement SPIDevice code for PX4 with thread per bus --- libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h | 1 + libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 5 +- libraries/AP_HAL_PX4/SPIDevice.cpp | 217 ++++++++++++++++++++ libraries/AP_HAL_PX4/SPIDevice.h | 113 ++++++++++ libraries/AP_HAL_PX4/SPIWrapper.h | 32 +++ libraries/AP_HAL_PX4/Scheduler.h | 1 + 6 files changed, 367 insertions(+), 2 deletions(-) create mode 100644 libraries/AP_HAL_PX4/SPIDevice.cpp create mode 100644 libraries/AP_HAL_PX4/SPIDevice.h create mode 100644 libraries/AP_HAL_PX4/SPIWrapper.h diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index 1642b466a6..8871ec6906 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h @@ -14,5 +14,6 @@ namespace PX4 { class NSHShellStream; class PX4I2CDriver; class PX4_I2C; + class PX4_SPI; class Semaphore; } diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index cd493ba5c4..f6704f0a0c 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp @@ -14,6 +14,7 @@ #include "Util.h" #include "GPIO.h" #include "I2CDevice.h" +#include "SPIDevice.h" #include #include @@ -29,7 +30,6 @@ using namespace PX4; -static Empty::SPIDeviceManager spiDeviceManager; //static Empty::GPIO gpioDriver; static PX4Scheduler schedulerInstance; @@ -41,6 +41,7 @@ static PX4Util utilInstance; static PX4GPIO gpioDriver; static PX4::I2CDeviceManager i2c_mgr_instance; +static PX4::SPIDeviceManager spi_mgr_instance; #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #define UARTA_DEFAULT_DEVICE "/dev/ttyACM0" @@ -82,7 +83,7 @@ HAL_PX4::HAL_PX4() : &uartEDriver, /* uartE */ &uartFDriver, /* uartF */ &i2c_mgr_instance, - &spiDeviceManager, /* spi */ + &spi_mgr_instance, &analogIn, /* analogin */ &storageDriver, /* storage */ &uartADriver, /* console */ diff --git a/libraries/AP_HAL_PX4/SPIDevice.cpp b/libraries/AP_HAL_PX4/SPIDevice.cpp new file mode 100644 index 0000000000..6aaacddd6c --- /dev/null +++ b/libraries/AP_HAL_PX4/SPIDevice.cpp @@ -0,0 +1,217 @@ +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ +#include "SPIDevice.h" + +#include +#include "board_config.h" +#include +#include +#include +#include +#include "Scheduler.h" +#include "Semaphores.h" + +extern bool _px4_thread_should_exit; + +namespace PX4 { + +static const AP_HAL::HAL &hal = AP_HAL::get_HAL(); + +#define MHZ (1000U*1000U) +#define KHZ (1000U) + +SPIDesc SPIDeviceManager::device_table[] = { + SPIDesc("mpu6000", PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_MPU, SPIDEV_MODE3, 500*KHZ, 20*MHZ), + SPIDesc("ms5611", PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, SPIDEV_MODE3, 10*MHZ, 10*MHZ), + + SPIDesc(nullptr, 0, (spi_dev_e)0, (spi_mode_e)0, 0, 0), +}; + +SPIDevice::SPIDevice(SPIBus &_bus, SPIDesc &_device_desc) + : bus(_bus) + , device_desc(_device_desc) + , px4dev(device_desc.bus, device_desc.name, device_desc.devname, device_desc.device, device_desc.mode, device_desc.lowspeed) +{ +} + +SPIDevice::~SPIDevice() +{ +} + +bool SPIDevice::set_speed(AP_HAL::Device::Speed speed) +{ + switch (speed) { + case AP_HAL::Device::SPEED_HIGH: + px4dev.set_speed(device_desc.highspeed); + break; + case AP_HAL::Device::SPEED_LOW: + px4dev.set_speed(device_desc.lowspeed); + break; + } + return true; +} + +bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) +{ + uint8_t buf[send_len+recv_len]; + if (send_len > 0) { + memcpy(buf, send, send_len); + } + if (recv_len > 0) { + memset(&buf[send_len], 0, recv_len); + } + bool ret = px4dev.do_transfer(buf, buf, send_len+recv_len); + if (recv_len > 0 && ret) { + memcpy(recv, &buf[send_len], recv_len); + } + return ret; +} + +bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len) +{ + uint8_t buf[len]; + memcpy(buf, send, len); + bool ret = px4dev.do_transfer(buf, buf, len); + if (ret) { + memcpy(recv, buf, len); + } + return ret; +} + +AP_HAL::Semaphore *SPIDevice::get_semaphore() +{ + return &bus.semaphore; +} + +/* + per-bus spi callback thread +*/ +void *SPIDevice::spi_thread(void *arg) +{ + struct SPIBus *binfo = (struct SPIBus *)arg; + while (!_px4_thread_should_exit) { + uint64_t now = AP_HAL::micros64(); + uint64_t next_needed = 0; + SPIBus::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; +} + +AP_HAL::Device::PeriodicHandle SPIDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) +{ + if (!bus.thread_started) { + bus.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_SPI_PRIORITY; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO); + + pthread_create(&bus.thread_ctx, &thread_attr, &SPIDevice::spi_thread, &bus); + } + SPIBus::callback_info *callback = new SPIBus::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 = bus.callbacks; + bus.callbacks = callback; + + return callback; +} + +bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec) +{ + return false; +} + + +/* + return a SPIDevice given a string device name + */ +AP_HAL::OwnPtr +SPIDeviceManager::get_device(const char *name) +{ + /* Find the bus description in the table */ + uint8_t i; + for (i = 0; device_table[i].name; i++) { + if (strcmp(device_table[i].name, name) == 0) { + break; + } + } + if (device_table[i].name == nullptr) { + AP_HAL::panic("SPI: invalid device name: %s", name); + } + + SPIDesc &desc = device_table[i]; + + // find the bus + struct SPIBus *busp; + for (busp = buses; busp; busp = busp->next) { + if (busp->bus == desc.bus) { + break; + } + } + if (busp == nullptr) { + // create a new one + busp = new SPIBus; + if (busp == nullptr) { + return nullptr; + } + busp->next = buses; + busp->bus = desc.bus; + buses = busp; + } + + return AP_HAL::OwnPtr(new SPIDevice(*busp, desc)); +} + +} diff --git a/libraries/AP_HAL_PX4/SPIDevice.h b/libraries/AP_HAL_PX4/SPIDevice.h new file mode 100644 index 0000000000..e0f47fc504 --- /dev/null +++ b/libraries/AP_HAL_PX4/SPIDevice.h @@ -0,0 +1,113 @@ +/* + * This file is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published + * by the Free Software Foundation, either version 3 of the License, + * or (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ +#pragma once + +#include +#include +#include +#include +#include "Semaphores.h" +#include "SPIWrapper.h" + +namespace PX4 { + +class SPIDesc; + +struct SPIBus { + struct SPIBus *next; + struct callback_info { + struct callback_info *next; + AP_HAL::Device::PeriodicCb cb; + uint32_t period_usec; + uint64_t next_usec; + } *callbacks; + Semaphore semaphore; + pthread_t thread_ctx; + bool thread_started; + uint8_t bus; +}; + +struct SPIDesc { + SPIDesc(const char *_name, uint8_t _bus, + enum spi_dev_e _device, enum spi_mode_e _mode, + uint32_t _lowspeed, uint32_t _highspeed) + : name(_name), bus(_bus), device(_device), mode(_mode), + lowspeed(_lowspeed), highspeed(_highspeed) + { + snprintf(devname, sizeof(devname), "/dev/%s", name); + } + + const char *name; + char devname[20]; + uint8_t bus; + enum spi_dev_e device; + enum spi_mode_e mode; + uint32_t lowspeed; + uint32_t highspeed; +}; + + +class SPIDevice : public AP_HAL::SPIDevice { +public: + SPIDevice(SPIBus &_bus, SPIDesc &_device_desc); + + virtual ~SPIDevice(); + + /* See AP_HAL::Device::set_speed() */ + bool set_speed(AP_HAL::Device::Speed speed) override; + + /* See AP_HAL::Device::transfer() */ + bool transfer(const uint8_t *send, uint32_t send_len, + uint8_t *recv, uint32_t recv_len) override; + + /* See AP_HAL::SPIDevice::transfer_fullduplex() */ + bool transfer_fullduplex(const uint8_t *send, uint8_t *recv, + uint32_t len) override; + + /* See AP_HAL::Device::get_semaphore() */ + AP_HAL::Semaphore *get_semaphore() override; + + /* See AP_HAL::Device::register_periodic_callback() */ + AP_HAL::Device::PeriodicHandle register_periodic_callback( + 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; + +private: + SPIBus &bus; + SPIDesc &device_desc; + PX4_SPI px4dev; + + static void *spi_thread(void *arg); +}; + +class SPIDeviceManager : public AP_HAL::SPIDeviceManager { +public: + friend class SPIDevice; + + static SPIDeviceManager *from(AP_HAL::SPIDeviceManager *spi_mgr) + { + return static_cast(spi_mgr); + } + + AP_HAL::OwnPtr get_device(const char *name); + +private: + static SPIDesc device_table[]; + struct SPIBus *buses; +}; + +} diff --git a/libraries/AP_HAL_PX4/SPIWrapper.h b/libraries/AP_HAL_PX4/SPIWrapper.h new file mode 100644 index 0000000000..b64f73e479 --- /dev/null +++ b/libraries/AP_HAL_PX4/SPIWrapper.h @@ -0,0 +1,32 @@ +#include + +#include +#include "board_config.h" +#include +#include "AP_HAL_PX4.h" + +extern const AP_HAL::HAL& hal; +/* + wrapper class for SPI to expose protected functions from PX4Firmware + */ +class PX4::PX4_SPI : public device::SPI { +public: + PX4_SPI(uint8_t bus, const char *name, const char *devname, enum spi_dev_e device, enum spi_mode_e mode, uint32_t frequency) : + SPI(name, devname, bus, device, mode, frequency) {} + + bool do_transfer(uint8_t *send, uint8_t *recv, uint32_t len) { + if (!init_done) { + if (init() != 0) { + return false; + } + init_done = true; + } + return transfer(send, recv, len) == 0; + } + void set_speed(uint32_t speed_hz) { + set_frequency(speed_hz); + } +private: + bool init_done; +}; + diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index fcb3d7b463..7caac0b9da 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -13,6 +13,7 @@ #define APM_MAIN_PRIORITY_BOOST 241 #define APM_MAIN_PRIORITY 180 #define APM_TIMER_PRIORITY 181 +#define APM_SPI_PRIORITY 182 #define APM_I2C_PRIORITY 178 #define APM_UART_PRIORITY 60 #define APM_STORAGE_PRIORITY 59