diff --git a/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp b/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp index b26fb7ca33..ecde45095b 100644 --- a/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp +++ b/libraries/AP_HAL_AVR/SPIDevice_SPI0.cpp @@ -69,6 +69,32 @@ inline uint8_t AVRSPI0DeviceDriver::_transfer(uint8_t data) { return read_spdr; } +/** + a specialised transfer function for the MPU6k. This saves 2 usec + per byte + */ +void AVRSPI0DeviceDriver::_transfer15(const uint8_t *tx, uint8_t *rx) +{ + spi0_transferflag = true; +#define TRANSFER1(i) do { SPDR = tx[i]; while(!(SPSR & _BV(SPIF))); rx[i] = SPDR; } while(0) + TRANSFER1(0); + TRANSFER1(1); + TRANSFER1(2); + TRANSFER1(3); + TRANSFER1(4); + TRANSFER1(5); + TRANSFER1(6); + TRANSFER1(7); + TRANSFER1(8); + TRANSFER1(9); + TRANSFER1(10); + TRANSFER1(11); + TRANSFER1(12); + TRANSFER1(13); + TRANSFER1(14); + spi0_transferflag = false; +} + void AVRSPI0DeviceDriver::transfer(const uint8_t *tx, uint16_t len) { for (uint16_t i = 0; i < len; i++) { _transfer(tx[i]); @@ -83,6 +109,12 @@ void AVRSPI0DeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, _transfer(tx[i]); } } else { + while (len >= 15) { + _transfer15(tx, rx); + tx += 15; + rx += 15; + len -= 15; + } for (uint16_t i = 0; i < len; i++) { rx[i] = _transfer(tx[i]); } diff --git a/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp b/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp index 97b85fc75f..6080c2fbc9 100644 --- a/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp +++ b/libraries/AP_HAL_AVR/SPIDevice_SPI2.cpp @@ -69,6 +69,34 @@ inline uint8_t AVRSPI2DeviceDriver::_transfer(uint8_t data) { return UDR2; } +/** + a specialist transfer function for the APM1 ADC + */ +void AVRSPI2DeviceDriver::_transfer17(const uint8_t *tx, uint8_t *rx) +{ +#define TRANSFER1(i) do { while ( !( UCSR2A & _BV(UDRE2)) ); \ + UDR2 = tx[i]; \ + while ( !(UCSR2A & _BV(RXC2)) ) ; \ + rx[i] = UDR2; } while (0) + TRANSFER1(0); + TRANSFER1(1); + TRANSFER1(2); + TRANSFER1(3); + TRANSFER1(4); + TRANSFER1(5); + TRANSFER1(6); + TRANSFER1(7); + TRANSFER1(8); + TRANSFER1(9); + TRANSFER1(10); + TRANSFER1(11); + TRANSFER1(12); + TRANSFER1(13); + TRANSFER1(14); + TRANSFER1(15); + TRANSFER1(16); +} + void AVRSPI2DeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, uint16_t len) { _cs_assert(); @@ -77,6 +105,12 @@ void AVRSPI2DeviceDriver::transaction(const uint8_t *tx, uint8_t *rx, _transfer(tx[i]); } } else { + while (len >= 17) { + _transfer17(tx, rx); + tx += 17; + rx += 17; + len -= 17; + } for (uint16_t i = 0; i < len; i++) { rx[i] = _transfer(tx[i]); } diff --git a/libraries/AP_HAL_AVR/SPIDevices.h b/libraries/AP_HAL_AVR/SPIDevices.h index 800b0f4231..4429c72fdd 100644 --- a/libraries/AP_HAL_AVR/SPIDevices.h +++ b/libraries/AP_HAL_AVR/SPIDevices.h @@ -31,6 +31,8 @@ private: void _cs_assert(); void _cs_release(); uint8_t _transfer(uint8_t data); + // used for MPU6k + void _transfer15(const uint8_t *tx, uint8_t *rx); static AP_HAL_AVR::AVRSemaphore _semaphore; @@ -66,6 +68,8 @@ private: void _cs_assert(); void _cs_release(); uint8_t _transfer(uint8_t data); + // used for APM1 ADC + void _transfer17(const uint8_t *tx, uint8_t *rx); static AP_HAL_AVR::AVRSemaphore _semaphore;