diff --git a/libraries/AP_HAL_Linux/GPIO.cpp b/libraries/AP_HAL_Linux/GPIO.cpp index 95e83e19b0..c2b5944f0f 100644 --- a/libraries/AP_HAL_Linux/GPIO.cpp +++ b/libraries/AP_HAL_Linux/GPIO.cpp @@ -21,6 +21,7 @@ LinuxGPIO::LinuxGPIO() void LinuxGPIO::init() { +#if LINUX_GPIO_NUM_BANKS == 4 int mem_fd; // Enable all GPIO banks // Without this, access to deactivated banks (i.e. those with no clock source set up) will (logically) fail with SIGBUS @@ -56,6 +57,7 @@ void LinuxGPIO::init() } close(mem_fd); +#endif // LINUX_GPIO_NUM_BANKS } void LinuxGPIO::pinMode(uint8_t pin, uint8_t output) @@ -71,6 +73,7 @@ void LinuxGPIO::pinMode(uint8_t pin, uint8_t output) *gpio_bank[bank].oe &= ~(1U< 0) { uint8_t n = count>8?8:count; struct i2c_msg msgs[2*n]; diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index 6e4e67bc7f..f82543caa7 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -20,20 +20,26 @@ extern const AP_HAL::HAL& hal; #define MHZ (1000U*1000U) +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[LINUX_SPI_DEVICE_NUM_DEVICES] = { - LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MS5611, SPI_MODE_3, 8, BBB_P9_42, 6*MHZ, 6*MHZ), - LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MPU6000, SPI_MODE_3, 8, BBB_P9_28, 500*1000, 20*MHZ), + // different SPI tables per board subtype + LinuxSPIDeviceDriver(1, AP_HAL::SPIDevice_MS5611, SPI_MODE_3, 8, BBB_P9_42, 6*MHZ, 6*MHZ), + LinuxSPIDeviceDriver(1, AP_HAL::SPIDevice_MPU6000, SPI_MODE_3, 8, BBB_P9_28, 500*1000, 20*MHZ), /* MPU9250 is restricted to 1MHz for non-data and interrupt registers */ - LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_MPU9250, SPI_MODE_3, 8, BBB_P9_23, 1*MHZ, 20*MHZ), - LinuxSPIDeviceDriver(0, LINUX_SPI_DEVICE_LSM9DS0, SPI_MODE_3, 8, BBB_P9_17, 10*MHZ,10*MHZ), - LinuxSPIDeviceDriver(1, LINUX_SPI_DEVICE_FRAM, SPI_MODE_0, 8, BBB_P8_12, 6*MHZ, 6*MHZ) + LinuxSPIDeviceDriver(1, AP_HAL::SPIDevice_MPU9250, SPI_MODE_3, 8, BBB_P9_23, 1*MHZ, 20*MHZ), + LinuxSPIDeviceDriver(0, AP_HAL::SPIDevice_LSM9DS0, SPI_MODE_3, 8, BBB_P9_17, 10*MHZ,10*MHZ), + LinuxSPIDeviceDriver(1, AP_HAL::SPIDevice_FRAM, SPI_MODE_0, 8, BBB_P8_12, 6*MHZ, 6*MHZ) }; +#else +// empty device table +LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[0]; +#endif // have a separate semaphore per bus LinuxSemaphore LinuxSPIDeviceManager::_semaphore[LINUX_SPI_NUM_BUSES]; int LinuxSPIDeviceManager::_fd[LINUX_SPI_NUM_BUSES]; -LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(uint8_t bus, LinuxSPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t lowspeed, uint32_t highspeed): +LinuxSPIDeviceDriver::LinuxSPIDeviceDriver(uint8_t bus, enum AP_HAL::SPIDevice type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t lowspeed, uint32_t highspeed): _bus(bus), _type(type), _mode(mode), @@ -112,23 +118,27 @@ void LinuxSPIDeviceManager::init(void *) } } -void LinuxSPIDeviceManager::cs_assert(LinuxSPIDeviceType type) +void LinuxSPIDeviceManager::cs_assert(enum AP_HAL::SPIDevice type) { for (uint8_t i=0; iread() != 1) { hal.scheduler->panic("two CS enabled at once"); } } } - _device[type]._cs->write(0); + for (uint8_t i=0; iwrite(0); + } + } } -void LinuxSPIDeviceManager::cs_release(LinuxSPIDeviceType type) +void LinuxSPIDeviceManager::cs_release(enum AP_HAL::SPIDevice type) { for (uint8_t i=0; i #include "Semaphores.h" -enum LinuxSPIDeviceType { - LINUX_SPI_DEVICE_MS5611 = 0, - LINUX_SPI_DEVICE_MPU6000 = 1, - LINUX_SPI_DEVICE_MPU9250 = 2, - LINUX_SPI_DEVICE_LSM9DS0 = 3, - LINUX_SPI_DEVICE_FRAM = 4, - LINUX_SPI_DEVICE_NUM_DEVICES = 5 -}; - +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE #define LINUX_SPI_NUM_BUSES 2 +#define LINUX_SPI_DEVICE_NUM_DEVICES 5 +#else +#define LINUX_SPI_NUM_BUSES 0 +#define LINUX_SPI_DEVICE_NUM_DEVICES 0 +#endif class Linux::LinuxSPIDeviceDriver : public AP_HAL::SPIDeviceDriver { public: friend class Linux::LinuxSPIDeviceManager; - LinuxSPIDeviceDriver(uint8_t bus, LinuxSPIDeviceType type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t lowspeed, uint32_t highspeed); + LinuxSPIDeviceDriver(uint8_t bus, enum AP_HAL::SPIDevice type, uint8_t mode, uint8_t bitsPerWord, uint8_t cs_pin, uint32_t lowspeed, uint32_t highspeed); void init(); AP_HAL::Semaphore *get_semaphore(); void transaction(const uint8_t *tx, uint8_t *rx, uint16_t len); @@ -38,7 +35,7 @@ private: uint32_t _lowspeed; uint32_t _highspeed; uint32_t _speed; - LinuxSPIDeviceType _type; + enum AP_HAL::SPIDevice _type; uint8_t _bus; }; @@ -49,8 +46,8 @@ public: static AP_HAL::Semaphore *get_semaphore(uint8_t bus); - static void cs_assert(enum LinuxSPIDeviceType type); - static void cs_release(enum LinuxSPIDeviceType type); + static void cs_assert(enum AP_HAL::SPIDevice type); + static void cs_release(enum AP_HAL::SPIDevice type); static void transaction(LinuxSPIDeviceDriver &driver, const uint8_t *tx, uint8_t *rx, uint16_t len); private: