diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index 7dc0694c8d..b2df5ac436 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -42,8 +42,8 @@ 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]; +LinuxSemaphore LinuxSPIDeviceManager::_semaphore[LINUX_SPI_MAX_BUSES]; +int LinuxSPIDeviceManager::_fd[LINUX_SPI_MAX_BUSES]; 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), @@ -111,15 +111,23 @@ void LinuxSPIDeviceDriver::transfer(const uint8_t *data, uint16_t len) void LinuxSPIDeviceManager::init(void *) { - char path[] = "/dev/spidevN.0"; - for (uint8_t i=0; ipanic("SPIDriver: unable to open SPI bus"); - } + for (uint8_t i=0; i= LINUX_SPI_MAX_BUSES) { + hal.scheduler->panic("SPIDriver: invalid bus number"); + } + if (_fd[_device[i]._bus] == -1) { + char path[] = "/dev/spidevN.0"; + path[11] = '0' + _device[i]._bus; + _fd[_device[i]._bus] = open(path, O_RDWR); + if (_fd[_device[i]._bus] == -1) { + hal.scheduler->panic("SPIDriver: unable to open SPI bus"); + } + printf("Opened %s\n", path); + fflush(stdout); + } _device[i].init(); } } diff --git a/libraries/AP_HAL_Linux/SPIDriver.h b/libraries/AP_HAL_Linux/SPIDriver.h index afbb472d3a..144ebe60f7 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.h +++ b/libraries/AP_HAL_Linux/SPIDriver.h @@ -6,16 +6,15 @@ #include "Semaphores.h" #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 6 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO -#define LINUX_SPI_NUM_BUSES 1 #define LINUX_SPI_DEVICE_NUM_DEVICES 1 #else -#define LINUX_SPI_NUM_BUSES 0 #define LINUX_SPI_DEVICE_NUM_DEVICES 0 #endif +#define LINUX_SPI_MAX_BUSES 3 + class Linux::LinuxSPIDeviceDriver : public AP_HAL::SPIDeviceDriver { public: friend class Linux::LinuxSPIDeviceManager; @@ -55,8 +54,8 @@ public: private: static LinuxSPIDeviceDriver _device[LINUX_SPI_DEVICE_NUM_DEVICES]; - static LinuxSemaphore _semaphore[LINUX_SPI_NUM_BUSES]; - static int _fd[LINUX_SPI_NUM_BUSES]; + static LinuxSemaphore _semaphore[LINUX_SPI_MAX_BUSES]; + static int _fd[LINUX_SPI_MAX_BUSES]; }; #endif // __AP_HAL_LINUX_SPIDRIVER_H__