diff --git a/conf/modules/actuators_sts3032.xml b/conf/modules/actuators_sts3032.xml index 2557e0441d..78cff1a6f5 100644 --- a/conf/modules/actuators_sts3032.xml +++ b/conf/modules/actuators_sts3032.xml @@ -43,7 +43,7 @@ - + diff --git a/conf/modules/uavcan.xml b/conf/modules/uavcan.xml index de19b90b50..5082c8bc96 100644 --- a/conf/modules/uavcan.xml +++ b/conf/modules/uavcan.xml @@ -38,6 +38,6 @@ - + diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c index f14dce126e..01f69a60a2 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.c +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.c @@ -123,12 +123,12 @@ static void uavcan_tx(void* p) pprz_mtx_lock(&iface->tx_fifo_mutex); while(true) { struct uavcan_msg_header_t header; - int ret = circular_buffer_get(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)); + int ret = framed_ring_buffer_get(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)); if(ret < 0) {break;} if(header.payload_len >= UAVCAN_MSG_MAX_SIZE) { chSysHalt("UAVCAN_MSG_MAX_SIZE too small"); } - ret = circular_buffer_get(&iface->_tx_fifo, msg_payload, UAVCAN_MSG_MAX_SIZE); + ret = framed_ring_buffer_get(&iface->_tx_fifo, msg_payload, UAVCAN_MSG_MAX_SIZE); if(ret < 0) {break;} pprz_mtx_lock(&iface->mutex); canardBroadcast(&iface->canard, @@ -215,7 +215,7 @@ static void uavcanInitIface(struct uavcan_iface_t *iface) pprz_mtx_init(&iface->tx_fifo_mutex); // Initialize tx fifo - circular_buffer_init(&iface->_tx_fifo, iface->_tx_fifo_buffer, UAVCAN_TX_FIFO_SIZE); + framed_ring_buffer_init(&iface->_tx_fifo, iface->_tx_fifo_buffer, UAVCAN_TX_FIFO_SIZE); // Initialize canard canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool), @@ -276,15 +276,15 @@ void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature .payload_len = payload_len }; - if(circular_buffer_put(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)) < 0) { + if(framed_ring_buffer_put(&iface->_tx_fifo, (uint8_t*)&header, sizeof(struct uavcan_msg_header_t)) < 0) { // fail to post header pprz_mtx_unlock(&iface->tx_fifo_mutex); return; } - if(circular_buffer_put(&iface->_tx_fifo, payload, payload_len) < 0) { + if(framed_ring_buffer_put(&iface->_tx_fifo, payload, payload_len) < 0) { // fail to post payload. Remove the header from the fifo - circular_buffer_drop(&iface->_tx_fifo); + framed_ring_buffer_drop_last(&iface->_tx_fifo); pprz_mtx_unlock(&iface->tx_fifo_mutex); return; } diff --git a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h index 17f8288fbe..38f00657ee 100644 --- a/sw/airborne/arch/chibios/modules/uavcan/uavcan.h +++ b/sw/airborne/arch/chibios/modules/uavcan/uavcan.h @@ -30,7 +30,7 @@ #include #include "mcu_periph/can.h" #include "modules/core/threads.h" -#include "utils/circular_buffer.h" +#include "utils/framed_ring_buffer.h" #ifndef UAVCAN_TX_FIFO_SIZE @@ -62,7 +62,7 @@ struct uavcan_iface_t { uint8_t canard_memory_pool[1024 * 2]; uint8_t _tx_fifo_buffer[UAVCAN_TX_FIFO_SIZE]; - struct circular_buffer _tx_fifo; + struct framed_ring_buffer _tx_fifo; pprz_mutex_t tx_fifo_mutex; uint8_t transfer_id; diff --git a/sw/airborne/modules/actuators/actuators_sts3032.c b/sw/airborne/modules/actuators/actuators_sts3032.c index a04a1dab54..85c0bd26ca 100644 --- a/sw/airborne/modules/actuators/actuators_sts3032.c +++ b/sw/airborne/modules/actuators/actuators_sts3032.c @@ -91,7 +91,7 @@ void actuators_sts3032_init(void) sts.rx_state = STS3032_RX_IDLE; sts.nb_bytes_expected = 1; sts.nb_failed_checksum = 0; - circular_buffer_init(&sts.msg_buf, cbuf, sizeof(cbuf)); + framed_ring_buffer_init(&sts.msg_buf, cbuf, sizeof(cbuf)); static const uint8_t tmp_ids[SERVOS_STS3032_NB] = STS3032_IDS; memcpy(sts.ids, tmp_ids, sizeof(tmp_ids)); @@ -135,7 +135,7 @@ static void sts3032_event(struct sts3032 *sts) uint32_t dt = now - sts->time_last_msg; if ((!sts->wait_reply && dt > STS3032_DELAY_MSG_MIN) || dt > STS3032_DELAY_MSG) { uint8_t buf[BUF_MAX_LENGHT]; - int size = circular_buffer_get(&sts->msg_buf, buf, BUF_MAX_LENGHT); + int size = framed_ring_buffer_get(&sts->msg_buf, buf, BUF_MAX_LENGHT); if (size > 0) { // first byte of the buffer indicate whether or not a replay is expected // the rest is the message itself @@ -313,7 +313,7 @@ static void write_buf(struct sts3032 *sts, uint8_t id, uint8_t *data, uint8_t le } buf[len + 6] = ~checksum; - circular_buffer_put(&sts->msg_buf, buf, len + 7); + framed_ring_buffer_put(&sts->msg_buf, buf, len + 7); } diff --git a/sw/airborne/modules/actuators/actuators_sts3032.h b/sw/airborne/modules/actuators/actuators_sts3032.h index a07e715dcc..844335c2a2 100644 --- a/sw/airborne/modules/actuators/actuators_sts3032.h +++ b/sw/airborne/modules/actuators/actuators_sts3032.h @@ -28,7 +28,7 @@ #include #include "mcu_periph/uart.h" -#include "utils/circular_buffer.h" +#include "utils/framed_ring_buffer.h" #include "generated/airframe.h" @@ -59,7 +59,7 @@ struct sts3032 { bool wait_reply; uint16_t nb_failed_checksum; - struct circular_buffer msg_buf; + struct framed_ring_buffer msg_buf; }; diff --git a/sw/airborne/utils/circular_buffer.c b/sw/airborne/utils/framed_ring_buffer.c similarity index 85% rename from sw/airborne/utils/circular_buffer.c rename to sw/airborne/utils/framed_ring_buffer.c index b384456212..a1995505b8 100644 --- a/sw/airborne/utils/circular_buffer.c +++ b/sw/airborne/utils/framed_ring_buffer.c @@ -6,11 +6,11 @@ * This file is part of paparazzi. See LICENCE file. */ -#include "circular_buffer.h" +#include "framed_ring_buffer.h" #include -void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t len) +void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len) { cb->_buf = buffer; cb->_buf_len = len; @@ -19,7 +19,7 @@ void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t le } -int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len) +int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len) { // buffer empty if (cb->read_offset == cb->write_offset) { return CIR_ERROR_NO_MSG; } @@ -49,9 +49,9 @@ int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len) return nb_bytes; } -int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t len) +int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len) { - size_t available = circular_buffer_available(cb); + size_t available = framed_ring_buffer_available(cb); if (len > available) { return CIR_ERROR_NO_SPACE_AVAILABLE; @@ -87,7 +87,7 @@ int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t l return 0; } -int circular_buffer_drop(struct circular_buffer *cb) { +int framed_ring_buffer_drop_last(struct framed_ring_buffer *cb) { // buffer empty if (cb->read_offset == cb->write_offset) { return CIR_ERROR_NO_MSG; } @@ -107,7 +107,7 @@ int circular_buffer_drop(struct circular_buffer *cb) { return 0; } -size_t circular_buffer_available(struct circular_buffer *cb) { +size_t framed_ring_buffer_available(struct framed_ring_buffer *cb) { // write_offset == read_offset is considered an empty buffer. => 3= 2 (lenght) + 1 (margin) int available = 0; if (cb->read_offset > cb->write_offset) { @@ -119,7 +119,7 @@ size_t circular_buffer_available(struct circular_buffer *cb) { return available > 0 ? (size_t)available : 0; } -void circular_buffer_clear(struct circular_buffer *cb) { +void framed_ring_buffer_clear(struct framed_ring_buffer *cb) { cb->read_offset = 0; cb->write_offset = 0; } diff --git a/sw/airborne/utils/circular_buffer.h b/sw/airborne/utils/framed_ring_buffer.h similarity index 66% rename from sw/airborne/utils/circular_buffer.h rename to sw/airborne/utils/framed_ring_buffer.h index 370281121f..0f9d68ba52 100644 --- a/sw/airborne/utils/circular_buffer.h +++ b/sw/airborne/utils/framed_ring_buffer.h @@ -15,12 +15,12 @@ * This is a general purpose circular buffer for storing variable lenght buffers in a FIFO order. * Buffers length are stored as uint16_t. * - * Declare a \ref circular_buffer and allocate a buffer that will outlive it. - * Initialize the \ref circular_buffer using \ref circular_buffer_init. + * Declare a \ref framed_ring_buffer and allocate a buffer that will outlive it. + * Initialize the \ref framed_ring_buffer using \ref framed_ring_buffer_init. * */ -struct circular_buffer { +struct framed_ring_buffer { size_t read_offset; size_t write_offset; size_t _buf_len; @@ -36,11 +36,11 @@ enum cir_error { /** * @brief initialize a circular buffer. - * @param cb circular_buffer structure - * @param buffer buffer used internally by the circular buffer + * @param cb framed_ring_buffer structure + * @param buffer buffer used internally by the framed ring buffer * @param len size of \p buffer */ -void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t len); +void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len); /** @@ -50,7 +50,7 @@ void circular_buffer_init(struct circular_buffer *cb, uint8_t *buffer, size_t le * @param len size of \p buf * @return Size of the data copied to \p buf, or an error code if negative. */ -int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len); +int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len); /** * @brief Copy \p buf in the circular buffer @@ -59,20 +59,20 @@ int circular_buffer_get(struct circular_buffer *cb, uint8_t *buf, size_t len); * @param len Size of \p buf * @return 0 on success, Error code if negative */ -int circular_buffer_put(struct circular_buffer *cb, const uint8_t *buf, size_t len); +int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len); /** * @brief Drop last inserted record */ -int circular_buffer_drop(struct circular_buffer *cb); +int framed_ring_buffer_drop_last(struct framed_ring_buffer *cb); /** * @brief Get the available sapce for the next buffer */ -size_t circular_buffer_available(struct circular_buffer *cb); +size_t framed_ring_buffer_available(struct framed_ring_buffer *cb); /** * @brief Clear buffer */ -void circular_buffer_clear(struct circular_buffer *cb); \ No newline at end of file +void framed_ring_buffer_clear(struct framed_ring_buffer *cb); \ No newline at end of file diff --git a/sw/airborne/utils/ring_buffer.c b/sw/airborne/utils/ring_buffer.c new file mode 100644 index 0000000000..f080a10d14 --- /dev/null +++ b/sw/airborne/utils/ring_buffer.c @@ -0,0 +1,81 @@ +/* + * General purpose circular buffer + * + * Copyright (C) 2021 Fabien-B + * + * This file is part of paparazzi. See LICENCE file. + */ + +#include "utils/ring_buffer.h" +#include + + +void ring_buffer_init(ring_buffer_t *rb, uint8_t *buf, size_t size) { + rb->read_offset = 0; + rb->write_offset = 0; + rb->size = size; + rb->buf = buf; +} + +size_t ring_buffer_available(ring_buffer_t *rb) { + if (rb->write_offset >= rb->read_offset) { + return rb->write_offset - rb->read_offset; + } + else { + return rb->size - (rb->read_offset - rb->write_offset); + } +} + +size_t ring_buffer_free_space(ring_buffer_t *ring_buffer) { + return ring_buffer->size - ring_buffer_available(ring_buffer); +} + +size_t ring_buffer_write(ring_buffer_t *rb, uint8_t *data, size_t len) { + size_t free_space = ring_buffer_free_space(rb); + if (len > free_space) { + len = free_space; + } + + size_t first_chunk = rb->size - rb->write_offset; + if (first_chunk > len) { + first_chunk = len; + } + + // Copy first chunk + memcpy(&rb->buf[rb->write_offset], data, first_chunk); + + // Copy second chunk if wrap needed + size_t remaining = len - first_chunk; + if (remaining > 0) { + memcpy(&rb->buf[0], data + first_chunk, remaining); + } + + rb->write_offset = (rb->write_offset + len) % rb->size; + + return len; +} + +size_t ring_buffer_read(ring_buffer_t *rb, uint8_t *read_buffer, size_t len) +{ + size_t available = ring_buffer_available(rb); + if (len > available) { + len = available; + } + + // how much until wrap? + size_t first_chunk = rb->size - rb->read_offset; + if (first_chunk > len) { + first_chunk = len; + } + + memcpy(read_buffer, &rb->buf[rb->read_offset], first_chunk); + + size_t remaining = len - first_chunk; + if (remaining > 0) { + memcpy(read_buffer + first_chunk, &rb->buf[0], remaining); + } + + rb->read_offset = (rb->read_offset + len) % rb->size; + + return len; +} diff --git a/sw/airborne/utils/ring_buffer.h b/sw/airborne/utils/ring_buffer.h new file mode 100644 index 0000000000..e75a6e70f3 --- /dev/null +++ b/sw/airborne/utils/ring_buffer.h @@ -0,0 +1,48 @@ +/* + * General purpose circular buffer + * + * Copyright (C) 2021 Fabien-B + * + * This file is part of paparazzi. See LICENCE file. + */ + +#pragma once +#include +#include + + +typedef struct { + size_t read_offset; + size_t write_offset; + size_t size; + uint8_t *buf; +} ring_buffer_t; + + +/** + * Init @param ring_buffer with the @param buf subjacent buffer of size @param size. + */ +void ring_buffer_init (ring_buffer_t *ring_buffer, uint8_t *buf, size_t size); + +/** + * Write @param data of size @param len in @param ring_buffer. + * @returns the number of byte effectively written in the ring_buffer. + */ +size_t ring_buffer_write(ring_buffer_t *ring_buffer, uint8_t* data, size_t len); + +/** + * Read @param len bytes from @param ring_buffer into @param read_buffer. + * @returns the number of byte effectively read from the ring_buffer. + */ +size_t ring_buffer_read(ring_buffer_t *ring_buffer, uint8_t* read_buffer, size_t len); + +/** + * @returns the number of bytes available in the @param ring_buffer. + */ +size_t ring_buffer_available(ring_buffer_t *ring_buffer); + +/** + * @returns the number of bytes that can be written in @param ring_buffer + */ +size_t ring_buffer_free_space(ring_buffer_t *ring_buffer); + diff --git a/tests/utils/Makefile b/tests/utils/Makefile index eada3748d4..de7101ab48 100644 --- a/tests/utils/Makefile +++ b/tests/utils/Makefile @@ -33,7 +33,7 @@ export PAPARAZZI_HOME ##################################################### # If you add more test files you add their names here -TESTS = test_circular_buffer.run +TESTS = test_framed_ring_buffer.run ################################################### # You should not need to touch the rest of the file @@ -52,7 +52,7 @@ build_tests: $(TESTS) test: build_tests prove $(VERBOSE) --exec '' ./*.run -test_circular_buffer.run: $(PAPARAZZI_SRC)/sw/airborne/utils/circular_buffer.c +test_framed_ring_buffer.run: $(PAPARAZZI_SRC)/sw/airborne/utils/framed_ring_buffer.c %.run: %.c @echo BUILD $@ diff --git a/tests/utils/test_circular_buffer.c b/tests/utils/test_framed_ring_buffer.c similarity index 69% rename from tests/utils/test_circular_buffer.c rename to tests/utils/test_framed_ring_buffer.c index 1250a0f3c7..1a3415272c 100644 --- a/tests/utils/test_circular_buffer.c +++ b/tests/utils/test_framed_ring_buffer.c @@ -6,12 +6,12 @@ #include "stdio.h" #include "stdlib.h" -#include "circular_buffer.h" +#include "framed_ring_buffer.h" #include #include "tap.h" uint8_t buffer[20]; -struct circular_buffer cbuf; +struct framed_ring_buffer cbuf; int main(int argc __attribute_maybe_unused__, char **argv __attribute_maybe_unused__) @@ -25,85 +25,85 @@ int main(int argc __attribute_maybe_unused__, char **argv __attribute_maybe_unus note("running circular buffer tests"); plan(23); - circular_buffer_init(&cbuf, buffer, sizeof(buffer)); + framed_ring_buffer_init(&cbuf, buffer, sizeof(buffer)); // get on an empty buffer - int ret = circular_buffer_get(&cbuf, dst, sizeof(dst)); + int ret = framed_ring_buffer_get(&cbuf, dst, sizeof(dst)); ok(ret == CIR_ERROR_NO_MSG, "expected CIR_ERROR_NO_MSG, got %d", ret); // put 10 bytes. 12 bytes will be occupied, 8 bytes free, 5 available for the next buffer - ret = circular_buffer_put(&cbuf, src, 10); - size_t available = circular_buffer_available(&cbuf); + ret = framed_ring_buffer_put(&cbuf, src, 10); + size_t available = framed_ring_buffer_available(&cbuf); ok(ret == 0, "expected 0, got %d\n", ret); ok(available == 5, "expected 5 bytes available, got %lu", available); // put 3 bytes. 12+5=17/20 bytes occupied, 3 left. 0 available for the next buffer - ret = circular_buffer_put(&cbuf, src, 3); - available = circular_buffer_available(&cbuf); + ret = framed_ring_buffer_put(&cbuf, src, 3); + available = framed_ring_buffer_available(&cbuf); ok(ret == 0, "expected 0, got %d\n", ret); ok(available == 0, "expected 0 bytes available, got %lu", available); // try puting 1 byte : should fail - ret = circular_buffer_put(&cbuf, src, 1); + ret = framed_ring_buffer_put(&cbuf, src, 1); ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret); // drop last buffer - circular_buffer_drop(&cbuf); + framed_ring_buffer_drop_last(&cbuf); // put 4 bytes. 12+6=18/20 bytes occupied, 2 left. 0 available for the next buffer - ret = circular_buffer_put(&cbuf, src, 4); - available = circular_buffer_available(&cbuf); + ret = framed_ring_buffer_put(&cbuf, src, 4); + available = framed_ring_buffer_available(&cbuf); ok(ret == 0, "expected 0, got %d\n", ret); ok(available == 0, "expected 0 bytes available, got %lu", available); // try puting 1 byte : should fail - ret = circular_buffer_put(&cbuf, src, 1); + ret = framed_ring_buffer_put(&cbuf, src, 1); ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret); // drop last buffer - circular_buffer_drop(&cbuf); + framed_ring_buffer_drop_last(&cbuf); // put 5 bytes. 12+7=19/20 bytes occupied, 0 left. 0 available for the next buffer - ret = circular_buffer_put(&cbuf, src, 5); - available = circular_buffer_available(&cbuf); + ret = framed_ring_buffer_put(&cbuf, src, 5); + available = framed_ring_buffer_available(&cbuf); ok(ret == 0, "expected 0, got %d\n", ret); ok(available == 0, "expected 0 bytes available, got %lu", available); // try puting 1 byte : should fail - ret = circular_buffer_put(&cbuf, src, 1); + ret = framed_ring_buffer_put(&cbuf, src, 1); ok(ret == CIR_ERROR_NO_SPACE_AVAILABLE, "expected CIR_ERROR_NO_SPACE_AVAILABLE, got %d\n", ret); // get first buffer: 10 bytes - ret = circular_buffer_get(&cbuf, dst, sizeof(dst)); + ret = framed_ring_buffer_get(&cbuf, dst, sizeof(dst)); ok(ret == 10, "expected %ld, got %d", 10, ret); ok(memcmp(dst, src, ret) == 0, "buffer corrupted"); - available = circular_buffer_available(&cbuf); + available = framed_ring_buffer_available(&cbuf); ok(available == 10, "expected 10 bytes available, got %lu", available); // put 10 bytes. len should wrap around the buffer. len: 1 byte at the end, 1 at the start, - ret = circular_buffer_put(&cbuf, src, 10); + ret = framed_ring_buffer_put(&cbuf, src, 10); ok(ret == 0, "expected 0, got %d\n", ret); // get 5 bytes buffer - ret = circular_buffer_get(&cbuf, dst, sizeof(dst)); + ret = framed_ring_buffer_get(&cbuf, dst, sizeof(dst)); ok(ret == 5, "expected %ld, got %d", 5, ret); ok(memcmp(dst, src, ret) == 0, "buffer corrupted"); // get 10 bytes buffer - ret = circular_buffer_get(&cbuf, dst, sizeof(dst)); + ret = framed_ring_buffer_get(&cbuf, dst, sizeof(dst)); ok(ret == 10, "expected %ld, got %d", 10, ret); ok(memcmp(dst, src, ret) == 0, "buffer corrupted"); // put 12 bytes buffer, data will wrap around // write_offset at 11, 2lenght+7data + 5data wrapped - ret = circular_buffer_put(&cbuf, src, 12); + ret = framed_ring_buffer_put(&cbuf, src, 12); ok(ret == 0, "expected 0, got %d\n", ret); // get 12 bytes buffer - ret = circular_buffer_get(&cbuf, dst, sizeof(dst)); + ret = framed_ring_buffer_get(&cbuf, dst, sizeof(dst)); ok(ret == 12, "expected %ld, got %d", 12, ret); ok(memcmp(dst, src, ret) == 0, "buffer corrupted");