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/utils/udpUtils.h@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 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 #ifndef RTPS_UDP_UTILS_H
smoritaemb 0:580aba13d1a1 26 #define RTPS_UDP_UTILS_H
smoritaemb 0:580aba13d1a1 27
smoritaemb 0:580aba13d1a1 28 #include "rtps/config.h"
smoritaemb 0:580aba13d1a1 29
smoritaemb 0:580aba13d1a1 30 namespace rtps {
smoritaemb 0:580aba13d1a1 31 namespace {
smoritaemb 0:580aba13d1a1 32 const uint16_t PB = 7400; // Port Base Number
smoritaemb 0:580aba13d1a1 33 const uint16_t DG = 250; // DomainId Gain
smoritaemb 0:580aba13d1a1 34 const uint16_t PG = 2; // ParticipantId Gain
smoritaemb 0:580aba13d1a1 35 // Additional Offsets
smoritaemb 0:580aba13d1a1 36 const uint16_t D0 = 0; // Builtin multicast
smoritaemb 0:580aba13d1a1 37 const uint16_t D1 = 10; // Builtin unicast
smoritaemb 0:580aba13d1a1 38 const uint16_t D2 = 1; // User multicast
smoritaemb 0:580aba13d1a1 39 const uint16_t D3 = 11; // User unicast
smoritaemb 0:580aba13d1a1 40 } // namespace
smoritaemb 0:580aba13d1a1 41
smoritaemb 0:580aba13d1a1 42 constexpr ip4_addr transformIP4ToU32(uint8_t MSB, uint8_t p2, uint8_t p1,
smoritaemb 0:580aba13d1a1 43 uint8_t LSB) {
smoritaemb 0:580aba13d1a1 44 return {((uint32_t)(LSB << 24)) | ((uint32_t)(p1 << 16)) |
smoritaemb 0:580aba13d1a1 45 ((uint32_t)(p2 << 8)) | MSB};
smoritaemb 0:580aba13d1a1 46 }
smoritaemb 0:580aba13d1a1 47
smoritaemb 0:580aba13d1a1 48 constexpr Ip4Port_t getBuiltInUnicastPort(ParticipantId_t participantId) {
smoritaemb 0:580aba13d1a1 49 return PB + DG * Config::DOMAIN_ID + D1 + PG * participantId;
smoritaemb 0:580aba13d1a1 50 }
smoritaemb 0:580aba13d1a1 51
smoritaemb 0:580aba13d1a1 52 constexpr Ip4Port_t getBuiltInMulticastPort() {
smoritaemb 0:580aba13d1a1 53 return PB + DG * Config::DOMAIN_ID + D0;
smoritaemb 0:580aba13d1a1 54 }
smoritaemb 0:580aba13d1a1 55
smoritaemb 0:580aba13d1a1 56 constexpr Ip4Port_t getUserUnicastPort(ParticipantId_t participantId) {
smoritaemb 0:580aba13d1a1 57 return PB + DG * Config::DOMAIN_ID + D3 + PG * participantId;
smoritaemb 0:580aba13d1a1 58 }
smoritaemb 0:580aba13d1a1 59
smoritaemb 0:580aba13d1a1 60 constexpr Ip4Port_t getUserMulticastPort() {
smoritaemb 0:580aba13d1a1 61 return PB + DG * Config::DOMAIN_ID + D2;
smoritaemb 0:580aba13d1a1 62 }
smoritaemb 0:580aba13d1a1 63
smoritaemb 0:580aba13d1a1 64 constexpr bool isUserPort(Ip4Port_t port) { return (port & 1) == 1; }
smoritaemb 0:580aba13d1a1 65
smoritaemb 0:580aba13d1a1 66 inline bool isMultiCastPort(Ip4Port_t port) {
smoritaemb 0:580aba13d1a1 67 const auto idWithoutBase = port - PB - DG * Config::DOMAIN_ID;
smoritaemb 0:580aba13d1a1 68 return idWithoutBase == D0 || idWithoutBase == D2;
smoritaemb 0:580aba13d1a1 69 }
smoritaemb 0:580aba13d1a1 70
smoritaemb 0:580aba13d1a1 71 inline ParticipantId_t getParticipantIdFromUnicastPort(Ip4Port_t port,
smoritaemb 0:580aba13d1a1 72 bool isUserPort) {
smoritaemb 0:580aba13d1a1 73 // if(isMultiCastPort(port)){ // TODO remove?
smoritaemb 0:580aba13d1a1 74 // return PARTICIPANT_ID_INVALID;
smoritaemb 0:580aba13d1a1 75 //}
smoritaemb 0:580aba13d1a1 76
smoritaemb 0:580aba13d1a1 77 const auto basePart = PB + DG * Config::DOMAIN_ID;
smoritaemb 0:580aba13d1a1 78 ParticipantId_t participantPart = port - basePart;
smoritaemb 0:580aba13d1a1 79
smoritaemb 0:580aba13d1a1 80 uint16_t offset = 0;
smoritaemb 0:580aba13d1a1 81 if (isUserPort) {
smoritaemb 0:580aba13d1a1 82 offset = D3;
smoritaemb 0:580aba13d1a1 83 } else {
smoritaemb 0:580aba13d1a1 84 offset = D1;
smoritaemb 0:580aba13d1a1 85 }
smoritaemb 0:580aba13d1a1 86
smoritaemb 0:580aba13d1a1 87 participantPart -= offset;
smoritaemb 0:580aba13d1a1 88
smoritaemb 0:580aba13d1a1 89 auto id = static_cast<ParticipantId_t>(participantPart / PG);
smoritaemb 0:580aba13d1a1 90 if (id * PG + basePart + offset == port) {
smoritaemb 0:580aba13d1a1 91 return id;
smoritaemb 0:580aba13d1a1 92 } else {
smoritaemb 0:580aba13d1a1 93 return PARTICIPANT_ID_INVALID;
smoritaemb 0:580aba13d1a1 94 }
smoritaemb 0:580aba13d1a1 95 }
smoritaemb 0:580aba13d1a1 96
smoritaemb 0:580aba13d1a1 97 } // namespace rtps
smoritaemb 0:580aba13d1a1 98
smoritaemb 0:580aba13d1a1 99 #endif // RTPS_UDP_H