From c898b28962868a954fd82169ca98b0d46d9ecc35 Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Wed, 5 Oct 2016 12:56:34 -0300 Subject: [PATCH] AP_HAL_Linux: use MIN where possible --- libraries/AP_HAL_Linux/RPIOUARTDriver.cpp | 29 ++++++----------------- libraries/AP_HAL_Linux/SPIUARTDriver.cpp | 5 ++-- 2 files changed, 9 insertions(+), 25 deletions(-) diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp index 85a2e46271..bb2336f9af 100644 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp @@ -4,6 +4,7 @@ #include #include +#include #include "px4io_protocol.h" @@ -162,19 +163,11 @@ void RPIOUARTDriver::_timer_tick(void) struct IOPacket _dma_packet_tx = {0}, _dma_packet_rx = {0}; /* get write_buf bytes */ - uint32_t n = _writebuf.available(); - - if (n > PKT_MAX_REGS * 2) { - n = PKT_MAX_REGS * 2; - } - - uint16_t _max_size = _baudrate / 10 / (1000000 / RPIOUART_POLL_TIME_INTERVAL); - if (n > _max_size) { - n = _max_size; - } + uint32_t max_size = MIN(PKT_MAX_REGS * 2, + _baudrate / 10 / (1000000 / RPIOUART_POLL_TIME_INTERVAL)); + uint32_t n = MIN(_writebuf.available(), max_size); _writebuf.read(&((uint8_t *)_dma_packet_tx.regs)[0], n); - _dma_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART; _dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER; _dma_packet_tx.offset = n; @@ -203,18 +196,10 @@ void RPIOUARTDriver::_timer_tick(void) /* release sem */ _dev->get_semaphore()->give(); - /* add bytes to read buf */ - n = _readbuf.space(); - if (_dma_packet_rx.page == PX4IO_PAGE_UART_BUFFER) { - - if (n > _dma_packet_rx.offset) { - n = _dma_packet_rx.offset; - } - - if (n > PKT_MAX_REGS * 2) { - n = PKT_MAX_REGS * 2; - } + /* add bytes to read buf */ + max_size = MIN(_dma_packet_rx.offset, PKT_MAX_REGS * 2); + n = MIN(_readbuf.space(), max_size); _readbuf.write(&((uint8_t *)_dma_packet_rx.regs)[0], n); } diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index 69d30a8d3e..310cc64d52 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -5,6 +5,7 @@ #include #include +#include extern const AP_HAL::HAL &hal; @@ -123,9 +124,7 @@ int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n) * doesn't like to be waiting. Making transactions more frequent but shorter * is a win. */ - if (n > 100) { - n = 100; - } + n = MIN(n, 100); if (!_dev->get_semaphore()->take_nonblocking()) { return 0;