S Morita / mbed-mros2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

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

embeddedRTPS/src/storages/SimpleHistoryCache.cpp@0:580aba13d1a1, 2021-12-30 (annotated)

Committer:
smoritaemb
Date:
Thu Dec 30 21:06:29 2021 +0900
Revision:
0:580aba13d1a1
Updated to catch up to mros2 v2.3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
smoritaemb 0:580aba13d1a1 1 /*
smoritaemb 0:580aba13d1a1 2 The MIT License
smoritaemb 0:580aba13d1a1 3 Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
smoritaemb 0:580aba13d1a1 4 Permission is hereby granted, free of charge, to any person obtaining a copy
smoritaemb 0:580aba13d1a1 5 of this software and associated documentation files (the "Software"), to deal
smoritaemb 0:580aba13d1a1 6 in the Software without restriction, including without limitation the rights
smoritaemb 0:580aba13d1a1 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
smoritaemb 0:580aba13d1a1 8 copies of the Software, and to permit persons to whom the Software is
smoritaemb 0:580aba13d1a1 9 furnished to do so, subject to the following conditions:
smoritaemb 0:580aba13d1a1 10 The above copyright notice and this permission notice shall be included in
smoritaemb 0:580aba13d1a1 11 all copies or substantial portions of the Software.
smoritaemb 0:580aba13d1a1 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
smoritaemb 0:580aba13d1a1 13 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
smoritaemb 0:580aba13d1a1 14 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
smoritaemb 0:580aba13d1a1 15 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
smoritaemb 0:580aba13d1a1 16 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
smoritaemb 0:580aba13d1a1 17 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
smoritaemb 0:580aba13d1a1 18 THE SOFTWARE
smoritaemb 0:580aba13d1a1 19
smoritaemb 0:580aba13d1a1 20 This file is part of embeddedRTPS.
smoritaemb 0:580aba13d1a1 21
smoritaemb 0:580aba13d1a1 22 Author: i11 - Embedded Software, RWTH Aachen University
smoritaemb 0:580aba13d1a1 23 */
smoritaemb 0:580aba13d1a1 24
smoritaemb 0:580aba13d1a1 25 #include "rtps/storages/SimpleHistoryCache.h"
smoritaemb 0:580aba13d1a1 26 #include <limits>
smoritaemb 0:580aba13d1a1 27
smoritaemb 0:580aba13d1a1 28 using rtps::SimpleHistoryCache;
smoritaemb 0:580aba13d1a1 29
smoritaemb 0:580aba13d1a1 30 SimpleHistoryCache::SimpleHistoryCache(SequenceNumber_t lastUsed)
smoritaemb 0:580aba13d1a1 31 : SimpleHistoryCache() {
smoritaemb 0:580aba13d1a1 32 m_lastUsedSequenceNumber = lastUsed;
smoritaemb 0:580aba13d1a1 33 }
smoritaemb 0:580aba13d1a1 34
smoritaemb 0:580aba13d1a1 35 bool SimpleHistoryCache::isFull() const {
smoritaemb 0:580aba13d1a1 36 uint16_t it = m_head;
smoritaemb 0:580aba13d1a1 37 incrementIterator(it);
smoritaemb 0:580aba13d1a1 38 return it == m_tail;
smoritaemb 0:580aba13d1a1 39 }
smoritaemb 0:580aba13d1a1 40
smoritaemb 0:580aba13d1a1 41 const rtps::SequenceNumber_t &SimpleHistoryCache::getSeqNumMin() const {
smoritaemb 0:580aba13d1a1 42 if (m_head == m_tail) {
smoritaemb 0:580aba13d1a1 43 return SEQUENCENUMBER_UNKNOWN;
smoritaemb 0:580aba13d1a1 44 } else {
smoritaemb 0:580aba13d1a1 45 return m_buffer[m_tail].sequenceNumber;
smoritaemb 0:580aba13d1a1 46 }
smoritaemb 0:580aba13d1a1 47 }
smoritaemb 0:580aba13d1a1 48
smoritaemb 0:580aba13d1a1 49 const rtps::SequenceNumber_t &SimpleHistoryCache::getSeqNumMax() const {
smoritaemb 0:580aba13d1a1 50 if (m_head == m_tail) {
smoritaemb 0:580aba13d1a1 51 return SEQUENCENUMBER_UNKNOWN;
smoritaemb 0:580aba13d1a1 52 } else {
smoritaemb 0:580aba13d1a1 53 return m_lastUsedSequenceNumber;
smoritaemb 0:580aba13d1a1 54 }
smoritaemb 0:580aba13d1a1 55 }
smoritaemb 0:580aba13d1a1 56
smoritaemb 0:580aba13d1a1 57 const rtps::CacheChange *SimpleHistoryCache::addChange(const uint8_t *data,
smoritaemb 0:580aba13d1a1 58 DataSize_t size) {
smoritaemb 0:580aba13d1a1 59 CacheChange change;
smoritaemb 0:580aba13d1a1 60 change.kind = ChangeKind_t::ALIVE;
smoritaemb 0:580aba13d1a1 61 change.data.reserve(size);
smoritaemb 0:580aba13d1a1 62 change.data.append(data, size);
smoritaemb 0:580aba13d1a1 63 change.sequenceNumber = ++m_lastUsedSequenceNumber;
smoritaemb 0:580aba13d1a1 64
smoritaemb 0:580aba13d1a1 65 CacheChange *place = &m_buffer[m_head];
smoritaemb 0:580aba13d1a1 66 incrementHead();
smoritaemb 0:580aba13d1a1 67
smoritaemb 0:580aba13d1a1 68 *place = std::move(change);
smoritaemb 0:580aba13d1a1 69 return place;
smoritaemb 0:580aba13d1a1 70 }
smoritaemb 0:580aba13d1a1 71
smoritaemb 0:580aba13d1a1 72 void SimpleHistoryCache::dropOldest() { removeUntilIncl(getSeqNumMin()); }
smoritaemb 0:580aba13d1a1 73
smoritaemb 0:580aba13d1a1 74 void SimpleHistoryCache::removeUntilIncl(SequenceNumber_t sn) {
smoritaemb 0:580aba13d1a1 75 if (m_head == m_tail) {
smoritaemb 0:580aba13d1a1 76 return;
smoritaemb 0:580aba13d1a1 77 }
smoritaemb 0:580aba13d1a1 78
smoritaemb 0:580aba13d1a1 79 if (getSeqNumMax() <= sn) { // We won't overrun head
smoritaemb 0:580aba13d1a1 80 m_head = m_tail;
smoritaemb 0:580aba13d1a1 81 return;
smoritaemb 0:580aba13d1a1 82 }
smoritaemb 0:580aba13d1a1 83
smoritaemb 0:580aba13d1a1 84 while (m_buffer[m_tail].sequenceNumber <= sn) {
smoritaemb 0:580aba13d1a1 85 incrementTail();
smoritaemb 0:580aba13d1a1 86 }
smoritaemb 0:580aba13d1a1 87 }
smoritaemb 0:580aba13d1a1 88
smoritaemb 0:580aba13d1a1 89 const rtps::CacheChange *
smoritaemb 0:580aba13d1a1 90 SimpleHistoryCache::getChangeBySN(SequenceNumber_t sn) const {
smoritaemb 0:580aba13d1a1 91 SequenceNumber_t minSN = getSeqNumMin();
smoritaemb 0:580aba13d1a1 92 if (sn < minSN || getSeqNumMax() < sn) {
smoritaemb 0:580aba13d1a1 93 return nullptr;
smoritaemb 0:580aba13d1a1 94 }
smoritaemb 0:580aba13d1a1 95 static_assert(std::is_unsigned<decltype(sn.low)>::value,
smoritaemb 0:580aba13d1a1 96 "Underflow well defined");
smoritaemb 0:580aba13d1a1 97 static_assert(sizeof(m_tail) <= sizeof(uint16_t), "Cast ist well defined");
smoritaemb 0:580aba13d1a1 98 // We don't overtake head, therefore difference of sn is within same range as
smoritaemb 0:580aba13d1a1 99 // iterators
smoritaemb 0:580aba13d1a1 100 uint16_t pos = m_tail + static_cast<uint16_t>(sn.low - minSN.low);
smoritaemb 0:580aba13d1a1 101
smoritaemb 0:580aba13d1a1 102 // Diff is smaller than the size of the array -> max one overflow
smoritaemb 0:580aba13d1a1 103 if (pos >= m_buffer.size()) {
smoritaemb 0:580aba13d1a1 104 pos -= m_buffer.size();
smoritaemb 0:580aba13d1a1 105 }
smoritaemb 0:580aba13d1a1 106 return &m_buffer[pos];
smoritaemb 0:580aba13d1a1 107 }
smoritaemb 0:580aba13d1a1 108
smoritaemb 0:580aba13d1a1 109 void SimpleHistoryCache::incrementHead() {
smoritaemb 0:580aba13d1a1 110 incrementIterator(m_head);
smoritaemb 0:580aba13d1a1 111 if (m_head == m_tail) {
smoritaemb 0:580aba13d1a1 112 // Move without check
smoritaemb 0:580aba13d1a1 113 incrementIterator(m_tail); // drop one
smoritaemb 0:580aba13d1a1 114 }
smoritaemb 0:580aba13d1a1 115 }
smoritaemb 0:580aba13d1a1 116
smoritaemb 0:580aba13d1a1 117 void SimpleHistoryCache::incrementTail() {
smoritaemb 0:580aba13d1a1 118 if (m_head != m_tail) {
smoritaemb 0:580aba13d1a1 119 incrementIterator(m_tail);
smoritaemb 0:580aba13d1a1 120 }
smoritaemb 0:580aba13d1a1 121 }
smoritaemb 0:580aba13d1a1 122
smoritaemb 0:580aba13d1a1 123 void SimpleHistoryCache::incrementIterator(uint16_t &iterator) const {
smoritaemb 0:580aba13d1a1 124 ++iterator;
smoritaemb 0:580aba13d1a1 125 if (iterator >= m_buffer.size()) {
smoritaemb 0:580aba13d1a1 126 iterator = 0;
smoritaemb 0:580aba13d1a1 127 }
smoritaemb 0:580aba13d1a1 128 }