FIrs
This commit is contained in:
Martino Ferrari
@@ -0,0 +1 @@
|
||||
include Makefile.inc
|
||||
@@ -0,0 +1,38 @@
|
||||
#############################################################
|
||||
#
|
||||
# Copyright 2015 F4E | European Joint Undertaking for ITER
|
||||
# and the Development of Fusion Energy ('Fusion for Energy')
|
||||
#
|
||||
# Licensed under the EUPL, Version 1.1 or - as soon they
|
||||
# will be approved by the European Commission - subsequent
|
||||
# versions of the EUPL (the "Licence");
|
||||
# You may not use this work except in compliance with the
|
||||
# Licence.
|
||||
# You may obtain a copy of the Licence at:
|
||||
#
|
||||
# http://ec.europa.eu/idabc/eupl
|
||||
#
|
||||
# Unless required by applicable law or agreed to in
|
||||
# writing, software distributed under the Licence is
|
||||
# distributed on an "AS IS" basis,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
|
||||
# express or implied.
|
||||
# See the Licence for the specific language governing
|
||||
# permissions and limitations under the Licence.
|
||||
#
|
||||
#############################################################
|
||||
|
||||
OBJSX=
|
||||
|
||||
SPB = UDPStreamer.x
|
||||
|
||||
PACKAGE=Components
|
||||
ROOT_DIR=../../..
|
||||
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
all: $(OBJS) $(SUBPROJ)
|
||||
echo $(OBJS)
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
|
||||
@@ -0,0 +1,25 @@
|
||||
#############################################################
|
||||
#
|
||||
# Copyright 2015 F4E | European Joint Undertaking for ITER
|
||||
# and the Development of Fusion Energy ('Fusion for Energy')
|
||||
#
|
||||
# Licensed under the EUPL, Version 1.1 or - as soon they
|
||||
# will be approved by the European Commission - subsequent
|
||||
# versions of the EUPL (the "Licence");
|
||||
# You may not use this work except in compliance with the
|
||||
# Licence.
|
||||
# You may obtain a copy of the Licence at:
|
||||
#
|
||||
# http://ec.europa.eu/idabc/eupl
|
||||
#
|
||||
# Unless required by applicable law or agreed to in
|
||||
# writing, software distributed under the Licence is
|
||||
# distributed on an "AS IS" basis,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
|
||||
# express or implied.
|
||||
# See the Licence for the specific language governing
|
||||
# permissions and limitations under the Licence.
|
||||
#
|
||||
#############################################################
|
||||
|
||||
include Makefile.inc
|
||||
@@ -0,0 +1,56 @@
|
||||
#############################################################
|
||||
#
|
||||
# Copyright 2015 F4E | European Joint Undertaking for ITER
|
||||
# and the Development of Fusion Energy ('Fusion for Energy')
|
||||
#
|
||||
# Licensed under the EUPL, Version 1.1 or - as soon they
|
||||
# will be approved by the European Commission - subsequent
|
||||
# versions of the EUPL (the "Licence");
|
||||
# You may not use this work except in compliance with the
|
||||
# Licence.
|
||||
# You may obtain a copy of the Licence at:
|
||||
#
|
||||
# http://ec.europa.eu/idabc/eupl
|
||||
#
|
||||
# Unless required by applicable law or agreed to in
|
||||
# writing, software distributed under the Licence is
|
||||
# distributed on an "AS IS" basis,
|
||||
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
|
||||
# express or implied.
|
||||
# See the Licence for the specific language governing
|
||||
# permissions and limitations under the Licence.
|
||||
#
|
||||
#############################################################
|
||||
|
||||
OBJSX = UDPStreamer.x \
|
||||
SineArrayGAM.x
|
||||
|
||||
PACKAGE=Components/DataSources
|
||||
ROOT_DIR=../../../../
|
||||
MAKEDEFAULTDIR=$(MARTe2_DIR)/MakeDefaults
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibDefs.$(TARGET)
|
||||
|
||||
INCLUDES += -I.
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L0Types
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L2Objects
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L3Streams
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Messages
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L4Configuration
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/BareMetal/L5GAMs
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L3Services
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L4Messages
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/Scheduler/L5GAMs
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L1Portability
|
||||
INCLUDES += -I$(MARTe2_DIR)/Source/Core/FileSystem/L3Streams
|
||||
|
||||
all: $(OBJS) \
|
||||
$(BUILD_DIR)/UDPStreamer$(LIBEXT) \
|
||||
$(BUILD_DIR)/UDPStreamer$(DLLEXT)
|
||||
echo $(OBJS)
|
||||
|
||||
include depends.$(TARGET)
|
||||
|
||||
include $(MAKEDEFAULTDIR)/MakeStdLibRules.$(TARGET)
|
||||
@@ -0,0 +1,118 @@
|
||||
/**
|
||||
* @file SineArrayGAM.cpp
|
||||
* @brief Source file for class SineArrayGAM
|
||||
* @date 15/05/2026
|
||||
* @author Martino Ferrari
|
||||
*
|
||||
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
||||
* the Development of Fusion Energy ('Fusion for Energy').
|
||||
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
||||
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
||||
* You may not use this work except in compliance with the Licence.
|
||||
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
||||
*
|
||||
* @warning Unless required by applicable law or agreed to in writing,
|
||||
* software distributed under the Licence is distributed on an "AS IS"
|
||||
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
||||
* or implied. See the Licence permissions and limitations under the Licence.
|
||||
*/
|
||||
|
||||
#define DLL_API
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Standard header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include <cmath>
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "SineArrayGAM.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Method definitions */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
SineArrayGAM::SineArrayGAM() :
|
||||
GAM(),
|
||||
frequency(1.0),
|
||||
amplitude(1.0),
|
||||
offset(0.0),
|
||||
phase(0.0),
|
||||
samplingRate(1000000.0),
|
||||
nElements(0u),
|
||||
sampleOffset(0ull),
|
||||
outputBuf(NULL_PTR(float32 *)) {
|
||||
}
|
||||
|
||||
SineArrayGAM::~SineArrayGAM() {
|
||||
}
|
||||
|
||||
bool SineArrayGAM::Initialise(StructuredDataI &data) {
|
||||
bool ok = GAM::Initialise(data);
|
||||
if (ok) {
|
||||
if (!data.Read("Frequency", frequency)) {
|
||||
frequency = 1.0;
|
||||
}
|
||||
if (!data.Read("Amplitude", amplitude)) {
|
||||
amplitude = 1.0;
|
||||
}
|
||||
if (!data.Read("Offset", offset)) {
|
||||
offset = 0.0;
|
||||
}
|
||||
if (!data.Read("Phase", phase)) {
|
||||
phase = 0.0;
|
||||
}
|
||||
if (!data.Read("SamplingRate", samplingRate)) {
|
||||
samplingRate = 1000000.0;
|
||||
}
|
||||
if (samplingRate <= 0.0) {
|
||||
REPORT_ERROR(ErrorManagement::InitialisationError,
|
||||
"SineArrayGAM: SamplingRate must be greater than zero");
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool SineArrayGAM::Setup() {
|
||||
bool ok = (GetNumberOfOutputSignals() == 1u);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::InitialisationError,
|
||||
"SineArrayGAM: exactly one output signal is required");
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32 sz = 0u;
|
||||
ok = GetSignalByteSize(OutputSignals, 0u, sz);
|
||||
if (ok) {
|
||||
nElements = sz / static_cast<uint32>(sizeof(float32));
|
||||
outputBuf = reinterpret_cast<float32 *>(GetOutputSignalMemory(0u));
|
||||
ok = (outputBuf != NULL_PTR(float32 *)) && (nElements > 0u);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::InitialisationError,
|
||||
"SineArrayGAM: failed to resolve output signal memory");
|
||||
}
|
||||
}
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool SineArrayGAM::Execute() {
|
||||
static const float64 TWO_PI = 6.28318530717958647692;
|
||||
const float64 twoPiF = TWO_PI * frequency;
|
||||
const float64 invSr = 1.0 / samplingRate;
|
||||
|
||||
for (uint32 i = 0u; i < nElements; i++) {
|
||||
float64 t = static_cast<float64>(sampleOffset + static_cast<uint64>(i)) * invSr;
|
||||
outputBuf[i] = static_cast<float32>(amplitude * std::sin(twoPiF * t + phase) + offset);
|
||||
}
|
||||
sampleOffset += static_cast<uint64>(nElements);
|
||||
return true;
|
||||
}
|
||||
|
||||
CLASS_REGISTER(SineArrayGAM, "1.0")
|
||||
|
||||
} /* namespace MARTe */
|
||||
@@ -0,0 +1,105 @@
|
||||
/**
|
||||
* @file SineArrayGAM.h
|
||||
* @brief GAM that fills a float32 array with a continuous sinusoidal waveform.
|
||||
* @date 15/05/2026
|
||||
* @author Martino Ferrari
|
||||
*
|
||||
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
||||
* the Development of Fusion Energy ('Fusion for Energy').
|
||||
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
||||
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
||||
* You may not use this work except in compliance with the Licence.
|
||||
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
||||
*
|
||||
* @warning Unless required by applicable law or agreed to in writing,
|
||||
* software distributed under the Licence is distributed on an "AS IS"
|
||||
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
||||
* or implied. See the Licence permissions and limitations under the Licence.
|
||||
*
|
||||
* @details Each Execute() call fills one float32 output array with N samples of:
|
||||
* v[k] = Amplitude * sin(2π * Frequency * (sampleOffset + k) / SamplingRate + Phase) + Offset
|
||||
*
|
||||
* The sampleOffset accumulates across calls so the waveform phase is continuous.
|
||||
*
|
||||
* Configuration:
|
||||
* <pre>
|
||||
* +MyGAM = {
|
||||
* Class = SineArrayGAM
|
||||
* Frequency = 1000.0 // Signal frequency in Hz (default 1.0)
|
||||
* Amplitude = 1.0 // Signal amplitude (default 1.0)
|
||||
* Offset = 0.0 // DC offset (default 0.0)
|
||||
* Phase = 0.0 // Phase in radians (default 0.0)
|
||||
* SamplingRate = 1000000.0 // Sample rate in Hz; must match UDPStreamer signal (default 1000000.0)
|
||||
* OutputSignals = {
|
||||
* Ch1 = { DataSource = DDB; Type = float32; NumberOfElements = 1000 }
|
||||
* }
|
||||
* }
|
||||
* </pre>
|
||||
*
|
||||
* Exactly one output signal of type float32 is required.
|
||||
*/
|
||||
|
||||
#ifndef SINEARRAYGAM_H_
|
||||
#define SINEARRAYGAM_H_
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Standard header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "CompilerTypes.h"
|
||||
#include "GAM.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Class declaration */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
class SineArrayGAM : public GAM {
|
||||
public:
|
||||
CLASS_REGISTER_DECLARATION()
|
||||
|
||||
/**
|
||||
* @brief Constructor. Sets safe defaults.
|
||||
*/
|
||||
SineArrayGAM();
|
||||
|
||||
/**
|
||||
* @brief Destructor.
|
||||
*/
|
||||
virtual ~SineArrayGAM();
|
||||
|
||||
/**
|
||||
* @brief Reads Frequency, Amplitude, Offset, Phase, SamplingRate from config.
|
||||
*/
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
|
||||
/**
|
||||
* @brief Resolves the output signal pointer and element count.
|
||||
* @return true if exactly one float32 output signal is present.
|
||||
*/
|
||||
virtual bool Setup();
|
||||
|
||||
/**
|
||||
* @brief Fills the output array with the next N sinusoidal samples.
|
||||
* @return true always.
|
||||
*/
|
||||
virtual bool Execute();
|
||||
|
||||
private:
|
||||
float64 frequency; /**< Signal frequency [Hz] */
|
||||
float64 amplitude; /**< Signal amplitude */
|
||||
float64 offset; /**< DC offset */
|
||||
float64 phase; /**< Phase offset [radians] */
|
||||
float64 samplingRate; /**< Sample rate [Hz] */
|
||||
uint32 nElements; /**< Number of output elements per cycle */
|
||||
uint64 sampleOffset; /**< Cumulative sample count for continuous phase */
|
||||
float32 *outputBuf; /**< Pointer to the output signal memory */
|
||||
};
|
||||
|
||||
} /* namespace MARTe */
|
||||
|
||||
#endif /* SINEARRAYGAM_H_ */
|
||||
@@ -0,0 +1,995 @@
|
||||
/**
|
||||
* @file UDPStreamer.cpp
|
||||
* @brief Source file for class UDPStreamer
|
||||
* @date 13/05/2026
|
||||
* @author Martino Ferrari
|
||||
*
|
||||
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
||||
* the Development of Fusion Energy ('Fusion for Energy').
|
||||
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
||||
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
||||
* You may not use this work except in compliance with the Licence.
|
||||
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
||||
*
|
||||
* @warning Unless required by applicable law or agreed to in writing,
|
||||
* software distributed under the Licence is distributed on an "AS IS"
|
||||
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
||||
* or implied. See the Licence permissions and limitations under the Licence.
|
||||
*
|
||||
* @details This source file contains the definition of all the methods for
|
||||
* the class UDPStreamer (public, protected, and private). Be aware that some
|
||||
* methods, such as those inline could be defined on the header file, instead.
|
||||
*/
|
||||
|
||||
#define DLL_API
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Standard header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include <sys/select.h>
|
||||
#include <unistd.h>
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "AdvancedErrorManagement.h"
|
||||
#include "EmbeddedThreadI.h"
|
||||
#include "GlobalObjectsDatabase.h"
|
||||
#include "HighResolutionTimer.h"
|
||||
#include "MemoryMapSynchronisedOutputBroker.h"
|
||||
#include "MemoryOperationsHelper.h"
|
||||
#include "Sleep.h"
|
||||
#include "Threads.h"
|
||||
#include "UDPStreamer.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Static definitions */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
/** Default port used when none is specified. */
|
||||
static const uint16 UDPS_DEFAULT_PORT = 44500u;
|
||||
|
||||
/** Default max payload per UDP datagram (bytes). */
|
||||
static const uint32 UDPS_DEFAULT_MAX_PAYLOAD = 1400u;
|
||||
|
||||
/** Minimum MaxPayloadSize: header + at least 1 byte of payload. */
|
||||
static const uint32 UDPS_MIN_PAYLOAD = static_cast<uint32>(sizeof(UDPSPacketHeader)) + 1u;
|
||||
|
||||
/** Server socket receive timeout in milliseconds. */
|
||||
static const uint32 UDPS_RECV_TIMEOUT_MS = 5u;
|
||||
|
||||
/** EventSem wait timeout in milliseconds for the data loop. */
|
||||
static const uint32 UDPS_DATA_WAIT_MS = 10u;
|
||||
|
||||
/** Sentinel: no time-signal reference; use the packet-level timestamp. */
|
||||
static const uint32 UDPS_NO_TIME_SIGNAL = 0xFFFFFFFFu;
|
||||
|
||||
/** Max signal name length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_SIGNAL_NAME = 64u;
|
||||
|
||||
/** Max unit string length in CONFIG packet (including null terminator). */
|
||||
static const uint32 UDPS_MAX_UNIT_LEN = 32u;
|
||||
|
||||
/** Size in bytes of one signal descriptor in the CONFIG payload. */
|
||||
static const uint32 UDPS_SIGNAL_DESC_SIZE =
|
||||
UDPS_MAX_SIGNAL_NAME /* name */
|
||||
+ 1u /* typeCode */
|
||||
+ 1u /* quantType */
|
||||
+ 1u /* numDimensions*/
|
||||
+ 4u /* numRows */
|
||||
+ 4u /* numCols */
|
||||
+ 8u /* rangeMin */
|
||||
+ 8u /* rangeMax */
|
||||
+ 1u /* timeMode */
|
||||
+ 8u /* samplingRate */
|
||||
+ 4u /* timeSignalIdx*/
|
||||
+ UDPS_MAX_UNIT_LEN; /* unit */
|
||||
|
||||
/** Bytes prepended to each DATA payload for the HRT packet timestamp. */
|
||||
static const uint32 UDPS_TIMESTAMP_BYTES = 8u;
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Method definitions */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
UDPStreamer::UDPStreamer() :
|
||||
MemoryDataSourceI(),
|
||||
EmbeddedServiceMethodBinderI(),
|
||||
executor(*this) {
|
||||
port = UDPS_DEFAULT_PORT;
|
||||
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
|
||||
cpuMask = 0xFFFFFFFFu;
|
||||
stackSize = THREADS_DEFAULT_STACKSIZE;
|
||||
numSigs = 0u;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
readyBuffer = NULL_PTR(uint8 *);
|
||||
scratchBuffer = NULL_PTR(uint8 *);
|
||||
wireBuffer = NULL_PTR(uint8 *);
|
||||
totalSrcBytes = 0u;
|
||||
totalWireBytes = 0u;
|
||||
syncTimestamp = 0u;
|
||||
clientConnected = false;
|
||||
packetCounter = 0u;
|
||||
|
||||
if (!dataSem.Create()) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError, "Could not create EventSem.");
|
||||
}
|
||||
bufMutex.Create(false);
|
||||
}
|
||||
|
||||
/*lint -e{1551} Destructor must guarantee thread and socket cleanup. */
|
||||
UDPStreamer::~UDPStreamer() {
|
||||
/* Unblock the background thread's dataSem wait so it can exit */
|
||||
(void) dataSem.Post();
|
||||
|
||||
if (executor.GetStatus() != EmbeddedThreadI::OffState) {
|
||||
if (!executor.Stop()) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"First Stop() attempt failed; retrying.");
|
||||
if (!executor.Stop()) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not stop background thread.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (serverSocket.IsValid()) {
|
||||
(void) serverSocket.Close();
|
||||
}
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
|
||||
if (signalInfos != NULL_PTR(UDPStreamerSignalInfo *)) {
|
||||
delete[] signalInfos;
|
||||
signalInfos = NULL_PTR(UDPStreamerSignalInfo *);
|
||||
}
|
||||
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
if (readyBuffer != NULL_PTR(uint8 *)) {
|
||||
heap->Free(reinterpret_cast<void *&>(readyBuffer));
|
||||
}
|
||||
if (scratchBuffer != NULL_PTR(uint8 *)) {
|
||||
heap->Free(reinterpret_cast<void *&>(scratchBuffer));
|
||||
}
|
||||
if (wireBuffer != NULL_PTR(uint8 *)) {
|
||||
heap->Free(reinterpret_cast<void *&>(wireBuffer));
|
||||
}
|
||||
|
||||
(void) dataSem.Close();
|
||||
}
|
||||
|
||||
bool UDPStreamer::Initialise(StructuredDataI &data) {
|
||||
bool ok = MemoryDataSourceI::Initialise(data);
|
||||
|
||||
if (ok) {
|
||||
if (!data.Read("Port", port)) {
|
||||
port = UDPS_DEFAULT_PORT;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Port not specified; using default %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
if (port <= 1024u) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Port %u is in the privileged range (<= 1024).",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
if (!data.Read("MaxPayloadSize", maxPayloadSize)) {
|
||||
maxPayloadSize = UDPS_DEFAULT_MAX_PAYLOAD;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"MaxPayloadSize not specified; using default %u.",
|
||||
maxPayloadSize);
|
||||
}
|
||||
if (maxPayloadSize < UDPS_MIN_PAYLOAD) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"MaxPayloadSize %u is too small (minimum %u).",
|
||||
maxPayloadSize, UDPS_MIN_PAYLOAD);
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
uint32 cpuMaskIn = 0xFFFFFFFFu;
|
||||
if (!data.Read("CPUMask", cpuMaskIn)) {
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"CPUMask not specified; using 0xFFFFFFFF.");
|
||||
}
|
||||
cpuMask = cpuMaskIn;
|
||||
}
|
||||
|
||||
if (ok) {
|
||||
if (!data.Read("StackSize", stackSize)) {
|
||||
stackSize = THREADS_DEFAULT_STACKSIZE;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"StackSize not specified; using MARTe2 default %u.",
|
||||
stackSize);
|
||||
}
|
||||
if (stackSize == 0u) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError, "StackSize must be > 0.");
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool UDPStreamer::SetConfiguredDatabase(StructuredDataI &data) {
|
||||
bool ok = MemoryDataSourceI::SetConfiguredDatabase(data);
|
||||
if (!ok) {
|
||||
return false;
|
||||
}
|
||||
|
||||
numSigs = GetNumberOfSignals();
|
||||
if (numSigs == 0u) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"At least one signal must be defined.");
|
||||
return false;
|
||||
}
|
||||
|
||||
signalInfos = new UDPStreamerSignalInfo[numSigs];
|
||||
/* Local array to hold time-signal names (resolved to indices in pass 3) */
|
||||
StreamString *timeSignalNames = new StreamString[numSigs];
|
||||
|
||||
/* --- Pass 1: populate from the MARTe2 framework APIs --- */
|
||||
totalSrcBytes = 0u;
|
||||
totalWireBytes = UDPS_TIMESTAMP_BYTES;
|
||||
|
||||
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
||||
StreamString sigName;
|
||||
ok = GetSignalName(i, sigName);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not get name for signal %u.", i);
|
||||
break;
|
||||
}
|
||||
signalInfos[i].name = sigName;
|
||||
signalInfos[i].type = GetSignalType(i);
|
||||
signalInfos[i].numDimensions = 0u;
|
||||
signalInfos[i].numElements = 1u;
|
||||
signalInfos[i].numRows = 1u;
|
||||
signalInfos[i].numCols = 1u;
|
||||
signalInfos[i].quantType = UDPStreamerQuantNone;
|
||||
signalInfos[i].rangeMin = 0.0;
|
||||
signalInfos[i].rangeMax = 1.0;
|
||||
signalInfos[i].timeMode = UDPStreamerTimePacket;
|
||||
signalInfos[i].samplingRate = 0.0;
|
||||
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
|
||||
signalInfos[i].unit = "";
|
||||
signalInfos[i].srcByteSize = 0u;
|
||||
signalInfos[i].wireByteSize = 0u;
|
||||
signalInfos[i].bufferOffset = 0u;
|
||||
timeSignalNames[i] = "";
|
||||
|
||||
uint8 ndims = 0u;
|
||||
(void) GetSignalNumberOfDimensions(i, ndims);
|
||||
signalInfos[i].numDimensions = ndims;
|
||||
|
||||
uint32 nelems = 1u;
|
||||
(void) GetSignalNumberOfElements(i, nelems);
|
||||
signalInfos[i].numElements = nelems;
|
||||
signalInfos[i].numCols = nelems;
|
||||
|
||||
uint32 bsz = 0u;
|
||||
(void) GetSignalByteSize(i, bsz);
|
||||
signalInfos[i].srcByteSize = bsz;
|
||||
signalInfos[i].bufferOffset = totalSrcBytes;
|
||||
totalSrcBytes += bsz;
|
||||
}
|
||||
|
||||
/* --- Pass 2: read custom per-signal fields from signalsDatabase ---
|
||||
* Note: DataSourceI::AddSignals() leaves signalsDatabase positioned at the
|
||||
* "Signals" node. We must reset to root before navigating. */
|
||||
if (ok) {
|
||||
(void) signalsDatabase.MoveToRoot();
|
||||
bool moved = signalsDatabase.MoveRelative("Signals");
|
||||
if (!moved) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not navigate to Signals in signalsDatabase.");
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
||||
bool moved = signalsDatabase.MoveRelative(signalInfos[i].name.Buffer());
|
||||
if (!moved) {
|
||||
/* Signal added by framework with no user-configured custom fields */
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Unit */
|
||||
StreamString unit = "";
|
||||
(void) signalsDatabase.Read("Unit", unit);
|
||||
signalInfos[i].unit = unit;
|
||||
|
||||
/* Range */
|
||||
(void) signalsDatabase.Read("RangeMin", signalInfos[i].rangeMin);
|
||||
(void) signalsDatabase.Read("RangeMax", signalInfos[i].rangeMax);
|
||||
|
||||
/* QuantizedType */
|
||||
StreamString quantStr = "";
|
||||
if (signalsDatabase.Read("QuantizedType", quantStr)) {
|
||||
if (quantStr == "uint8") {
|
||||
signalInfos[i].quantType = UDPStreamerQuantUint8;
|
||||
}
|
||||
else if (quantStr == "int8") {
|
||||
signalInfos[i].quantType = UDPStreamerQuantInt8;
|
||||
}
|
||||
else if (quantStr == "uint16") {
|
||||
signalInfos[i].quantType = UDPStreamerQuantUint16;
|
||||
}
|
||||
else if (quantStr == "int16") {
|
||||
signalInfos[i].quantType = UDPStreamerQuantInt16;
|
||||
}
|
||||
else if (quantStr == "none") {
|
||||
signalInfos[i].quantType = UDPStreamerQuantNone;
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: unknown QuantizedType '%s'. "
|
||||
"Allowed: none|uint8|int8|uint16|int16.",
|
||||
signalInfos[i].name.Buffer(), quantStr.Buffer());
|
||||
ok = false;
|
||||
}
|
||||
if (ok && (signalInfos[i].quantType != UDPStreamerQuantNone)) {
|
||||
TypeDescriptor td = signalInfos[i].type;
|
||||
bool isFloat = ((td == Float32Bit) || (td == Float64Bit));
|
||||
if (!isFloat) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: QuantizedType only supported for "
|
||||
"float32/float64 signals.",
|
||||
signalInfos[i].name.Buffer());
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* TimeMode */
|
||||
if (ok) {
|
||||
StreamString timeModeStr;
|
||||
(void) signalsDatabase.Read("TimeMode", timeModeStr);
|
||||
if (timeModeStr.Size() == 0u) {
|
||||
timeModeStr = "PacketTime";
|
||||
}
|
||||
if (timeModeStr == "PacketTime") {
|
||||
signalInfos[i].timeMode = UDPStreamerTimePacket;
|
||||
}
|
||||
else if (timeModeStr == "FullArray") {
|
||||
signalInfos[i].timeMode = UDPStreamerTimeFullArray;
|
||||
}
|
||||
else if (timeModeStr == "FirstSample") {
|
||||
signalInfos[i].timeMode = UDPStreamerTimeFirstSample;
|
||||
}
|
||||
else if (timeModeStr == "LastSample") {
|
||||
signalInfos[i].timeMode = UDPStreamerTimeLastSample;
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: unknown TimeMode '%s'. "
|
||||
"Allowed: PacketTime|FullArray|FirstSample|LastSample.",
|
||||
signalInfos[i].name.Buffer(), timeModeStr.Buffer());
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* TimeSignal (required when TimeMode != PacketTime) */
|
||||
if (ok && (signalInfos[i].timeMode != UDPStreamerTimePacket)) {
|
||||
StreamString tsName = "";
|
||||
if (!signalsDatabase.Read("TimeSignal", tsName)) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: TimeSignal must be specified when "
|
||||
"TimeMode != PacketTime.",
|
||||
signalInfos[i].name.Buffer());
|
||||
ok = false;
|
||||
}
|
||||
else {
|
||||
timeSignalNames[i] = tsName;
|
||||
/* Index resolved in pass 3 */
|
||||
signalInfos[i].timeSignalIdx = UDPS_NO_TIME_SIGNAL;
|
||||
}
|
||||
}
|
||||
|
||||
/* SamplingRate */
|
||||
if (ok) {
|
||||
(void) signalsDatabase.Read("SamplingRate", signalInfos[i].samplingRate);
|
||||
bool needsRate = (signalInfos[i].timeMode == UDPStreamerTimeFirstSample ||
|
||||
signalInfos[i].timeMode == UDPStreamerTimeLastSample);
|
||||
if (needsRate && (signalInfos[i].samplingRate <= 0.0)) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: SamplingRate > 0 is required for "
|
||||
"FirstSample/LastSample TimeMode.",
|
||||
signalInfos[i].name.Buffer());
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
(void) signalsDatabase.MoveToAncestor(1u);
|
||||
}
|
||||
|
||||
if (ok || true) { /* always attempt to restore navigation */
|
||||
(void) signalsDatabase.MoveToAncestor(1u);
|
||||
}
|
||||
|
||||
/* --- Pass 3: resolve TimeSignal names to signal indices --- */
|
||||
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
||||
if (signalInfos[i].timeMode == UDPStreamerTimePacket) {
|
||||
continue; /* no time signal needed */
|
||||
}
|
||||
bool found = false;
|
||||
for (uint32 j = 0u; j < numSigs; j++) {
|
||||
if (signalInfos[j].name == timeSignalNames[i]) {
|
||||
signalInfos[i].timeSignalIdx = j;
|
||||
found = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!found) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: TimeSignal '%s' not found among declared signals.",
|
||||
signalInfos[i].name.Buffer(),
|
||||
timeSignalNames[i].Buffer());
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
delete[] timeSignalNames;
|
||||
timeSignalNames = NULL_PTR(StreamString *);
|
||||
|
||||
/* --- Pass 4: validate time-signal dimensions and compute wire sizes --- */
|
||||
for (uint32 i = 0u; i < numSigs && ok; i++) {
|
||||
/* Compute wire byte size per element */
|
||||
uint32 elemWireBytes = 0u;
|
||||
switch (signalInfos[i].quantType) {
|
||||
case UDPStreamerQuantUint8:
|
||||
case UDPStreamerQuantInt8:
|
||||
elemWireBytes = 1u;
|
||||
break;
|
||||
case UDPStreamerQuantUint16:
|
||||
case UDPStreamerQuantInt16:
|
||||
elemWireBytes = 2u;
|
||||
break;
|
||||
default:
|
||||
/* Raw copy: element size = total / numElements */
|
||||
if (signalInfos[i].numElements > 0u) {
|
||||
elemWireBytes = signalInfos[i].srcByteSize / signalInfos[i].numElements;
|
||||
}
|
||||
break;
|
||||
}
|
||||
signalInfos[i].wireByteSize = elemWireBytes * signalInfos[i].numElements;
|
||||
totalWireBytes += signalInfos[i].wireByteSize;
|
||||
|
||||
/* Validate time signal dimensions */
|
||||
uint32 tsIdx = signalInfos[i].timeSignalIdx;
|
||||
if (tsIdx != UDPS_NO_TIME_SIGNAL) {
|
||||
uint32 tsElems = signalInfos[tsIdx].numElements;
|
||||
if (signalInfos[i].timeMode == UDPStreamerTimeFullArray) {
|
||||
if (tsElems != signalInfos[i].numElements) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: FullArray TimeMode requires TimeSignal "
|
||||
"%s to have the same NumberOfElements (%u vs %u).",
|
||||
signalInfos[i].name.Buffer(),
|
||||
signalInfos[tsIdx].name.Buffer(),
|
||||
tsElems, signalInfos[i].numElements);
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
else if ((signalInfos[i].timeMode == UDPStreamerTimeFirstSample) ||
|
||||
(signalInfos[i].timeMode == UDPStreamerTimeLastSample)) {
|
||||
if (tsElems != 1u) {
|
||||
REPORT_ERROR(ErrorManagement::ParametersError,
|
||||
"Signal %s: FirstSample/LastSample TimeMode requires "
|
||||
"a scalar TimeSignal (found %u elements).",
|
||||
signalInfos[i].name.Buffer(), tsElems);
|
||||
ok = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool UDPStreamer::AllocateMemory() {
|
||||
bool ok = MemoryDataSourceI::AllocateMemory();
|
||||
if (!ok) {
|
||||
return false;
|
||||
}
|
||||
|
||||
/* stateMemorySize is populated by MemoryDataSourceI::AllocateMemory() */
|
||||
if (totalSrcBytes == 0u) {
|
||||
totalSrcBytes = stateMemorySize;
|
||||
}
|
||||
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
|
||||
/* readyBuffer: copy of signal memory shared with background thread */
|
||||
readyBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
|
||||
if (readyBuffer == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate readyBuffer.");
|
||||
return false;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Set(readyBuffer, 0, totalSrcBytes);
|
||||
|
||||
/* scratchBuffer: background-thread-private copy for serialization */
|
||||
scratchBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalSrcBytes));
|
||||
if (scratchBuffer == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate scratchBuffer.");
|
||||
return false;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Set(scratchBuffer, 0, totalSrcBytes);
|
||||
|
||||
/* wireBuffer: serialized/quantized payload for transmission */
|
||||
wireBuffer = reinterpret_cast<uint8 *>(heap->Malloc(totalWireBytes));
|
||||
if (wireBuffer == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError, "Could not allocate wireBuffer.");
|
||||
return false;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Set(wireBuffer, 0, totalWireBytes);
|
||||
|
||||
/* Update buffer offsets to match actual MemoryDataSourceI layout */
|
||||
for (uint32 i = 0u; i < numSigs; i++) {
|
||||
void *addr = NULL_PTR(void *);
|
||||
if (GetSignalMemoryBuffer(i, 0u, addr)) {
|
||||
signalInfos[i].bufferOffset =
|
||||
static_cast<uint32>(reinterpret_cast<uint8 *>(addr) - memory);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
const char8 *UDPStreamer::GetBrokerName(StructuredDataI &data,
|
||||
const SignalDirection direction) {
|
||||
const char8 *brokerName = "";
|
||||
if (direction == OutputSignals) {
|
||||
brokerName = "MemoryMapSynchronisedOutputBroker";
|
||||
}
|
||||
return brokerName;
|
||||
}
|
||||
|
||||
bool UDPStreamer::PrepareNextState(const char8 *const currentStateName,
|
||||
const char8 *const nextStateName) {
|
||||
bool ok = true;
|
||||
|
||||
/* Open server socket (idempotent: skip if already valid) */
|
||||
if (!serverSocket.IsValid()) {
|
||||
ok = serverSocket.Open();
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not open server UDP socket.");
|
||||
}
|
||||
if (ok) {
|
||||
ok = serverSocket.Listen(port);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not bind server socket to port %u.",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
}
|
||||
/* Socket stays blocking; Read() uses the timeout via select() internally. */
|
||||
}
|
||||
|
||||
/* Start the background thread (idempotent) */
|
||||
if (ok && (executor.GetStatus() == EmbeddedThreadI::OffState)) {
|
||||
executor.SetName(GetName());
|
||||
executor.SetCPUMask(ProcessorType(cpuMask));
|
||||
executor.SetStackSize(stackSize);
|
||||
ErrorManagement::ErrorType startErr = executor.Start();
|
||||
ok = (startErr == ErrorManagement::NoError);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not start background thread.");
|
||||
}
|
||||
}
|
||||
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool UDPStreamer::Synchronise() {
|
||||
/* Capture timestamp as early as possible */
|
||||
uint64 ts = HighResolutionTimer::Counter();
|
||||
|
||||
/* RT-safe copy of signal memory → readyBuffer */
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
(void) MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
||||
syncTimestamp = ts;
|
||||
bufMutex.FastUnLock();
|
||||
|
||||
/* Wake the background sender thread */
|
||||
(void) dataSem.Post();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
ErrorManagement::ErrorType ret = ErrorManagement::NoError;
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::StartupStage) {
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"UDPStreamer background thread started (port %u).",
|
||||
static_cast<uint32>(port));
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::MainStage) {
|
||||
/* --- Poll server socket for incoming client commands ---
|
||||
* Use select() directly so we get a silent timeout with no log spam.
|
||||
* MARTe2's Read(buf, size, timeout) calls recvfrom() and logs an error
|
||||
* on every timeout (EAGAIN from SO_RCVTIMEO). */
|
||||
uint8 cmdBuf[256u];
|
||||
Handle sockFd = serverSocket.GetReadHandle();
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(static_cast<int>(sockFd), &rfds);
|
||||
struct timeval tv;
|
||||
tv.tv_sec = 0;
|
||||
tv.tv_usec = static_cast<long>(UDPS_RECV_TIMEOUT_MS) * 1000L;
|
||||
int nReady = select(static_cast<int>(sockFd) + 1, &rfds, NULL_PTR(fd_set *), NULL_PTR(fd_set *), &tv);
|
||||
if (nReady > 0) {
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(cmdBuf));
|
||||
bool received = serverSocket.Read(reinterpret_cast<char8 *>(cmdBuf), recvSize);
|
||||
if (received && (recvSize >= static_cast<uint32>(sizeof(UDPSPacketHeader)))) {
|
||||
HandleClientCommand(cmdBuf, recvSize);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- Wait for RT thread to post new data --- */
|
||||
/* ResetWait atomically lowers the barrier then waits, preventing missed posts */
|
||||
ErrorManagement::ErrorType waitErr =
|
||||
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
bool dataReady = (waitErr == ErrorManagement::NoError);
|
||||
|
||||
if (dataReady && clientConnected) {
|
||||
/* Copy readyBuffer → scratchBuffer under brief spinlock */
|
||||
uint64 ts = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
(void) MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer, totalSrcBytes);
|
||||
ts = syncTimestamp;
|
||||
bufMutex.FastUnLock();
|
||||
|
||||
/* Serialize signal data into wireBuffer */
|
||||
QuantizeAndSerialize(scratchBuffer, ts);
|
||||
|
||||
/* Send (fragmented if needed) */
|
||||
packetCounter++;
|
||||
if (!SendFragmented(UDPS_TYPE_DATA, packetCounter, wireBuffer, totalWireBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send DATA packet (counter=%u).", packetCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
if (clientConnected) {
|
||||
(void) clientSocket.Close();
|
||||
clientConnected = false;
|
||||
}
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"UDPStreamer background thread terminated.");
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void UDPStreamer::HandleClientCommand(const uint8 *buf, uint32 size) {
|
||||
if (size < static_cast<uint32>(sizeof(UDPSPacketHeader))) {
|
||||
return;
|
||||
}
|
||||
const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
|
||||
if (hdr->magic != UDPS_MAGIC) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (hdr->type == UDPS_TYPE_CONNECT) {
|
||||
InternetHost src = serverSocket.GetSource();
|
||||
|
||||
/* Disconnect any previous client */
|
||||
if (clientConnected) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
|
||||
/* Open a new client socket and connect to the requesting address */
|
||||
bool sockOk = clientSocket.Open();
|
||||
if (sockOk) {
|
||||
sockOk = clientSocket.Connect(src.GetAddress().Buffer(), src.GetPort());
|
||||
}
|
||||
if (sockOk) {
|
||||
clientConnected = true;
|
||||
REPORT_ERROR(ErrorManagement::Information,
|
||||
"Client connected from %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
|
||||
/* Send CONFIG packet */
|
||||
uint32 configBufSize = 4u + (numSigs * UDPS_SIGNAL_DESC_SIZE) + 32u;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *cfgBuf = reinterpret_cast<uint8 *>(heap->Malloc(configBufSize));
|
||||
if (cfgBuf != NULL_PTR(uint8 *)) {
|
||||
uint32 cfgPayloadSize = 0u;
|
||||
if (BuildConfigPayload(cfgBuf, configBufSize, cfgPayloadSize)) {
|
||||
(void) SendFragmented(UDPS_TYPE_CONFIG, 0u, cfgBuf, cfgPayloadSize);
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not build CONFIG payload.");
|
||||
}
|
||||
heap->Free(reinterpret_cast<void *&>(cfgBuf));
|
||||
}
|
||||
}
|
||||
else {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Could not connect to client %s:%u.",
|
||||
src.GetAddress().Buffer(),
|
||||
static_cast<uint32>(src.GetPort()));
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_DISCONNECT) {
|
||||
REPORT_ERROR(ErrorManagement::Information, "Client sent DISCONNECT.");
|
||||
if (clientConnected) {
|
||||
if (clientSocket.IsValid()) {
|
||||
(void) clientSocket.Close();
|
||||
}
|
||||
clientConnected = false;
|
||||
}
|
||||
}
|
||||
else if (hdr->type == UDPS_TYPE_ACK) {
|
||||
/* Optional: track acknowledged counters for loss detection */
|
||||
if (size >= static_cast<uint32>(sizeof(UDPSPacketHeader)) + 4u) {
|
||||
uint32 ackedCounter = 0u;
|
||||
const uint8 *pl = buf + sizeof(UDPSPacketHeader);
|
||||
(void) MemoryOperationsHelper::Copy(&ackedCounter, pl, 4u);
|
||||
REPORT_ERROR(ErrorManagement::Debug,
|
||||
"ACK received for packet counter %u.", ackedCounter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::BuildConfigPayload(uint8 *buf,
|
||||
uint32 bufSize,
|
||||
uint32 &payloadSize) {
|
||||
payloadSize = 0u;
|
||||
|
||||
/* 4 bytes: number of signals */
|
||||
if ((payloadSize + 4u) > bufSize) {
|
||||
return false;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Copy(buf + payloadSize, &numSigs, 4u);
|
||||
payloadSize += 4u;
|
||||
|
||||
for (uint32 i = 0u; i < numSigs; i++) {
|
||||
if ((payloadSize + UDPS_SIGNAL_DESC_SIZE) > bufSize) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8 *p = buf + payloadSize;
|
||||
|
||||
/* Name: 64 bytes, zero-padded */
|
||||
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_SIGNAL_NAME);
|
||||
uint32 nameLen = static_cast<uint32>(signalInfos[i].name.Size());
|
||||
if (nameLen >= UDPS_MAX_SIGNAL_NAME) {
|
||||
nameLen = UDPS_MAX_SIGNAL_NAME - 1u;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].name.Buffer(), nameLen);
|
||||
p += UDPS_MAX_SIGNAL_NAME;
|
||||
|
||||
/* Type code: 1 byte */
|
||||
*p = TypeDescriptorToCode(signalInfos[i].type);
|
||||
p += 1u;
|
||||
|
||||
/* Quant type: 1 byte */
|
||||
*p = static_cast<uint8>(signalInfos[i].quantType);
|
||||
p += 1u;
|
||||
|
||||
/* numDimensions: 1 byte */
|
||||
*p = signalInfos[i].numDimensions;
|
||||
p += 1u;
|
||||
|
||||
/* numRows: 4 bytes */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numRows, 4u);
|
||||
p += 4u;
|
||||
|
||||
/* numCols: 4 bytes */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].numCols, 4u);
|
||||
p += 4u;
|
||||
|
||||
/* rangeMin: 8 bytes (float64) */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMin, 8u);
|
||||
p += 8u;
|
||||
|
||||
/* rangeMax: 8 bytes (float64) */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].rangeMax, 8u);
|
||||
p += 8u;
|
||||
|
||||
/* timeMode: 1 byte */
|
||||
*p = static_cast<uint8>(signalInfos[i].timeMode);
|
||||
p += 1u;
|
||||
|
||||
/* samplingRate: 8 bytes (float64) */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].samplingRate, 8u);
|
||||
p += 8u;
|
||||
|
||||
/* timeSignalIdx: 4 bytes */
|
||||
(void) MemoryOperationsHelper::Copy(p, &signalInfos[i].timeSignalIdx, 4u);
|
||||
p += 4u;
|
||||
|
||||
/* Unit: 32 bytes, zero-padded */
|
||||
(void) MemoryOperationsHelper::Set(p, 0, UDPS_MAX_UNIT_LEN);
|
||||
uint32 unitLen = static_cast<uint32>(signalInfos[i].unit.Size());
|
||||
if (unitLen >= UDPS_MAX_UNIT_LEN) {
|
||||
unitLen = UDPS_MAX_UNIT_LEN - 1u;
|
||||
}
|
||||
(void) MemoryOperationsHelper::Copy(p, signalInfos[i].unit.Buffer(), unitLen);
|
||||
p += UDPS_MAX_UNIT_LEN;
|
||||
|
||||
payloadSize += UDPS_SIGNAL_DESC_SIZE;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void UDPStreamer::QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp) {
|
||||
uint8 *dst = wireBuffer;
|
||||
|
||||
/* 8-byte packet timestamp */
|
||||
(void) MemoryOperationsHelper::Copy(dst, ×tamp, UDPS_TIMESTAMP_BYTES);
|
||||
dst += UDPS_TIMESTAMP_BYTES;
|
||||
|
||||
for (uint32 i = 0u; i < numSigs; i++) {
|
||||
const uint8 *src = srcBuf + signalInfos[i].bufferOffset;
|
||||
|
||||
if (signalInfos[i].quantType == UDPStreamerQuantNone) {
|
||||
/* Raw copy */
|
||||
(void) MemoryOperationsHelper::Copy(dst, src, signalInfos[i].srcByteSize);
|
||||
dst += signalInfos[i].srcByteSize;
|
||||
}
|
||||
else {
|
||||
float64 rMin = signalInfos[i].rangeMin;
|
||||
float64 rRange = signalInfos[i].rangeMax - rMin;
|
||||
if (rRange == 0.0) {
|
||||
rRange = 1.0; /* guard against divide-by-zero */
|
||||
}
|
||||
bool isSrcFloat32 = (signalInfos[i].type == Float32Bit);
|
||||
uint32 nelems = signalInfos[i].numElements;
|
||||
const uint8 *s = src;
|
||||
|
||||
for (uint32 e = 0u; e < nelems; e++) {
|
||||
float64 rawVal = 0.0;
|
||||
if (isSrcFloat32) {
|
||||
float32 f32 = 0.0f;
|
||||
(void) MemoryOperationsHelper::Copy(&f32, s, 4u);
|
||||
rawVal = static_cast<float64>(f32);
|
||||
s += 4u;
|
||||
}
|
||||
else {
|
||||
(void) MemoryOperationsHelper::Copy(&rawVal, s, 8u);
|
||||
s += 8u;
|
||||
}
|
||||
|
||||
/* Normalize and clamp to [0.0, 1.0] */
|
||||
float64 norm = (rawVal - rMin) / rRange;
|
||||
if (norm < 0.0) { norm = 0.0; }
|
||||
if (norm > 1.0) { norm = 1.0; }
|
||||
|
||||
switch (signalInfos[i].quantType) {
|
||||
case UDPStreamerQuantUint8: {
|
||||
uint8 q = static_cast<uint8>(norm * 255.0);
|
||||
*dst = q;
|
||||
dst += 1u;
|
||||
break;
|
||||
}
|
||||
case UDPStreamerQuantInt8: {
|
||||
int8 q = static_cast<int8>((norm * 254.0) - 127.0);
|
||||
(void) MemoryOperationsHelper::Copy(dst, &q, 1u);
|
||||
dst += 1u;
|
||||
break;
|
||||
}
|
||||
case UDPStreamerQuantUint16: {
|
||||
uint16 q = static_cast<uint16>(norm * 65535.0);
|
||||
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
|
||||
dst += 2u;
|
||||
break;
|
||||
}
|
||||
case UDPStreamerQuantInt16: {
|
||||
int16 q = static_cast<int16>((norm * 65534.0) - 32767.0);
|
||||
(void) MemoryOperationsHelper::Copy(dst, &q, 2u);
|
||||
dst += 2u;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPStreamer::SendFragmented(uint8 type,
|
||||
uint32 counter,
|
||||
const uint8 *payload,
|
||||
uint32 payloadSize) {
|
||||
uint32 headerSize = static_cast<uint32>(sizeof(UDPSPacketHeader));
|
||||
uint32 maxChunk = maxPayloadSize - headerSize;
|
||||
uint32 totalFrags = (payloadSize == 0u) ? 1u :
|
||||
((payloadSize + maxChunk - 1u) / maxChunk);
|
||||
|
||||
uint32 sendBufSize = headerSize + maxChunk;
|
||||
HeapI *heap = GlobalObjectsDatabase::Instance()->GetStandardHeap();
|
||||
uint8 *sendBuf = reinterpret_cast<uint8 *>(heap->Malloc(sendBufSize));
|
||||
if (sendBuf == NULL_PTR(uint8 *)) {
|
||||
REPORT_ERROR(ErrorManagement::FatalError,
|
||||
"Could not allocate send buffer (%u bytes).", sendBufSize);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ok = true;
|
||||
uint32 offs = 0u;
|
||||
|
||||
for (uint32 f = 0u; (f < totalFrags) && ok; f++) {
|
||||
uint32 chunkSize = payloadSize - offs;
|
||||
if (chunkSize > maxChunk) {
|
||||
chunkSize = maxChunk;
|
||||
}
|
||||
|
||||
UDPSPacketHeader *hdr = reinterpret_cast<UDPSPacketHeader *>(sendBuf);
|
||||
hdr->magic = UDPS_MAGIC;
|
||||
hdr->type = type;
|
||||
hdr->counter = counter;
|
||||
hdr->fragmentIdx = static_cast<uint16>(f);
|
||||
hdr->totalFragments = static_cast<uint16>(totalFrags);
|
||||
hdr->payloadBytes = chunkSize;
|
||||
|
||||
if (chunkSize > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(sendBuf + headerSize,
|
||||
payload + offs,
|
||||
chunkSize);
|
||||
}
|
||||
|
||||
uint32 sendSize = headerSize + chunkSize;
|
||||
ok = clientSocket.Write(reinterpret_cast<const char8 *>(sendBuf), sendSize);
|
||||
if (!ok) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Fragment %u/%u send failed.", f + 1u, totalFrags);
|
||||
}
|
||||
offs += chunkSize;
|
||||
}
|
||||
|
||||
heap->Free(reinterpret_cast<void *&>(sendBuf));
|
||||
return ok;
|
||||
}
|
||||
|
||||
uint8 UDPStreamer::TypeDescriptorToCode(TypeDescriptor td) {
|
||||
uint8 code = UDPS_TYPECODE_UNKNOWN;
|
||||
if (td == UnsignedInteger8Bit) { code = UDPS_TYPECODE_UINT8; }
|
||||
else if (td == SignedInteger8Bit) { code = UDPS_TYPECODE_INT8; }
|
||||
else if (td == UnsignedInteger16Bit) { code = UDPS_TYPECODE_UINT16; }
|
||||
else if (td == SignedInteger16Bit) { code = UDPS_TYPECODE_INT16; }
|
||||
else if (td == UnsignedInteger32Bit) { code = UDPS_TYPECODE_UINT32; }
|
||||
else if (td == SignedInteger32Bit) { code = UDPS_TYPECODE_INT32; }
|
||||
else if (td == UnsignedInteger64Bit) { code = UDPS_TYPECODE_UINT64; }
|
||||
else if (td == SignedInteger64Bit) { code = UDPS_TYPECODE_INT64; }
|
||||
else if (td == Float32Bit) { code = UDPS_TYPECODE_FLOAT32; }
|
||||
else if (td == Float64Bit) { code = UDPS_TYPECODE_FLOAT64; }
|
||||
return code;
|
||||
}
|
||||
|
||||
uint16 UDPStreamer::GetPort() const {
|
||||
return port;
|
||||
}
|
||||
|
||||
uint32 UDPStreamer::GetMaxPayloadSize() const {
|
||||
return maxPayloadSize;
|
||||
}
|
||||
|
||||
bool UDPStreamer::IsClientConnected() const {
|
||||
return clientConnected;
|
||||
}
|
||||
|
||||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||||
|
||||
} /* namespace MARTe */
|
||||
@@ -0,0 +1,330 @@
|
||||
/**
|
||||
* @file UDPStreamer.h
|
||||
* @brief Header file for class UDPStreamer
|
||||
* @date 13/05/2026
|
||||
* @author Martino Ferrari
|
||||
*
|
||||
* @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and
|
||||
* the Development of Fusion Energy ('Fusion for Energy').
|
||||
* Licensed under the EUPL, Version 1.1 or - as soon they will be approved
|
||||
* by the European Commission - subsequent versions of the EUPL (the "Licence")
|
||||
* You may not use this work except in compliance with the Licence.
|
||||
* You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl
|
||||
*
|
||||
* @warning Unless required by applicable law or agreed to in writing,
|
||||
* software distributed under the Licence is distributed on an "AS IS"
|
||||
* basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
|
||||
* or implied. See the Licence permissions and limitations under the Licence.
|
||||
*
|
||||
* @details This header file contains the declaration of the class UDPStreamer
|
||||
* with all of its public, protected and private members. It may also include
|
||||
* definitions for inline methods which need to be visible to the compiler.
|
||||
*/
|
||||
|
||||
#ifndef UDPSTREAMER_H_
|
||||
#define UDPSTREAMER_H_
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Standard header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Project header includes */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
#include "BasicUDPSocket.h"
|
||||
#include "CompilerTypes.h"
|
||||
#include "EmbeddedServiceMethodBinderI.h"
|
||||
#include "EventSem.h"
|
||||
#include "FastPollingMutexSem.h"
|
||||
#include "MemoryDataSourceI.h"
|
||||
#include "SingleThreadService.h"
|
||||
#include "StreamString.h"
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
/* Class declaration */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
namespace MARTe {
|
||||
|
||||
/**
|
||||
* @brief Quantization types for float signals.
|
||||
*/
|
||||
typedef enum {
|
||||
UDPStreamerQuantNone = 0u, /**< No quantization; raw wire format */
|
||||
UDPStreamerQuantUint8 = 1u, /**< Quantize to uint8 [0, 255] */
|
||||
UDPStreamerQuantInt8 = 2u, /**< Quantize to int8 [-127, 127] */
|
||||
UDPStreamerQuantUint16 = 3u, /**< Quantize to uint16 [0, 65535] */
|
||||
UDPStreamerQuantInt16 = 4u /**< Quantize to int16 [-32767, 32767] */
|
||||
} UDPStreamerQuantType;
|
||||
|
||||
/**
|
||||
* @brief Time reference modes for signals.
|
||||
*/
|
||||
typedef enum {
|
||||
UDPStreamerTimePacket = 0u, /**< Use the packet-level timestamp (HRT at Synchronise time) */
|
||||
UDPStreamerTimeFullArray = 1u, /**< Time signal has same NumberOfElements; one timestamp per element */
|
||||
UDPStreamerTimeFirstSample = 2u, /**< Time signal is scalar = timestamp of first element */
|
||||
UDPStreamerTimeLastSample = 3u /**< Time signal is scalar = timestamp of last element */
|
||||
} UDPStreamerTimeMode;
|
||||
|
||||
/**
|
||||
* @brief Per-signal metadata used at runtime for serialization.
|
||||
*/
|
||||
struct UDPStreamerSignalInfo {
|
||||
StreamString name; /**< Signal name */
|
||||
TypeDescriptor type; /**< MARTe2 type descriptor */
|
||||
UDPStreamerQuantType quantType; /**< Quantization type (none = raw copy) */
|
||||
uint8 numDimensions; /**< Number of dimensions (0=scalar, 1=array, 2=matrix) */
|
||||
uint32 numElements; /**< Total number of elements (rows * cols) */
|
||||
uint32 numRows; /**< Number of rows */
|
||||
uint32 numCols; /**< Number of columns */
|
||||
float64 rangeMin; /**< Min of physical range (for quantization) */
|
||||
float64 rangeMax; /**< Max of physical range (for quantization) */
|
||||
UDPStreamerTimeMode timeMode; /**< How time is encoded for this signal */
|
||||
float64 samplingRate; /**< Hz, used for First/LastSample modes */
|
||||
uint32 timeSignalIdx; /**< Index of the time-reference signal; 0xFFFFFFFFu = PacketTime */
|
||||
StreamString unit; /**< Physical unit string */
|
||||
uint32 srcByteSize; /**< Bytes in MARTe2 memory */
|
||||
uint32 wireByteSize; /**< Bytes on the wire (may differ when quantized) */
|
||||
uint32 bufferOffset; /**< Byte offset in the flat MemoryDataSourceI memory buffer */
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Magic number for UDPStreamer packets: 'UDPS' in little-endian.
|
||||
*/
|
||||
static const uint32 UDPS_MAGIC = 0x53504455u;
|
||||
|
||||
/**
|
||||
* @brief Packet type codes.
|
||||
*/
|
||||
static const uint8 UDPS_TYPE_DATA = 0u; /**< Server → Client: signal data */
|
||||
static const uint8 UDPS_TYPE_CONFIG = 1u; /**< Server → Client: signal configuration */
|
||||
static const uint8 UDPS_TYPE_ACK = 2u; /**< Client → Server: acknowledge counter */
|
||||
static const uint8 UDPS_TYPE_CONNECT = 3u; /**< Client → Server: connect request */
|
||||
static const uint8 UDPS_TYPE_DISCONNECT = 4u; /**< Client → Server: disconnect */
|
||||
|
||||
/**
|
||||
* @brief Wire packet header (17 bytes, packed).
|
||||
*/
|
||||
struct UDPSPacketHeader {
|
||||
uint32 magic; /**< Must equal UDPS_MAGIC */
|
||||
uint8 type; /**< One of UDPS_TYPE_* */
|
||||
uint32 counter; /**< Sequence counter (per data update, not per fragment) */
|
||||
uint16 fragmentIdx; /**< 0-based index of this fragment */
|
||||
uint16 totalFragments; /**< Total fragments for this update (1 = no fragmentation) */
|
||||
uint32 payloadBytes; /**< Bytes of payload immediately following this header */
|
||||
} __attribute__((packed));
|
||||
|
||||
/**
|
||||
* @brief Signal type codes transmitted in the CONFIG packet.
|
||||
* These are simplified type codes independent of MARTe2 internals.
|
||||
*/
|
||||
static const uint8 UDPS_TYPECODE_UINT8 = 0u;
|
||||
static const uint8 UDPS_TYPECODE_INT8 = 1u;
|
||||
static const uint8 UDPS_TYPECODE_UINT16 = 2u;
|
||||
static const uint8 UDPS_TYPECODE_INT16 = 3u;
|
||||
static const uint8 UDPS_TYPECODE_UINT32 = 4u;
|
||||
static const uint8 UDPS_TYPECODE_INT32 = 5u;
|
||||
static const uint8 UDPS_TYPECODE_UINT64 = 6u;
|
||||
static const uint8 UDPS_TYPECODE_INT64 = 7u;
|
||||
static const uint8 UDPS_TYPECODE_FLOAT32 = 8u;
|
||||
static const uint8 UDPS_TYPECODE_FLOAT64 = 9u;
|
||||
static const uint8 UDPS_TYPECODE_UNKNOWN = 255u;
|
||||
|
||||
/**
|
||||
* @brief A DataSource that streams MARTe2 signals to a single UDP client.
|
||||
*
|
||||
* @details This output DataSource accepts signals from GAMs and forwards them
|
||||
* asynchronously to a connected UDP client. A dedicated background thread handles
|
||||
* all network I/O so that the real-time thread is only blocked for a fast spinlock
|
||||
* and a memcpy during Synchronise().
|
||||
*
|
||||
* Protocol:
|
||||
* - Client sends CONNECT → server replies with CONFIG describing all signals.
|
||||
* - Server sends DATA packets (fragmented if needed) on every RT cycle.
|
||||
* - Client sends ACK packets (optional, for monitoring loss).
|
||||
* - Client sends DISCONNECT to terminate the session.
|
||||
*
|
||||
* Signal configuration syntax:
|
||||
* <pre>
|
||||
* +UDPStreamer1 = {
|
||||
* Class = UDPStreamer
|
||||
* Port = 44500 // Optional (default 44500)
|
||||
* MaxPayloadSize = 1400 // Optional (default 1400); max payload bytes per UDP datagram
|
||||
* CPUMask = 0x2 // Optional, affinity for background thread
|
||||
* StackSize = 1048576 // Optional, stack size for background thread
|
||||
* Signals = {
|
||||
* Time = {
|
||||
* Type = uint64
|
||||
* }
|
||||
* Pressure = {
|
||||
* Type = float32
|
||||
* NumberOfDimensions = 1
|
||||
* NumberOfElements = 100
|
||||
* Unit = "Pa" // Optional
|
||||
* RangeMin = 0.0 // Optional (required for quantization)
|
||||
* RangeMax = 1000000.0 // Optional (required for quantization)
|
||||
* QuantizedType = uint16 // Optional: none|uint8|int8|uint16|int16
|
||||
* TimeMode = LastSample // Optional: PacketTime|FullArray|FirstSample|LastSample
|
||||
* TimeSignal = Time // Required when TimeMode != PacketTime
|
||||
* SamplingRate = 10000.0 // Required when TimeMode = FirstSample or LastSample
|
||||
* }
|
||||
* Temperature = {
|
||||
* Type = float64
|
||||
* Unit = "K"
|
||||
* TimeMode = PacketTime
|
||||
* }
|
||||
* }
|
||||
* }
|
||||
* </pre>
|
||||
*
|
||||
* Notes:
|
||||
* - QuantizedType is only valid for float32/float64 signals.
|
||||
* - TimeMode = PacketTime uses the HRT counter captured in Synchronise().
|
||||
* - TimeMode = FullArray requires TimeSignal to have the same NumberOfElements.
|
||||
* - TimeMode = FirstSample/LastSample requires a scalar TimeSignal and SamplingRate.
|
||||
*/
|
||||
class UDPStreamer : public MemoryDataSourceI, public EmbeddedServiceMethodBinderI {
|
||||
public:
|
||||
CLASS_REGISTER_DECLARATION()
|
||||
|
||||
/**
|
||||
* @brief Constructor. Initialises all members to safe defaults.
|
||||
*/
|
||||
UDPStreamer();
|
||||
|
||||
/**
|
||||
* @brief Destructor. Stops the background thread and frees allocated memory.
|
||||
*/
|
||||
virtual ~UDPStreamer();
|
||||
|
||||
/**
|
||||
* @brief Parses top-level configuration parameters (Port, MaxPayloadSize, CPUMask, StackSize).
|
||||
* @return true if all mandatory parameters are valid.
|
||||
*/
|
||||
virtual bool Initialise(StructuredDataI &data);
|
||||
|
||||
/**
|
||||
* @brief Validates signal types and builds the per-signal metadata array.
|
||||
* @details Reads per-signal custom fields (Unit, RangeMin, RangeMax, QuantizedType,
|
||||
* TimeMode, TimeSignal, SamplingRate) from signalsDatabase and populates signalInfos[].
|
||||
* @return true if all signal configurations are valid.
|
||||
*/
|
||||
virtual bool SetConfiguredDatabase(StructuredDataI &data);
|
||||
|
||||
/**
|
||||
* @brief Allocates signal memory (via parent) plus readyBuffer, scratchBuffer, wireBuffer.
|
||||
* @return true if all memory allocations succeed.
|
||||
*/
|
||||
virtual bool AllocateMemory();
|
||||
|
||||
/**
|
||||
* @brief Returns the broker name for the given signal direction.
|
||||
* @return "MemoryMapSynchronisedOutputBroker" for OutputSignals; "" otherwise.
|
||||
*/
|
||||
virtual const char8 *GetBrokerName(StructuredDataI &data,
|
||||
const SignalDirection direction);
|
||||
|
||||
/**
|
||||
* @brief Called by the RT thread after all GAMs have written output signals.
|
||||
* @details Copies signal memory to readyBuffer under a fast spinlock, then posts
|
||||
* the dataSem to wake the background thread. Returns immediately.
|
||||
* @return true always.
|
||||
*/
|
||||
virtual bool Synchronise();
|
||||
|
||||
/**
|
||||
* @brief Opens the server socket and starts the background thread.
|
||||
* @return true if the socket and thread start successfully.
|
||||
*/
|
||||
virtual bool PrepareNextState(const char8 *const currentStateName,
|
||||
const char8 *const nextStateName);
|
||||
|
||||
/**
|
||||
* @brief Background thread entry point.
|
||||
* @details Handles client CONNECT/DISCONNECT/ACK and sends DATA packets.
|
||||
*/
|
||||
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
|
||||
|
||||
/**
|
||||
* @brief Returns the listening port number.
|
||||
*/
|
||||
uint16 GetPort() const;
|
||||
|
||||
/**
|
||||
* @brief Returns the maximum payload size per UDP datagram.
|
||||
*/
|
||||
uint32 GetMaxPayloadSize() const;
|
||||
|
||||
/**
|
||||
* @brief Returns true if a client is currently connected.
|
||||
*/
|
||||
bool IsClientConnected() const;
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Serializes the CONFIG payload into buf and sets payloadSize.
|
||||
* @return true if serialization succeeds.
|
||||
*/
|
||||
bool BuildConfigPayload(uint8 *buf, uint32 bufSize, uint32 &payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Quantizes and serializes all signals from srcBuf into wireBuffer.
|
||||
* @param[in] srcBuf Flat source buffer (signal data in MARTe2 format).
|
||||
* @param[in] timestamp HRT counter to embed as packet timestamp.
|
||||
*/
|
||||
void QuantizeAndSerialize(const uint8 *srcBuf, uint64 timestamp);
|
||||
|
||||
/**
|
||||
* @brief Sends payload as one or more fragmented UDP datagrams to the connected client.
|
||||
* @return true if all datagrams were sent without error.
|
||||
*/
|
||||
bool SendFragmented(uint8 type, uint32 counter, const uint8 *payload, uint32 payloadSize);
|
||||
|
||||
/**
|
||||
* @brief Handles a single received command packet from a client.
|
||||
*/
|
||||
void HandleClientCommand(const uint8 *buf, uint32 size);
|
||||
|
||||
/**
|
||||
* @brief Maps a MARTe2 TypeDescriptor to the UDPS_TYPECODE_* constants.
|
||||
*/
|
||||
static uint8 TypeDescriptorToCode(TypeDescriptor td);
|
||||
|
||||
/* Configuration parameters */
|
||||
uint16 port; /**< UDP server port */
|
||||
uint32 maxPayloadSize; /**< Max payload bytes per UDP packet (excluding header) */
|
||||
uint32 cpuMask; /**< Background thread CPU affinity */
|
||||
uint32 stackSize; /**< Background thread stack size */
|
||||
|
||||
/* Signal metadata */
|
||||
uint32 numSigs; /**< Number of signals */
|
||||
UDPStreamerSignalInfo *signalInfos; /**< Per-signal metadata array */
|
||||
|
||||
/* Double-buffering for RT safety */
|
||||
uint8 *readyBuffer; /**< Protected copy of signal memory for background thread */
|
||||
uint8 *scratchBuffer; /**< Local copy used during serialization (avoids holding lock) */
|
||||
uint32 totalSrcBytes; /**< Total bytes of signal data (= stateMemorySize) */
|
||||
FastPollingMutexSem bufMutex; /**< Protects readyBuffer against concurrent access */
|
||||
EventSem dataSem; /**< Wakes background thread when new data is ready */
|
||||
volatile uint64 syncTimestamp; /**< HRT counter captured in Synchronise() */
|
||||
|
||||
/* Wire serialization buffer */
|
||||
uint8 *wireBuffer; /**< Pre-allocated buffer for the serialized DATA payload */
|
||||
uint32 totalWireBytes; /**< Total bytes of all signals after quantization + 8-byte timestamp prefix */
|
||||
|
||||
/* Networking */
|
||||
BasicUDPSocket serverSocket; /**< Bound to port; receives client commands */
|
||||
BasicUDPSocket clientSocket; /**< Configured to send to the connected client */
|
||||
volatile bool clientConnected; /**< True when a client has successfully connected */
|
||||
|
||||
/* Thread management */
|
||||
SingleThreadService executor; /**< Background thread service */
|
||||
|
||||
/* Packet sequencing */
|
||||
uint32 packetCounter; /**< Incremented for each DATA update sent */
|
||||
};
|
||||
|
||||
} /* namespace MARTe */
|
||||
|
||||
#endif /* UDPSTREAMER_H_ */
|
||||
Reference in New Issue
Block a user