S Morita / mbed-mros2

Porting mros2 as an Mbed library.

Dependents:   mbed-os-example-mros2 example-mbed-mros2-sub-pose example-mbed-mros2-pub-twist example-mbed-mros2-mturtle-teleop

embeddedRTPS/include/rtps/storages/ThreadSafeCircularBuffer.tpp@7:c80f65422d99, 2022-03-19 (annotated)

Committer:
smoritaemb
Date:
Sat Mar 19 09:23:37 2022 +0900
Revision:
7:c80f65422d99
Parent:
0:580aba13d1a1 
Merge test_assortment_of_msgs branch.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1
smoritaemb 0:580aba13d1a1 2 #ifndef RTPS_THREADSAFECIRCULARBUFFER_TPP
smoritaemb 0:580aba13d1a1 3 #define RTPS_THREADSAFECIRCULARBUFFER_TPP
smoritaemb 0:580aba13d1a1 4
smoritaemb 0:580aba13d1a1 5 #include "rtps/utils/Lock.h"
smoritaemb 0:580aba13d1a1 6
smoritaemb 0:580aba13d1a1 7 #define TSCB_VERBOSE 0
smoritaemb 0:580aba13d1a1 8 namespace rtps {
smoritaemb 0:580aba13d1a1 9
smoritaemb 0:580aba13d1a1 10 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 11 bool ThreadSafeCircularBuffer<T, SIZE>::init() {
smoritaemb 0:580aba13d1a1 12 if (m_initialized) {
smoritaemb 0:580aba13d1a1 13 return true;
smoritaemb 0:580aba13d1a1 14 }
smoritaemb 0:580aba13d1a1 15 if (sys_mutex_new(&m_mutex) != ERR_OK) {
smoritaemb 0:580aba13d1a1 16 #if TSCB_VERBOSE
smoritaemb 0:580aba13d1a1 17 printf("Failed to create mutex \n");
smoritaemb 0:580aba13d1a1 18 #endif
smoritaemb 0:580aba13d1a1 19 return false;
smoritaemb 0:580aba13d1a1 20 } else {
smoritaemb 0:580aba13d1a1 21 #if TSCB_VERBOSE
smoritaemb 0:580aba13d1a1 22 printf("Successfully created mutex at %p\n", static_cast<void *>(&m_mutex));
smoritaemb 0:580aba13d1a1 23 #endif
smoritaemb 0:580aba13d1a1 24 m_initialized = true;
smoritaemb 0:580aba13d1a1 25 return true;
smoritaemb 0:580aba13d1a1 26 }
smoritaemb 0:580aba13d1a1 27 }
smoritaemb 0:580aba13d1a1 28
smoritaemb 0:580aba13d1a1 29 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 30 ThreadSafeCircularBuffer<T, SIZE>::~ThreadSafeCircularBuffer() {
smoritaemb 0:580aba13d1a1 31 if (m_initialized) {
smoritaemb 0:580aba13d1a1 32 sys_mutex_free(&m_mutex);
smoritaemb 0:580aba13d1a1 33 }
smoritaemb 0:580aba13d1a1 34 }
smoritaemb 0:580aba13d1a1 35
smoritaemb 0:580aba13d1a1 36 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 37 bool ThreadSafeCircularBuffer<T, SIZE>::moveElementIntoBuffer(T &&elem) {
smoritaemb 0:580aba13d1a1 38 Lock lock(m_mutex);
smoritaemb 0:580aba13d1a1 39 if (!isFull()) {
smoritaemb 0:580aba13d1a1 40 m_buffer[m_head] = std::move(elem);
smoritaemb 0:580aba13d1a1 41 incrementHead();
smoritaemb 0:580aba13d1a1 42 return true;
smoritaemb 0:580aba13d1a1 43 } else {
smoritaemb 0:580aba13d1a1 44 return false;
smoritaemb 0:580aba13d1a1 45 }
smoritaemb 0:580aba13d1a1 46 }
smoritaemb 0:580aba13d1a1 47
smoritaemb 0:580aba13d1a1 48 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 49 bool ThreadSafeCircularBuffer<T, SIZE>::moveFirstInto(T &hull) {
smoritaemb 0:580aba13d1a1 50 Lock lock(m_mutex);
smoritaemb 0:580aba13d1a1 51 if (m_head != m_tail) {
smoritaemb 0:580aba13d1a1 52 hull = std::move(m_buffer[m_tail]);
smoritaemb 0:580aba13d1a1 53 incrementTail();
smoritaemb 0:580aba13d1a1 54 return true;
smoritaemb 0:580aba13d1a1 55 } else {
smoritaemb 0:580aba13d1a1 56 return false;
smoritaemb 0:580aba13d1a1 57 }
smoritaemb 0:580aba13d1a1 58 }
smoritaemb 0:580aba13d1a1 59
smoritaemb 0:580aba13d1a1 60 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 61 void ThreadSafeCircularBuffer<T, SIZE>::clear() {
smoritaemb 0:580aba13d1a1 62 Lock lock(m_mutex);
smoritaemb 0:580aba13d1a1 63 m_head = m_tail;
smoritaemb 0:580aba13d1a1 64 }
smoritaemb 0:580aba13d1a1 65
smoritaemb 0:580aba13d1a1 66 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 67 bool ThreadSafeCircularBuffer<T, SIZE>::isFull() {
smoritaemb 0:580aba13d1a1 68 auto it = m_head;
smoritaemb 0:580aba13d1a1 69 incrementIterator(it);
smoritaemb 0:580aba13d1a1 70 return it == m_tail;
smoritaemb 0:580aba13d1a1 71 }
smoritaemb 0:580aba13d1a1 72
smoritaemb 0:580aba13d1a1 73 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 74 inline void
smoritaemb 0:580aba13d1a1 75 ThreadSafeCircularBuffer<T, SIZE>::incrementIterator(uint16_t &iterator) {
smoritaemb 0:580aba13d1a1 76 ++iterator;
smoritaemb 0:580aba13d1a1 77 if (iterator >= m_buffer.size()) {
smoritaemb 0:580aba13d1a1 78 iterator = 0;
smoritaemb 0:580aba13d1a1 79 }
smoritaemb 0:580aba13d1a1 80 }
smoritaemb 0:580aba13d1a1 81
smoritaemb 0:580aba13d1a1 82 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 83 inline void ThreadSafeCircularBuffer<T, SIZE>::incrementTail() {
smoritaemb 0:580aba13d1a1 84 incrementIterator(m_tail);
smoritaemb 0:580aba13d1a1 85 }
smoritaemb 0:580aba13d1a1 86
smoritaemb 0:580aba13d1a1 87 template <typename T, uint16_t SIZE>
smoritaemb 0:580aba13d1a1 88 inline void ThreadSafeCircularBuffer<T, SIZE>::incrementHead() {
smoritaemb 0:580aba13d1a1 89 incrementIterator(m_head);
smoritaemb 0:580aba13d1a1 90 if (m_head == m_tail) {
smoritaemb 0:580aba13d1a1 91 incrementTail();
smoritaemb 0:580aba13d1a1 92 }
smoritaemb 0:580aba13d1a1 93 }
smoritaemb 0:580aba13d1a1 94 } // namespace rtps
smoritaemb 0:580aba13d1a1 95
smoritaemb 0:580aba13d1a1 96 #endif // RTPS_THREADSAFECIRCULARBUFFER_TPP