diff --git a/libraries/AP_HAL_Linux/SPIDriver.cpp b/libraries/AP_HAL_Linux/SPIDriver.cpp index f1f41d1693..e0c56ac6be 100644 --- a/libraries/AP_HAL_Linux/SPIDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIDriver.cpp @@ -20,6 +20,7 @@ using namespace Linux; extern const AP_HAL::HAL& hal; #define MHZ (1000U*1000U) +#define KHZ (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] = { @@ -36,7 +37,7 @@ LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[LINUX_SPI_DEVICE_NUM_DEVICES LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[LINUX_SPI_DEVICE_NUM_DEVICES] = { /* MPU9250 is restricted to 1MHz for non-data and interrupt registers */ LinuxSPIDeviceDriver(0, 1, AP_HAL::SPIDevice_MPU9250, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 16*MHZ), - LinuxSPIDeviceDriver(0, 0, AP_HAL::SPIDevice_Ublox, SPI_MODE_0, 8, SPI_CS_KERNEL, 1*MHZ, 4*MHZ), + LinuxSPIDeviceDriver(0, 0, AP_HAL::SPIDevice_Ublox, SPI_MODE_0, 8, SPI_CS_KERNEL, 250*KHZ, 4*MHZ), }; #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI LinuxSPIDeviceDriver LinuxSPIDeviceManager::_device[LINUX_SPI_DEVICE_NUM_DEVICES] = { diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index 60d15699f2..a2a52a1579 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -9,10 +9,12 @@ extern const AP_HAL::HAL& hal; -#define SPIUART_DEBUG 1 -#if SPIUART_DEBUG +#define SPIUART_DEBUG 0 + #include -#define debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) + +#if SPIUART_DEBUG +#define debug(fmt, args ...) do {hal.console->printf("[SPIUARTDriver]: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) #define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) #else #define debug(fmt, args ...) @@ -86,7 +88,14 @@ void LinuxSPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _writebuf_tail = 0; } - _buffer = new uint8_t[rxS]; + if (_buffer == NULL) { + /* Do not allocate new buffer, if we're just changing speed */ + _buffer = new uint8_t[rxS]; + if (_buffer == NULL) { + hal.console->printf("Not enough memory\n"); + hal.scheduler->panic("Not enough memory\n"); + } + } _spi = hal.spi->device(AP_HAL::SPIDevice_Ublox); @@ -96,7 +105,27 @@ void LinuxSPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _spi_sem = _spi->get_semaphore(); - _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + switch (b) { + case 4000000U: + if (is_initialized()) { + /* Do not allow speed changes before device is initialized, because + * it can lead to misconfiguraration. Once the device is initialized, + * it's sage to update speed + */ + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); + debug("Set higher SPI-frequency"); + } else { + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + debug("Set lower SPI-frequency"); + } + break; + default: + _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); + debug("Set lower SPI-frequency"); + debug("%s: wrong baudrate (%u) for SPI-driven device. setting default speed", __func__, b); + break; + } + _initialised = true; }