Merge branch 'master' into gaisser_fixes_fixedMap
This commit is contained in:
commit
976fd54f5e
42
container/DynamicFIFO.h
Normal file
42
container/DynamicFIFO.h
Normal file
@ -0,0 +1,42 @@
|
||||
#ifndef FSFW_CONTAINER_DYNAMICFIFO_H_
|
||||
#define FSFW_CONTAINER_DYNAMICFIFO_H_
|
||||
|
||||
#include "FIFOBase.h"
|
||||
#include <vector>
|
||||
|
||||
/**
|
||||
* @brief Simple First-In-First-Out data structure. The maximum size
|
||||
* can be set in the constructor.
|
||||
* @details
|
||||
* The maximum capacity can be determined at run-time, so this container
|
||||
* performs dynamic memory allocation!
|
||||
* The public interface of FIFOBase exposes the user interface for the FIFO.
|
||||
* @tparam T Entry Type
|
||||
* @tparam capacity Maximum capacity
|
||||
*/
|
||||
template<typename T>
|
||||
class DynamicFIFO: public FIFOBase<T> {
|
||||
public:
|
||||
DynamicFIFO(size_t maxCapacity): FIFOBase<T>(nullptr, maxCapacity),
|
||||
fifoVector(maxCapacity) {
|
||||
// trying to pass the pointer of the uninitialized vector
|
||||
// to the FIFOBase constructor directly lead to a super evil bug.
|
||||
// So we do it like this now.
|
||||
this->setContainer(fifoVector.data());
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Custom copy constructor which prevents setting the
|
||||
* underlying pointer wrong.
|
||||
*/
|
||||
DynamicFIFO(const DynamicFIFO& other): FIFOBase<T>(other),
|
||||
fifoVector(other.maxCapacity) {
|
||||
this->setContainer(fifoVector.data());
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
std::vector<T> fifoVector;
|
||||
};
|
||||
|
||||
#endif /* FSFW_CONTAINER_DYNAMICFIFO_H_ */
|
@ -1,82 +1,35 @@
|
||||
#ifndef FIFO_H_
|
||||
#define FIFO_H_
|
||||
#ifndef FSFW_CONTAINER_FIFO_H_
|
||||
#define FSFW_CONTAINER_FIFO_H_
|
||||
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include "FIFOBase.h"
|
||||
#include <array>
|
||||
|
||||
/**
|
||||
* @brief Simple First-In-First-Out data structure
|
||||
* @brief Simple First-In-First-Out data structure with size fixed at
|
||||
* compile time
|
||||
* @details
|
||||
* Performs no dynamic memory allocation.
|
||||
* The public interface of FIFOBase exposes the user interface for the FIFO.
|
||||
* @tparam T Entry Type
|
||||
* @tparam capacity Maximum capacity
|
||||
*/
|
||||
template<typename T, uint8_t capacity>
|
||||
class FIFO {
|
||||
private:
|
||||
uint8_t readIndex, writeIndex, currentSize;
|
||||
T data[capacity];
|
||||
|
||||
uint8_t next(uint8_t current) {
|
||||
++current;
|
||||
if (current == capacity) {
|
||||
current = 0;
|
||||
}
|
||||
return current;
|
||||
}
|
||||
template<typename T, size_t capacity>
|
||||
class FIFO: public FIFOBase<T> {
|
||||
public:
|
||||
FIFO() :
|
||||
readIndex(0), writeIndex(0), currentSize(0) {
|
||||
FIFO(): FIFOBase<T>(nullptr, capacity) {
|
||||
this->setContainer(fifoArray.data());
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Custom copy constructor to set pointer correctly.
|
||||
* @param other
|
||||
*/
|
||||
FIFO(const FIFO& other): FIFOBase<T>(other) {
|
||||
this->setContainer(fifoArray.data());
|
||||
}
|
||||
|
||||
bool empty() {
|
||||
return (currentSize == 0);
|
||||
}
|
||||
|
||||
bool full() {
|
||||
return (currentSize == capacity);
|
||||
}
|
||||
|
||||
uint8_t size(){
|
||||
return currentSize;
|
||||
}
|
||||
|
||||
ReturnValue_t insert(T value) {
|
||||
if (full()) {
|
||||
return FULL;
|
||||
} else {
|
||||
data[writeIndex] = value;
|
||||
writeIndex = next(writeIndex);
|
||||
++currentSize;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t retrieve(T *value) {
|
||||
if (empty()) {
|
||||
return EMPTY;
|
||||
} else {
|
||||
*value = data[readIndex];
|
||||
readIndex = next(readIndex);
|
||||
--currentSize;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t peek(T * value) {
|
||||
if(empty()) {
|
||||
return EMPTY;
|
||||
} else {
|
||||
*value = data[readIndex];
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t pop() {
|
||||
T value;
|
||||
return this->retrieve(&value);
|
||||
}
|
||||
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS;
|
||||
static const ReturnValue_t FULL = MAKE_RETURN_CODE(1);
|
||||
static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2);
|
||||
private:
|
||||
std::array<T, capacity> fifoArray;
|
||||
};
|
||||
|
||||
#endif /* FIFO_H_ */
|
||||
#endif /* FSFW_CONTAINER_FIFO_H_ */
|
||||
|
65
container/FIFOBase.h
Normal file
65
container/FIFOBase.h
Normal file
@ -0,0 +1,65 @@
|
||||
#ifndef FSFW_CONTAINER_FIFOBASE_H_
|
||||
#define FSFW_CONTAINER_FIFOBASE_H_
|
||||
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include <cstddef>
|
||||
#include <cstring>
|
||||
|
||||
template <typename T>
|
||||
class FIFOBase {
|
||||
public:
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS;
|
||||
static const ReturnValue_t FULL = MAKE_RETURN_CODE(1);
|
||||
static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2);
|
||||
|
||||
/** Default ctor, takes pointer to first entry of underlying container
|
||||
* and maximum capacity */
|
||||
FIFOBase(T* values, const size_t maxCapacity);
|
||||
|
||||
/**
|
||||
* Insert value into FIFO
|
||||
* @param value
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t insert(T value);
|
||||
/**
|
||||
* Retrieve item from FIFO. This removes the item from the FIFO.
|
||||
* @param value
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t retrieve(T *value);
|
||||
/**
|
||||
* Retrieve item from FIFO without removing it from FIFO.
|
||||
* @param value
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t peek(T * value);
|
||||
/**
|
||||
* Remove item from FIFO.
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t pop();
|
||||
|
||||
bool empty();
|
||||
bool full();
|
||||
size_t size();
|
||||
|
||||
|
||||
size_t getMaxCapacity() const;
|
||||
|
||||
protected:
|
||||
void setContainer(T* data);
|
||||
size_t maxCapacity = 0;
|
||||
|
||||
T* values;
|
||||
|
||||
size_t readIndex = 0;
|
||||
size_t writeIndex = 0;
|
||||
size_t currentSize = 0;
|
||||
|
||||
size_t next(size_t current);
|
||||
};
|
||||
|
||||
#include "FIFOBase.tpp"
|
||||
|
||||
#endif /* FSFW_CONTAINER_FIFOBASE_H_ */
|
87
container/FIFOBase.tpp
Normal file
87
container/FIFOBase.tpp
Normal file
@ -0,0 +1,87 @@
|
||||
#ifndef FSFW_CONTAINER_FIFOBASE_TPP_
|
||||
#define FSFW_CONTAINER_FIFOBASE_TPP_
|
||||
|
||||
#ifndef FSFW_CONTAINER_FIFOBASE_H_
|
||||
#error Include FIFOBase.h before FIFOBase.tpp!
|
||||
#endif
|
||||
|
||||
template<typename T>
|
||||
inline FIFOBase<T>::FIFOBase(T* values, const size_t maxCapacity):
|
||||
maxCapacity(maxCapacity), values(values){};
|
||||
|
||||
template<typename T>
|
||||
inline ReturnValue_t FIFOBase<T>::insert(T value) {
|
||||
if (full()) {
|
||||
return FULL;
|
||||
} else {
|
||||
values[writeIndex] = value;
|
||||
writeIndex = next(writeIndex);
|
||||
++currentSize;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
inline ReturnValue_t FIFOBase<T>::retrieve(T* value) {
|
||||
if (empty()) {
|
||||
return EMPTY;
|
||||
} else {
|
||||
*value = values[readIndex];
|
||||
readIndex = next(readIndex);
|
||||
--currentSize;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
inline ReturnValue_t FIFOBase<T>::peek(T* value) {
|
||||
if(empty()) {
|
||||
return EMPTY;
|
||||
} else {
|
||||
*value = values[readIndex];
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
inline ReturnValue_t FIFOBase<T>::pop() {
|
||||
T value;
|
||||
return this->retrieve(&value);
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
inline bool FIFOBase<T>::empty() {
|
||||
return (currentSize == 0);
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
inline bool FIFOBase<T>::full() {
|
||||
return (currentSize == maxCapacity);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline size_t FIFOBase<T>::size() {
|
||||
return currentSize;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline size_t FIFOBase<T>::next(size_t current) {
|
||||
++current;
|
||||
if (current == maxCapacity) {
|
||||
current = 0;
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline size_t FIFOBase<T>::getMaxCapacity() const {
|
||||
return maxCapacity;
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
inline void FIFOBase<T>::setContainer(T *data) {
|
||||
this->values = data;
|
||||
}
|
||||
|
||||
#endif
|
@ -1,18 +1,23 @@
|
||||
#include "SimpleRingBuffer.h"
|
||||
#include <string.h>
|
||||
|
||||
SimpleRingBuffer::SimpleRingBuffer(uint32_t size, bool overwriteOld) :
|
||||
RingBufferBase<>(0, size, overwriteOld), buffer(NULL) {
|
||||
SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld) :
|
||||
RingBufferBase<>(0, size, overwriteOld) {
|
||||
buffer = new uint8_t[size];
|
||||
}
|
||||
|
||||
SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size,
|
||||
bool overwriteOld):
|
||||
RingBufferBase<>(0, size, overwriteOld), buffer(buffer) {}
|
||||
|
||||
|
||||
SimpleRingBuffer::~SimpleRingBuffer() {
|
||||
delete[] buffer;
|
||||
}
|
||||
|
||||
ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data,
|
||||
uint32_t amount) {
|
||||
if (availableWriteSpace() >= amount || overwriteOld) {
|
||||
if (availableWriteSpace() >= amount or overwriteOld) {
|
||||
uint32_t amountTillWrap = writeTillWrap();
|
||||
if (amountTillWrap >= amount) {
|
||||
memcpy(&buffer[write], data, amount);
|
||||
@ -38,7 +43,7 @@ ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, uint32_t amount,
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
if (trueAmount != NULL) {
|
||||
if (trueAmount != nullptr) {
|
||||
*trueAmount = amount;
|
||||
}
|
||||
if (amountTillWrap >= amount) {
|
||||
@ -60,9 +65,10 @@ ReturnValue_t SimpleRingBuffer::deleteData(uint32_t amount,
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
if (trueAmount != NULL) {
|
||||
if (trueAmount != nullptr) {
|
||||
*trueAmount = amount;
|
||||
}
|
||||
incrementRead(amount, READ_PTR);
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
|
@ -4,17 +4,64 @@
|
||||
#include "RingBufferBase.h"
|
||||
#include <stddef.h>
|
||||
|
||||
/**
|
||||
* @brief Circular buffer implementation, useful for buffering
|
||||
* into data streams.
|
||||
* @details
|
||||
* Note that the deleteData() has to be called to increment the read pointer.
|
||||
* This class allocated dynamically, so
|
||||
* @ingroup containers
|
||||
*/
|
||||
class SimpleRingBuffer: public RingBufferBase<> {
|
||||
public:
|
||||
SimpleRingBuffer(uint32_t size, bool overwriteOld);
|
||||
/**
|
||||
* This constructor allocates a new internal buffer with the supplied size.
|
||||
* @param size
|
||||
* @param overwriteOld
|
||||
*/
|
||||
SimpleRingBuffer(const size_t size, bool overwriteOld);
|
||||
/**
|
||||
* This constructor takes an external buffer with the specified size.
|
||||
* @param buffer
|
||||
* @param size
|
||||
* @param overwriteOld
|
||||
*/
|
||||
SimpleRingBuffer(uint8_t* buffer, const size_t size, bool overwriteOld);
|
||||
|
||||
virtual ~SimpleRingBuffer();
|
||||
|
||||
/**
|
||||
* Write to circular buffer and increment write pointer by amount
|
||||
* @param data
|
||||
* @param amount
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t writeData(const uint8_t* data, uint32_t amount);
|
||||
ReturnValue_t readData(uint8_t* data, uint32_t amount, bool readRemaining = false, uint32_t* trueAmount = NULL);
|
||||
ReturnValue_t deleteData(uint32_t amount, bool deleteRemaining = false, uint32_t* trueAmount = NULL);
|
||||
|
||||
/**
|
||||
* Read from circular buffer at read pointer
|
||||
* @param data
|
||||
* @param amount
|
||||
* @param readRemaining
|
||||
* @param trueAmount
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t readData(uint8_t* data, uint32_t amount,
|
||||
bool readRemaining = false, uint32_t* trueAmount = nullptr);
|
||||
|
||||
/**
|
||||
* Delete data starting by incrementing read pointer
|
||||
* @param amount
|
||||
* @param deleteRemaining
|
||||
* @param trueAmount
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t deleteData(uint32_t amount, bool deleteRemaining = false,
|
||||
uint32_t* trueAmount = nullptr);
|
||||
private:
|
||||
// static const uint8_t TEMP_READ_PTR = 1;
|
||||
static const uint8_t READ_PTR = 0;
|
||||
uint8_t* buffer;
|
||||
uint8_t* buffer = nullptr;
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */
|
||||
|
@ -8,13 +8,16 @@
|
||||
const uint16_t EventManager::POOL_SIZES[N_POOLS] = {
|
||||
sizeof(EventMatchTree::Node), sizeof(EventIdRangeMatcher),
|
||||
sizeof(ReporterRangeMatcher) };
|
||||
//If one checks registerListener calls, there are around 40 (to max 50) objects registering for certain events.
|
||||
//Each listener requires 1 or 2 EventIdMatcher and 1 or 2 ReportRangeMatcher. So a good guess is 75 to a max of 100 pools required for each, which fits well.
|
||||
// If one checks registerListener calls, there are around 40 (to max 50)
|
||||
// objects registering for certain events.
|
||||
// Each listener requires 1 or 2 EventIdMatcher and 1 or 2 ReportRangeMatcher.
|
||||
// So a good guess is 75 to a max of 100 pools required for each, which fits well.
|
||||
// SHOULDDO: Shouldn't this be in the config folder and passed via ctor?
|
||||
const uint16_t EventManager::N_ELEMENTS[N_POOLS] = { 240, 120, 120 };
|
||||
|
||||
EventManager::EventManager(object_id_t setObjectId) :
|
||||
SystemObject(setObjectId), eventReportQueue(NULL), mutex(NULL), factoryBackend(
|
||||
0, POOL_SIZES, N_ELEMENTS, false, true) {
|
||||
SystemObject(setObjectId),
|
||||
factoryBackend(0, POOL_SIZES, N_ELEMENTS, false, true) {
|
||||
mutex = MutexFactory::instance()->createMutex();
|
||||
eventReportQueue = QueueFactory::instance()->createMessageQueue(
|
||||
MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE);
|
||||
@ -108,41 +111,45 @@ ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener,
|
||||
|
||||
#ifdef DEBUG
|
||||
|
||||
//forward declaration, should be implemented by mission
|
||||
const char* translateObject(object_id_t object);
|
||||
const char * translateEvents(Event event);
|
||||
|
||||
void EventManager::printEvent(EventMessage* message) {
|
||||
const char *string = 0;
|
||||
switch (message->getSeverity()) {
|
||||
case SEVERITY::INFO:
|
||||
// string = translateObject(message->getReporter());
|
||||
// sif::info << "EVENT: ";
|
||||
// if (string != 0) {
|
||||
// sif::info << string;
|
||||
// } else {
|
||||
// sif::info << "0x" << std::hex << message->getReporter() << std::dec;
|
||||
// }
|
||||
// sif::info << " reported " << translateEvents(message->getEvent()) << " ("
|
||||
// << std::dec << message->getEventId() << std::hex << ") P1: 0x"
|
||||
// << message->getParameter1() << " P2: 0x"
|
||||
// << message->getParameter2() << std::dec << std::endl;
|
||||
break;
|
||||
default:
|
||||
#ifdef DEBUG_INFO_EVENT
|
||||
string = translateObject(message->getReporter());
|
||||
sif::error << "EVENT: ";
|
||||
sif::info << "EVENT: ";
|
||||
if (string != 0) {
|
||||
sif::error << string;
|
||||
sif::info << string;
|
||||
} else {
|
||||
sif::error << "0x" << std::hex << message->getReporter() << std::dec;
|
||||
sif::info << "0x" << std::hex << message->getReporter() << std::dec;
|
||||
}
|
||||
sif::error << " reported " << translateEvents(message->getEvent()) << " ("
|
||||
sif::info << " reported " << translateEvents(message->getEvent()) << " ("
|
||||
<< std::dec << message->getEventId() << std::hex << ") P1: 0x"
|
||||
<< message->getParameter1() << " P2: 0x"
|
||||
<< message->getParameter2() << std::dec << std::endl;
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
string = translateObject(message->getReporter());
|
||||
sif::debug << "EventManager: ";
|
||||
if (string != 0) {
|
||||
sif::debug << string;
|
||||
}
|
||||
else {
|
||||
sif::debug << "0x" << std::hex << message->getReporter() << std::dec;
|
||||
}
|
||||
sif::debug << " reported " << translateEvents(message->getEvent())
|
||||
<< " (" << std::dec << message->getEventId() << ") "
|
||||
<< std::endl;
|
||||
|
||||
sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1()
|
||||
<< ", P1 Dec: " << std::dec << message->getParameter1()
|
||||
<< std::endl;
|
||||
sif::debug << std::hex << "P2 Hex: 0x" << message->getParameter2()
|
||||
<< ", P2 Dec: " << std::dec << message->getParameter2()
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -10,6 +10,12 @@
|
||||
#include "../ipc/MutexIF.h"
|
||||
#include <map>
|
||||
|
||||
#ifdef DEBUG
|
||||
// forward declaration, should be implemented by mission
|
||||
extern const char* translateObject(object_id_t object);
|
||||
extern const char* translateEvents(Event event);
|
||||
#endif
|
||||
|
||||
class EventManager: public EventManagerIF,
|
||||
public ExecutableObjectIF,
|
||||
public SystemObject {
|
||||
@ -36,11 +42,11 @@ public:
|
||||
ReturnValue_t performOperation(uint8_t opCode);
|
||||
protected:
|
||||
|
||||
MessageQueueIF* eventReportQueue;
|
||||
MessageQueueIF* eventReportQueue = nullptr;
|
||||
|
||||
std::map<MessageQueueId_t, EventMatchTree> listenerList;
|
||||
|
||||
MutexIF* mutex;
|
||||
MutexIF* mutex = nullptr;
|
||||
|
||||
static const uint8_t N_POOLS = 3;
|
||||
LocalPool<N_POOLS> factoryBackend;
|
||||
|
@ -1,92 +1,99 @@
|
||||
#include "timevalOperations.h"
|
||||
|
||||
timeval& operator+=(timeval& lhs, const timeval& rhs) {
|
||||
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
sum += rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
lhs.tv_sec = sum / 1000000;
|
||||
lhs.tv_usec = sum - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator+(timeval lhs, const timeval& rhs) {
|
||||
lhs += rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval& operator-=(timeval& lhs, const timeval& rhs) {
|
||||
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
sum -= rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
lhs.tv_sec = sum / 1000000;
|
||||
lhs.tv_usec = sum - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator-(timeval lhs, const timeval& rhs) {
|
||||
lhs -= rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
double operator/(const timeval& lhs, const timeval& rhs) {
|
||||
double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 / rhs64;
|
||||
}
|
||||
|
||||
timeval& operator/=(timeval& lhs, double scalar) {
|
||||
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
product /= scalar;
|
||||
lhs.tv_sec = product / 1000000;
|
||||
lhs.tv_usec = product - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator/(timeval lhs, double scalar) {
|
||||
lhs /= scalar;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval& operator*=(timeval& lhs, double scalar) {
|
||||
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
product *= scalar;
|
||||
lhs.tv_sec = product / 1000000;
|
||||
lhs.tv_usec = product - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator*(timeval lhs, double scalar) {
|
||||
lhs *= scalar;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator*(double scalar, timeval rhs) {
|
||||
rhs *= scalar;
|
||||
return rhs;
|
||||
}
|
||||
|
||||
bool operator==(const timeval& lhs, const timeval& rhs) {
|
||||
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 == rhs64;
|
||||
}
|
||||
bool operator!=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator==(lhs, rhs);
|
||||
}
|
||||
bool operator<(const timeval& lhs, const timeval& rhs) {
|
||||
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 < rhs64;
|
||||
}
|
||||
bool operator>(const timeval& lhs, const timeval& rhs) {
|
||||
return operator<(rhs, lhs);
|
||||
}
|
||||
bool operator<=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator>(lhs, rhs);
|
||||
}
|
||||
bool operator>=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator<(lhs, rhs);
|
||||
}
|
||||
|
||||
double timevalOperations::toDouble(const timeval timeval) {
|
||||
double result = timeval.tv_sec * 1000000. + timeval.tv_usec;
|
||||
return result / 1000000.;
|
||||
}
|
||||
#include "timevalOperations.h"
|
||||
|
||||
timeval& operator+=(timeval& lhs, const timeval& rhs) {
|
||||
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
sum += rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
lhs.tv_sec = sum / 1000000;
|
||||
lhs.tv_usec = sum - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator+(timeval lhs, const timeval& rhs) {
|
||||
lhs += rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval& operator-=(timeval& lhs, const timeval& rhs) {
|
||||
int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
sum -= rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
lhs.tv_sec = sum / 1000000;
|
||||
lhs.tv_usec = sum - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator-(timeval lhs, const timeval& rhs) {
|
||||
lhs -= rhs;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
double operator/(const timeval& lhs, const timeval& rhs) {
|
||||
double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 / rhs64;
|
||||
}
|
||||
|
||||
timeval& operator/=(timeval& lhs, double scalar) {
|
||||
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
product /= scalar;
|
||||
lhs.tv_sec = product / 1000000;
|
||||
lhs.tv_usec = product - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator/(timeval lhs, double scalar) {
|
||||
lhs /= scalar;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval& operator*=(timeval& lhs, double scalar) {
|
||||
int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
product *= scalar;
|
||||
lhs.tv_sec = product / 1000000;
|
||||
lhs.tv_usec = product - lhs.tv_sec * 1000000;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator*(timeval lhs, double scalar) {
|
||||
lhs *= scalar;
|
||||
return lhs;
|
||||
}
|
||||
|
||||
timeval operator*(double scalar, timeval rhs) {
|
||||
rhs *= scalar;
|
||||
return rhs;
|
||||
}
|
||||
|
||||
bool operator==(const timeval& lhs, const timeval& rhs) {
|
||||
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 == rhs64;
|
||||
}
|
||||
bool operator!=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator==(lhs, rhs);
|
||||
}
|
||||
bool operator<(const timeval& lhs, const timeval& rhs) {
|
||||
int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec;
|
||||
int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec;
|
||||
return lhs64 < rhs64;
|
||||
}
|
||||
bool operator>(const timeval& lhs, const timeval& rhs) {
|
||||
return operator<(rhs, lhs);
|
||||
}
|
||||
bool operator<=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator>(lhs, rhs);
|
||||
}
|
||||
bool operator>=(const timeval& lhs, const timeval& rhs) {
|
||||
return !operator<(lhs, rhs);
|
||||
}
|
||||
|
||||
double timevalOperations::toDouble(const timeval timeval) {
|
||||
double result = timeval.tv_sec * 1000000. + timeval.tv_usec;
|
||||
return result / 1000000.;
|
||||
}
|
||||
|
||||
timeval timevalOperations::toTimeval(const double seconds) {
|
||||
timeval tval;
|
||||
tval.tv_sec = seconds;
|
||||
tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6);
|
||||
return tval;
|
||||
}
|
||||
|
@ -41,6 +41,7 @@ namespace timevalOperations {
|
||||
* @return seconds
|
||||
*/
|
||||
double toDouble(const timeval timeval);
|
||||
timeval toTimeval(const double seconds);
|
||||
}
|
||||
|
||||
#endif /* TIMEVALOPERATIONS_H_ */
|
||||
|
138
osal/linux/TcUnixUdpPollingTask.cpp
Normal file
138
osal/linux/TcUnixUdpPollingTask.cpp
Normal file
@ -0,0 +1,138 @@
|
||||
#include "TcUnixUdpPollingTask.h"
|
||||
#include "../../globalfunctions/arrayprinter.h"
|
||||
|
||||
TcUnixUdpPollingTask::TcUnixUdpPollingTask(object_id_t objectId,
|
||||
object_id_t tmtcUnixUdpBridge, size_t frameSize,
|
||||
double timeoutSeconds): SystemObject(objectId),
|
||||
tmtcBridgeId(tmtcUnixUdpBridge) {
|
||||
|
||||
if(frameSize > 0) {
|
||||
this->frameSize = frameSize;
|
||||
}
|
||||
else {
|
||||
this->frameSize = DEFAULT_MAX_FRAME_SIZE;
|
||||
}
|
||||
|
||||
// Set up reception buffer with specified frame size.
|
||||
// For now, it is assumed that only one frame is held in the buffer!
|
||||
receptionBuffer.reserve(this->frameSize);
|
||||
receptionBuffer.resize(this->frameSize);
|
||||
|
||||
if(timeoutSeconds == -1) {
|
||||
receptionTimeout = DEFAULT_TIMEOUT;
|
||||
}
|
||||
else {
|
||||
receptionTimeout = timevalOperations::toTimeval(timeoutSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
TcUnixUdpPollingTask::~TcUnixUdpPollingTask() {}
|
||||
|
||||
ReturnValue_t TcUnixUdpPollingTask::performOperation(uint8_t opCode) {
|
||||
// Poll for new UDP datagrams in permanent loop.
|
||||
while(1) {
|
||||
//! Sender Address is cached here.
|
||||
struct sockaddr_in senderAddress;
|
||||
socklen_t senderSockLen = 0;
|
||||
ssize_t bytesReceived = recvfrom(serverUdpSocket,
|
||||
receptionBuffer.data(), frameSize, receptionFlags,
|
||||
reinterpret_cast<sockaddr*>(&senderAddress), &senderSockLen);
|
||||
if(bytesReceived < 0) {
|
||||
// handle error
|
||||
sif::error << "TcSocketPollingTask::performOperation: Reception"
|
||||
"error." << std::endl;
|
||||
handleReadError();
|
||||
|
||||
continue;
|
||||
}
|
||||
// sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived
|
||||
// << " bytes received" << std::endl;
|
||||
|
||||
ReturnValue_t result = handleSuccessfullTcRead(bytesReceived);
|
||||
if(result != HasReturnvaluesIF::RETURN_FAILED) {
|
||||
|
||||
}
|
||||
tmtcBridge->registerCommConnect();
|
||||
tmtcBridge->checkAndSetClientAddress(senderAddress);
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t TcUnixUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) {
|
||||
store_address_t storeId;
|
||||
ReturnValue_t result = tcStore->addData(&storeId,
|
||||
receptionBuffer.data(), bytesRead);
|
||||
// arrayprinter::print(receptionBuffer.data(), bytesRead);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data "
|
||||
"storage failed" << std::endl;
|
||||
sif::error << "Packet size: " << bytesRead << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
|
||||
TmTcMessage message(storeId);
|
||||
|
||||
result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "Serial Polling: Sending message to queue failed"
|
||||
<< std::endl;
|
||||
tcStore->deleteData(storeId);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
ReturnValue_t TcUnixUdpPollingTask::initialize() {
|
||||
tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
|
||||
if (tcStore == nullptr) {
|
||||
sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!"
|
||||
<< std::endl;
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
tmtcBridge = objectManager->get<TmTcUnixUdpBridge>(tmtcBridgeId);
|
||||
if(tmtcBridge == nullptr) {
|
||||
sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid"
|
||||
" TMTC bridge object!" << std::endl;
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
serverUdpSocket = tmtcBridge->serverSocket;
|
||||
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t TcUnixUdpPollingTask::initializeAfterTaskCreation() {
|
||||
// Initialize the destination after task creation. This ensures
|
||||
// that the destination will be set in the TMTC bridge.
|
||||
targetTcDestination = tmtcBridge->getRequestQueue();
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void TcUnixUdpPollingTask::setTimeout(double timeoutSeconds) {
|
||||
timeval tval;
|
||||
tval = timevalOperations::toTimeval(timeoutSeconds);
|
||||
int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO,
|
||||
&tval, sizeof(receptionTimeout));
|
||||
if(result == -1) {
|
||||
sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting "
|
||||
"receive timeout failed with " << strerror(errno) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: sleep after error detection to prevent spam
|
||||
void TcUnixUdpPollingTask::handleReadError() {
|
||||
switch(errno) {
|
||||
case(EAGAIN): {
|
||||
// todo: When working in timeout mode, this will occur more often
|
||||
// and is not an error.
|
||||
sif::error << "TcUnixUdpPollingTask::handleReadError: Timeout."
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
sif::error << "TcUnixUdpPollingTask::handleReadError: "
|
||||
<< strerror(errno) << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
67
osal/linux/TcUnixUdpPollingTask.h
Normal file
67
osal/linux/TcUnixUdpPollingTask.h
Normal file
@ -0,0 +1,67 @@
|
||||
#ifndef FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
|
||||
#define FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_
|
||||
|
||||
#include "../../objectmanager/SystemObject.h"
|
||||
#include "../../osal/linux/TmTcUnixUdpBridge.h"
|
||||
#include "../../tasks/ExecutableObjectIF.h"
|
||||
|
||||
#include <sys/socket.h>
|
||||
#include <vector>
|
||||
|
||||
/**
|
||||
* @brief This class can be used to implement the polling of a Unix socket,
|
||||
* using UDP for now.
|
||||
* @details
|
||||
* The task will be blocked while the specified number of bytes has not been
|
||||
* received, so TC reception is handled inside a separate task.
|
||||
* This class caches the IP address of the sender. It is assumed there
|
||||
* is only one sender for now.
|
||||
*/
|
||||
class TcUnixUdpPollingTask: public SystemObject,
|
||||
public ExecutableObjectIF {
|
||||
friend class TmTcUnixUdpBridge;
|
||||
public:
|
||||
static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048;
|
||||
//! 0.5 default milliseconds timeout for now.
|
||||
static constexpr timeval DEFAULT_TIMEOUT = {.tv_sec = 0, .tv_usec = 500};
|
||||
|
||||
TcUnixUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge,
|
||||
size_t frameSize = 0, double timeoutSeconds = -1);
|
||||
virtual~ TcUnixUdpPollingTask();
|
||||
|
||||
/**
|
||||
* Turn on optional timeout for UDP polling. In the default mode,
|
||||
* the receive function will block until a packet is received.
|
||||
* @param timeoutSeconds
|
||||
*/
|
||||
void setTimeout(double timeoutSeconds);
|
||||
|
||||
virtual ReturnValue_t performOperation(uint8_t opCode) override;
|
||||
virtual ReturnValue_t initialize() override;
|
||||
virtual ReturnValue_t initializeAfterTaskCreation() override;
|
||||
|
||||
protected:
|
||||
StorageManagerIF* tcStore = nullptr;
|
||||
|
||||
private:
|
||||
//! TMTC bridge is cached.
|
||||
object_id_t tmtcBridgeId = objects::NO_OBJECT;
|
||||
TmTcUnixUdpBridge* tmtcBridge = nullptr;
|
||||
MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE;
|
||||
//! Reception flags: https://linux.die.net/man/2/recvfrom.
|
||||
int receptionFlags = 0;
|
||||
|
||||
//! Server socket, which is member of TMTC bridge and is assigned in
|
||||
//! constructor
|
||||
int serverUdpSocket = 0;
|
||||
|
||||
std::vector<uint8_t> receptionBuffer;
|
||||
|
||||
size_t frameSize = 0;
|
||||
timeval receptionTimeout;
|
||||
|
||||
ReturnValue_t handleSuccessfullTcRead(size_t bytesRead);
|
||||
void handleReadError();
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */
|
170
osal/linux/TmTcUnixUdpBridge.cpp
Normal file
170
osal/linux/TmTcUnixUdpBridge.cpp
Normal file
@ -0,0 +1,170 @@
|
||||
#include "TmTcUnixUdpBridge.h"
|
||||
#include "../../serviceinterface/ServiceInterfaceStream.h"
|
||||
#include "../../ipc/MutexHelper.h"
|
||||
|
||||
#include <errno.h>
|
||||
#include <arpa/inet.h>
|
||||
|
||||
|
||||
TmTcUnixUdpBridge::TmTcUnixUdpBridge(object_id_t objectId,
|
||||
object_id_t tcDestination, object_id_t tmStoreId, object_id_t tcStoreId,
|
||||
uint16_t serverPort, uint16_t clientPort):
|
||||
TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) {
|
||||
mutex = MutexFactory::instance()->createMutex();
|
||||
|
||||
uint16_t setServerPort = DEFAULT_UDP_SERVER_PORT;
|
||||
if(serverPort != 0xFFFF) {
|
||||
setServerPort = serverPort;
|
||||
}
|
||||
|
||||
uint16_t setClientPort = DEFAULT_UDP_CLIENT_PORT;
|
||||
if(clientPort != 0xFFFF) {
|
||||
setClientPort = clientPort;
|
||||
}
|
||||
|
||||
// Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html
|
||||
//clientSocket = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
|
||||
if(serverSocket < 0) {
|
||||
sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not open"
|
||||
" UDP socket!" << std::endl;
|
||||
handleSocketError();
|
||||
return;
|
||||
}
|
||||
|
||||
serverAddress.sin_family = AF_INET;
|
||||
|
||||
// Accept packets from any interface.
|
||||
//serverAddress.sin_addr.s_addr = inet_addr("127.73.73.0");
|
||||
serverAddress.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
serverAddress.sin_port = htons(setServerPort);
|
||||
serverAddressLen = sizeof(serverAddress);
|
||||
setsockopt(serverSocket, SOL_SOCKET, SO_REUSEADDR, &serverSocketOptions,
|
||||
sizeof(serverSocketOptions));
|
||||
|
||||
clientAddress.sin_family = AF_INET;
|
||||
clientAddress.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
clientAddress.sin_port = htons(setClientPort);
|
||||
clientAddressLen = sizeof(clientAddress);
|
||||
|
||||
int result = bind(serverSocket,
|
||||
reinterpret_cast<struct sockaddr*>(&serverAddress),
|
||||
serverAddressLen);
|
||||
if(result == -1) {
|
||||
sif::error << "TmTcUnixUdpBridge::TmTcUnixUdpBridge: Could not bind "
|
||||
"local port " << setServerPort << " to server socket!"
|
||||
<< std::endl;
|
||||
handleBindError();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
TmTcUnixUdpBridge::~TmTcUnixUdpBridge() {
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcUnixUdpBridge::sendTm(const uint8_t *data, size_t dataLen) {
|
||||
int flags = 0;
|
||||
|
||||
clientAddress.sin_addr.s_addr = htons(INADDR_ANY);
|
||||
//clientAddress.sin_addr.s_addr = inet_addr("127.73.73.1");
|
||||
clientAddressLen = sizeof(serverAddress);
|
||||
|
||||
// char ipAddress [15];
|
||||
// sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
|
||||
// &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
|
||||
|
||||
ssize_t bytesSent = sendto(serverSocket, data, dataLen, flags,
|
||||
reinterpret_cast<sockaddr*>(&clientAddress), clientAddressLen);
|
||||
if(bytesSent < 0) {
|
||||
sif::error << "TmTcUnixUdpBridge::sendTm: Send operation failed."
|
||||
<< std::endl;
|
||||
handleSendError();
|
||||
}
|
||||
// sif::debug << "TmTcUnixUdpBridge::sendTm: " << bytesSent << " bytes were"
|
||||
// " sent." << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void TmTcUnixUdpBridge::checkAndSetClientAddress(sockaddr_in newAddress) {
|
||||
MutexHelper lock(mutex, MutexIF::TimeoutType::WAITING, 10);
|
||||
|
||||
// char ipAddress [15];
|
||||
// sif::debug << "IP Address Sender: "<< inet_ntop(AF_INET,
|
||||
// &newAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
|
||||
// sif::debug << "IP Address Old: " << inet_ntop(AF_INET,
|
||||
// &clientAddress.sin_addr.s_addr, ipAddress, 15) << std::endl;
|
||||
|
||||
// Set new IP address if it has changed.
|
||||
if(clientAddress.sin_addr.s_addr != newAddress.sin_addr.s_addr) {
|
||||
clientAddress.sin_addr.s_addr = newAddress.sin_addr.s_addr;
|
||||
clientAddressLen = sizeof(clientAddress);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void TmTcUnixUdpBridge::handleSocketError() {
|
||||
// See: https://man7.org/linux/man-pages/man2/socket.2.html
|
||||
switch(errno) {
|
||||
case(EACCES):
|
||||
case(EINVAL):
|
||||
case(EMFILE):
|
||||
case(ENFILE):
|
||||
case(EAFNOSUPPORT):
|
||||
case(ENOBUFS):
|
||||
case(ENOMEM):
|
||||
case(EPROTONOSUPPORT):
|
||||
sif::error << "TmTcUnixBridge::handleSocketError: Socket creation failed"
|
||||
<< " with " << strerror(errno) << std::endl;
|
||||
break;
|
||||
default:
|
||||
sif::error << "TmTcUnixBridge::handleSocketError: Unknown error"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void TmTcUnixUdpBridge::handleBindError() {
|
||||
// See: https://man7.org/linux/man-pages/man2/bind.2.html
|
||||
switch(errno) {
|
||||
case(EACCES): {
|
||||
/*
|
||||
Ephermeral ports can be shown with following command:
|
||||
sysctl -A | grep ip_local_port_range
|
||||
*/
|
||||
sif::error << "TmTcUnixBridge::handleBindError: Port access issue."
|
||||
"Ports 1-1024 are reserved on UNIX systems and require root "
|
||||
"rights while ephermeral ports should not be used as well."
|
||||
<< std::endl;
|
||||
}
|
||||
break;
|
||||
case(EADDRINUSE):
|
||||
case(EBADF):
|
||||
case(EINVAL):
|
||||
case(ENOTSOCK):
|
||||
case(EADDRNOTAVAIL):
|
||||
case(EFAULT):
|
||||
case(ELOOP):
|
||||
case(ENAMETOOLONG):
|
||||
case(ENOENT):
|
||||
case(ENOMEM):
|
||||
case(ENOTDIR):
|
||||
case(EROFS): {
|
||||
sif::error << "TmTcUnixBridge::handleBindError: Socket creation failed"
|
||||
<< " with " << strerror(errno) << std::endl;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::error << "TmTcUnixBridge::handleBindError: Unknown error"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void TmTcUnixUdpBridge::handleSendError() {
|
||||
switch(errno) {
|
||||
default:
|
||||
sif::error << "TmTcUnixBridge::handleSendError: "
|
||||
<< strerror(errno) << std::endl;
|
||||
}
|
||||
}
|
||||
|
48
osal/linux/TmTcUnixUdpBridge.h
Normal file
48
osal/linux/TmTcUnixUdpBridge.h
Normal file
@ -0,0 +1,48 @@
|
||||
#ifndef FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
|
||||
#define FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_
|
||||
|
||||
#include "../../tmtcservices/AcceptsTelecommandsIF.h"
|
||||
#include "../../tmtcservices/TmTcBridge.h"
|
||||
#include <sys/socket.h>
|
||||
#include <netinet/in.h>
|
||||
#include <netinet/udp.h>
|
||||
|
||||
class TmTcUnixUdpBridge: public TmTcBridge {
|
||||
friend class TcUnixUdpPollingTask;
|
||||
public:
|
||||
// The ports chosen here should not be used by any other process.
|
||||
// List of used ports on Linux: /etc/services
|
||||
static constexpr uint16_t DEFAULT_UDP_SERVER_PORT = 7301;
|
||||
static constexpr uint16_t DEFAULT_UDP_CLIENT_PORT = 7302;
|
||||
|
||||
TmTcUnixUdpBridge(object_id_t objectId, object_id_t tcDestination,
|
||||
object_id_t tmStoreId, object_id_t tcStoreId,
|
||||
uint16_t serverPort = 0xFFFF,uint16_t clientPort = 0xFFFF);
|
||||
virtual~ TmTcUnixUdpBridge();
|
||||
|
||||
void checkAndSetClientAddress(sockaddr_in clientAddress);
|
||||
|
||||
protected:
|
||||
virtual ReturnValue_t sendTm(const uint8_t * data, size_t dataLen) override;
|
||||
|
||||
private:
|
||||
int serverSocket = 0;
|
||||
|
||||
const int serverSocketOptions = 0;
|
||||
|
||||
struct sockaddr_in clientAddress;
|
||||
socklen_t clientAddressLen = 0;
|
||||
|
||||
struct sockaddr_in serverAddress;
|
||||
socklen_t serverAddressLen = 0;
|
||||
|
||||
//! Access to the client address is mutex protected as it is set
|
||||
//! by another task.
|
||||
MutexIF* mutex;
|
||||
|
||||
void handleSocketError();
|
||||
void handleBindError();
|
||||
void handleSendError();
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_OSAL_LINUX_TMTCUNIXUDPBRIDGE_H_ */
|
@ -1,12 +1,27 @@
|
||||
#ifndef HASPARAMETERSIF_H_
|
||||
#define HASPARAMETERSIF_H_
|
||||
#ifndef FSFW_PARAMETERS_HASPARAMETERSIF_H_
|
||||
#define FSFW_PARAMETERS_HASPARAMETERSIF_H_
|
||||
|
||||
#include "ParameterWrapper.h"
|
||||
#include "../parameters/ParameterWrapper.h"
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include <stdint.h>
|
||||
|
||||
/** Each parameter is identified with a unique parameter ID */
|
||||
typedef uint32_t ParameterId_t;
|
||||
|
||||
/**
|
||||
* @brief This interface is used by components which have modifiable
|
||||
* parameters, e.g. atittude controllers
|
||||
* @details
|
||||
* Each parameter has a unique parameter ID. The first byte of the parameter
|
||||
* ID is the domain ID which can be used to identify unqiue spacecraft domains
|
||||
* (e.g. control and sensor domain in the AOCS controller).
|
||||
*
|
||||
* The second and third byte represent the matrix ID, which can represent
|
||||
* a 8-bit row and column number and the last byte...
|
||||
*
|
||||
* Yeah, it it matrix ID oder parameter ID now and is index a 16 bit number
|
||||
* of a 8 bit number now?
|
||||
*/
|
||||
class HasParametersIF {
|
||||
public:
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::HAS_PARAMETERS_IF;
|
||||
@ -32,13 +47,11 @@ public:
|
||||
return (domainId << 24) + (parameterId << 8) + index;
|
||||
}
|
||||
|
||||
virtual ~HasParametersIF() {
|
||||
}
|
||||
virtual ~HasParametersIF() {}
|
||||
|
||||
/**
|
||||
* Always set parameter before checking newValues!
|
||||
*
|
||||
*
|
||||
* @param domainId
|
||||
* @param parameterId
|
||||
* @param parameterWrapper
|
||||
@ -51,4 +64,4 @@ public:
|
||||
const ParameterWrapper *newValues, uint16_t startAtIndex) = 0;
|
||||
};
|
||||
|
||||
#endif /* HASPARAMETERSIF_H_ */
|
||||
#endif /* FSFW_PARAMETERS_HASPARAMETERSIF_H_ */
|
||||
|
@ -1,11 +1,9 @@
|
||||
#include "../objectmanager/ObjectManagerIF.h"
|
||||
#include "ParameterHelper.h"
|
||||
#include "ParameterMessage.h"
|
||||
#include "../objectmanager/ObjectManagerIF.h"
|
||||
|
||||
ParameterHelper::ParameterHelper(ReceivesParameterMessagesIF* owner) :
|
||||
owner(owner), storage(NULL) {
|
||||
|
||||
}
|
||||
owner(owner) {}
|
||||
|
||||
ParameterHelper::~ParameterHelper() {
|
||||
}
|
||||
@ -28,7 +26,6 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) {
|
||||
}
|
||||
break;
|
||||
case ParameterMessage::CMD_PARAMETER_LOAD: {
|
||||
|
||||
uint8_t domain = HasParametersIF::getDomain(
|
||||
ParameterMessage::getParameterId(message));
|
||||
uint16_t parameterId = HasParametersIF::getMatrixId(
|
||||
@ -36,12 +33,14 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) {
|
||||
uint8_t index = HasParametersIF::getIndex(
|
||||
ParameterMessage::getParameterId(message));
|
||||
|
||||
const uint8_t *storedStream;
|
||||
size_t storedStreamSize;
|
||||
const uint8_t *storedStream = nullptr;
|
||||
size_t storedStreamSize = 0;
|
||||
result = storage->getData(
|
||||
ParameterMessage::getStoreId(message), &storedStream,
|
||||
&storedStreamSize);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
sif::error << "ParameterHelper::handleParameterMessage: Getting"
|
||||
" store data failed for load command." << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
@ -125,7 +124,8 @@ ReturnValue_t ParameterHelper::initialize() {
|
||||
}
|
||||
}
|
||||
|
||||
void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand) {
|
||||
void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason,
|
||||
Command_t initialCommand) {
|
||||
CommandMessage reply;
|
||||
reply.setReplyRejected(reason, initialCommand);
|
||||
MessageQueueSenderIF::sendMessage(to, &reply, ownerQueueId);
|
||||
|
@ -1,9 +1,16 @@
|
||||
#ifndef PARAMETERHELPER_H_
|
||||
#define PARAMETERHELPER_H_
|
||||
#ifndef FSFW_PARAMETERS_PARAMETERHELPER_H_
|
||||
#define FSFW_PARAMETERS_PARAMETERHELPER_H_
|
||||
|
||||
#include "ParameterMessage.h"
|
||||
#include "ReceivesParameterMessagesIF.h"
|
||||
#include "../ipc/MessageQueueIF.h"
|
||||
|
||||
/**
|
||||
* @brief Helper class to handle parameter messages.
|
||||
* @details
|
||||
* This class simplfies handling of parameter messages, which are sent
|
||||
* to a class which implements ReceivesParameterMessagesIF.
|
||||
*/
|
||||
class ParameterHelper {
|
||||
public:
|
||||
ParameterHelper(ReceivesParameterMessagesIF *owner);
|
||||
@ -15,13 +22,15 @@ public:
|
||||
private:
|
||||
ReceivesParameterMessagesIF *owner;
|
||||
|
||||
MessageQueueId_t ownerQueueId;
|
||||
MessageQueueId_t ownerQueueId = MessageQueueIF::NO_QUEUE;
|
||||
|
||||
StorageManagerIF *storage;
|
||||
StorageManagerIF *storage = nullptr;
|
||||
|
||||
ReturnValue_t sendParameter(MessageQueueId_t to, uint32_t id, const ParameterWrapper *description);
|
||||
ReturnValue_t sendParameter(MessageQueueId_t to, uint32_t id,
|
||||
const ParameterWrapper *description);
|
||||
|
||||
void rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand);
|
||||
void rejectCommand(MessageQueueId_t to, ReturnValue_t reason,
|
||||
Command_t initialCommand);
|
||||
};
|
||||
|
||||
#endif /* PARAMETERHELPER_H_ */
|
||||
#endif /* FSFW_PARAMETERS_PARAMETERHELPER_H_ */
|
||||
|
@ -1,4 +1,4 @@
|
||||
#include "ParameterMessage.h"
|
||||
#include "../parameters/ParameterMessage.h"
|
||||
#include "../objectmanager/ObjectManagerIF.h"
|
||||
|
||||
ParameterId_t ParameterMessage::getParameterId(const CommandMessage* message) {
|
||||
|
@ -1,8 +1,8 @@
|
||||
#ifndef PARAMETERMESSAGE_H_
|
||||
#define PARAMETERMESSAGE_H_
|
||||
#ifndef FSFW_PARAMETERS_PARAMETERMESSAGE_H_
|
||||
#define FSFW_PARAMETERS_PARAMETERMESSAGE_H_
|
||||
|
||||
#include "../ipc/CommandMessage.h"
|
||||
#include "HasParametersIF.h"
|
||||
#include "../ipc/CommandMessage.h"
|
||||
#include "../storagemanager/StorageManagerIF.h"
|
||||
|
||||
class ParameterMessage {
|
||||
@ -26,4 +26,4 @@ public:
|
||||
|
||||
};
|
||||
|
||||
#endif /* PARAMETERMESSAGE_H_ */
|
||||
#endif /* FSFW_PARAMETERS_PARAMETERMESSAGE_H_ */
|
||||
|
@ -1,20 +1,19 @@
|
||||
#include "ParameterWrapper.h"
|
||||
|
||||
ParameterWrapper::ParameterWrapper() :
|
||||
pointsToStream(false), type(Type::UNKNOWN_TYPE), rows(0), columns(0), data(
|
||||
NULL), readonlyData(NULL) {
|
||||
pointsToStream(false), type(Type::UNKNOWN_TYPE) {
|
||||
}
|
||||
|
||||
ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns,
|
||||
void *data) :
|
||||
pointsToStream(false), type(type), rows(rows), columns(columns), data(
|
||||
data), readonlyData(data) {
|
||||
pointsToStream(false), type(type), rows(rows), columns(columns),
|
||||
data(data), readonlyData(data) {
|
||||
}
|
||||
|
||||
ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns,
|
||||
const void *data) :
|
||||
pointsToStream(false), type(type), rows(rows), columns(columns), data(
|
||||
NULL), readonlyData(data) {
|
||||
pointsToStream(false), type(type), rows(rows), columns(columns),
|
||||
data(nullptr), readonlyData(data) {
|
||||
}
|
||||
|
||||
ParameterWrapper::~ParameterWrapper() {
|
||||
@ -141,6 +140,7 @@ ReturnValue_t ParameterWrapper::deSerializeData(uint8_t startingRow,
|
||||
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t ParameterWrapper::deSerialize(const uint8_t **buffer,
|
||||
size_t *size, Endianness streamEndianness) {
|
||||
return deSerialize(buffer, size, streamEndianness, 0);
|
||||
@ -184,16 +184,16 @@ ReturnValue_t ParameterWrapper::set(const uint8_t *stream, size_t streamSize,
|
||||
return SerializeIF::STREAM_TOO_SHORT;
|
||||
}
|
||||
|
||||
data = NULL;
|
||||
data = nullptr;
|
||||
readonlyData = stream;
|
||||
pointsToStream = true;
|
||||
|
||||
stream += dataSize;
|
||||
if (remainingStream != NULL) {
|
||||
if (remainingStream != nullptr) {
|
||||
*remainingStream = stream;
|
||||
}
|
||||
streamSize -= dataSize;
|
||||
if (remainingSize != NULL) {
|
||||
if (remainingSize != nullptr) {
|
||||
*remainingSize = streamSize;
|
||||
}
|
||||
|
||||
@ -265,15 +265,15 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from,
|
||||
result = UNKNOW_DATATYPE;
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
//need a type to do arithmetic
|
||||
uint8_t *toDataWithType = (uint8_t*) data;
|
||||
uint8_t* typedData = static_cast<uint8_t*>(data);
|
||||
for (uint8_t fromRow = 0; fromRow < from->rows; fromRow++) {
|
||||
memcpy(
|
||||
toDataWithType
|
||||
+ (((startingRow + fromRow) * columns)
|
||||
+ startingColumn) * typeSize,
|
||||
from->readonlyData, typeSize * from->columns);
|
||||
size_t offset = (((startingRow + fromRow) * columns) +
|
||||
startingColumn) * typeSize;
|
||||
std::memcpy(typedData + offset, from->readonlyData,
|
||||
typeSize * from->columns);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1,12 +1,16 @@
|
||||
#ifndef PARAMETERWRAPPER_H_
|
||||
#define PARAMETERWRAPPER_H_
|
||||
#ifndef FSFW_PARAMETERS_PARAMETERWRAPPER_H_
|
||||
#define FSFW_PARAMETERS_PARAMETERWRAPPER_H_
|
||||
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include "../serialize/SerializeAdapter.h"
|
||||
#include "../serialize/SerializeIF.h"
|
||||
#include <stddef.h>
|
||||
#include "../globalfunctions/Type.h"
|
||||
#include <cstddef>
|
||||
|
||||
/**
|
||||
* @brief
|
||||
* @details
|
||||
*/
|
||||
class ParameterWrapper: public SerializeIF {
|
||||
friend class DataPoolParameterWrapper;
|
||||
public:
|
||||
@ -36,32 +40,21 @@ public:
|
||||
virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
|
||||
Endianness streamEndianness, uint16_t startWritingAtIndex = 0);
|
||||
|
||||
/**
|
||||
* Get a specific parameter value by supplying the row and the column.
|
||||
* @tparam T Type of target data
|
||||
* @param value [out] Pointer to storage location
|
||||
* @param row
|
||||
* @param column
|
||||
* @return
|
||||
* -@c RETURN_OK if element was retrieved successfully
|
||||
* -@c NOT_SET data has not been set yet
|
||||
* -@c DATATYPE_MISSMATCH Invalid supplied type
|
||||
* -@c OUT_OF_BOUNDS Invalid row and/or column.
|
||||
*/
|
||||
template<typename T>
|
||||
ReturnValue_t getElement(T *value, uint8_t row = 0, uint8_t column = 0) const {
|
||||
if (readonlyData == NULL){
|
||||
return NOT_SET;
|
||||
}
|
||||
|
||||
if (PodTypeConversion<T>::type != type) {
|
||||
return DATATYPE_MISSMATCH;
|
||||
}
|
||||
|
||||
if ((row >= rows) || (column >= columns)) {
|
||||
return OUT_OF_BOUNDS;
|
||||
}
|
||||
|
||||
if (pointsToStream) {
|
||||
const uint8_t *streamWithtype = (const uint8_t *) readonlyData;
|
||||
streamWithtype += (row * columns + column) * type.getSize();
|
||||
int32_t size = type.getSize();
|
||||
return SerializeAdapter::deSerialize(value, &streamWithtype,
|
||||
&size, true);
|
||||
} else {
|
||||
const T *dataWithType = (const T *) readonlyData;
|
||||
*value = dataWithType[row * columns + column];
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
ReturnValue_t getElement(T *value, uint8_t row = 0,
|
||||
uint8_t column = 0) const;
|
||||
|
||||
template<typename T>
|
||||
void set(T *data, uint8_t rows, uint8_t columns) {
|
||||
@ -111,21 +104,22 @@ public:
|
||||
void setMatrix(const T& member) {
|
||||
this->set(member[0], sizeof(member)/sizeof(member[0]), sizeof(member[0])/sizeof(member[0][0]));
|
||||
}
|
||||
|
||||
ReturnValue_t set(const uint8_t *stream, size_t streamSize,
|
||||
const uint8_t **remainingStream = NULL, size_t *remainingSize =
|
||||
NULL);
|
||||
const uint8_t **remainingStream = nullptr,
|
||||
size_t *remainingSize = nullptr);
|
||||
|
||||
ReturnValue_t copyFrom(const ParameterWrapper *from,
|
||||
uint16_t startWritingAtIndex);
|
||||
|
||||
private:
|
||||
bool pointsToStream;
|
||||
bool pointsToStream = false;
|
||||
|
||||
Type type;
|
||||
uint8_t rows;
|
||||
uint8_t columns;
|
||||
void *data;
|
||||
const void *readonlyData;
|
||||
uint8_t rows = 0;
|
||||
uint8_t columns = 0;
|
||||
void *data = nullptr;
|
||||
const void *readonlyData = nullptr;
|
||||
|
||||
template<typename T>
|
||||
ReturnValue_t serializeData(uint8_t** buffer, size_t* size,
|
||||
@ -136,4 +130,33 @@ private:
|
||||
const void *from, uint8_t fromRows, uint8_t fromColumns);
|
||||
};
|
||||
|
||||
#endif /* PARAMETERWRAPPER_H_ */
|
||||
template <typename T>
|
||||
inline ReturnValue_t ParameterWrapper::getElement(T *value, uint8_t row,
|
||||
uint8_t column) const {
|
||||
if (readonlyData == nullptr){
|
||||
return NOT_SET;
|
||||
}
|
||||
|
||||
if (PodTypeConversion<T>::type != type) {
|
||||
return DATATYPE_MISSMATCH;
|
||||
}
|
||||
|
||||
if ((row >= rows) or (column >= columns)) {
|
||||
return OUT_OF_BOUNDS;
|
||||
}
|
||||
|
||||
if (pointsToStream) {
|
||||
const uint8_t *streamWithType = static_cast<const uint8_t*>(readonlyData);
|
||||
streamWithType += (row * columns + column) * type.getSize();
|
||||
int32_t size = type.getSize();
|
||||
return SerializeAdapter::deSerialize(value, &streamWithType,
|
||||
&size, true);
|
||||
}
|
||||
else {
|
||||
const T *dataWithType = static_cast<const T*>(readonlyData);
|
||||
*value = dataWithType[row * columns + column];
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* FSFW_PARAMETERS_PARAMETERWRAPPER_H_ */
|
||||
|
@ -1,5 +1,5 @@
|
||||
#ifndef RECEIVESPARAMETERMESSAGESIF_H_
|
||||
#define RECEIVESPARAMETERMESSAGESIF_H_
|
||||
#ifndef FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_
|
||||
#define FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_
|
||||
|
||||
|
||||
#include "HasParametersIF.h"
|
||||
@ -16,4 +16,4 @@ public:
|
||||
};
|
||||
|
||||
|
||||
#endif /* RECEIVESPARAMETERMESSAGESIF_H_ */
|
||||
#endif /* FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_ */
|
||||
|
@ -1,5 +1,5 @@
|
||||
#ifndef FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
|
||||
#define FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
|
||||
#ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
|
||||
#define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_
|
||||
|
||||
#include "../objectmanager/SystemObject.h"
|
||||
#include "../storagemanager/StorageManagerIF.h"
|
||||
@ -345,4 +345,4 @@ private:
|
||||
void checkTimeout();
|
||||
};
|
||||
|
||||
#endif /* COMMANDINGSERVICEBASE_H_ */
|
||||
#endif /* FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ */
|
||||
|
@ -1,7 +1,7 @@
|
||||
#include "TmTcBridge.h"
|
||||
#include "../tmtcservices/TmTcBridge.h"
|
||||
|
||||
#include "../ipc/QueueFactory.h"
|
||||
#include "AcceptsTelecommandsIF.h"
|
||||
#include "../tmtcservices/AcceptsTelecommandsIF.h"
|
||||
#include "../serviceinterface/ServiceInterfaceStream.h"
|
||||
#include "../globalfunctions/arrayprinter.h"
|
||||
|
||||
@ -66,6 +66,8 @@ ReturnValue_t TmTcBridge::initialize() {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
tmFifo = new DynamicFIFO<store_address_t>(maxNumberOfPacketsStored);
|
||||
|
||||
tmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
|
||||
return RETURN_OK;
|
||||
}
|
||||
@ -90,102 +92,122 @@ ReturnValue_t TmTcBridge::handleTc() {
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcBridge::handleTm() {
|
||||
ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
|
||||
ReturnValue_t result = handleTmQueue();
|
||||
if(result != RETURN_OK) {
|
||||
sif::warning << "TmTcBridge: Reading TM Queue failed" << std::endl;
|
||||
return RETURN_FAILED;
|
||||
sif::error << "TmTcBridge::handleTm: Error handling TM queue!"
|
||||
<< std::endl;
|
||||
status = result;
|
||||
}
|
||||
|
||||
if(tmStored and communicationLinkUp) {
|
||||
result = handleStoredTm();
|
||||
if(tmStored and communicationLinkUp and
|
||||
(packetSentCounter < sentPacketsPerCycle)) {
|
||||
result = handleStoredTm();
|
||||
if(result != RETURN_OK) {
|
||||
sif::error << "TmTcBridge::handleTm: Error handling stored TMs!"
|
||||
<< std::endl;
|
||||
status = result;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
|
||||
packetSentCounter = 0;
|
||||
return status;
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcBridge::handleTmQueue() {
|
||||
TmTcMessage message;
|
||||
const uint8_t* data = nullptr;
|
||||
size_t size = 0;
|
||||
ReturnValue_t status = HasReturnvaluesIF::RETURN_OK;
|
||||
for (ReturnValue_t result = tmTcReceptionQueue->receiveMessage(&message);
|
||||
result == RETURN_OK; result = tmTcReceptionQueue->receiveMessage(&message))
|
||||
result == HasReturnvaluesIF::RETURN_OK;
|
||||
result = tmTcReceptionQueue->receiveMessage(&message))
|
||||
{
|
||||
if(communicationLinkUp == false) {
|
||||
result = storeDownlinkData(&message);
|
||||
return result;
|
||||
//sif::info << (int) packetSentCounter << std::endl;
|
||||
if(communicationLinkUp == false or
|
||||
packetSentCounter >= sentPacketsPerCycle) {
|
||||
storeDownlinkData(&message);
|
||||
continue;
|
||||
}
|
||||
|
||||
result = tmStore->getData(message.getStorageId(), &data, &size);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
status = result;
|
||||
continue;
|
||||
}
|
||||
|
||||
result = sendTm(data, size);
|
||||
if (result != RETURN_OK) {
|
||||
sif::warning << "TmTcBridge: Could not send TM packet" << std::endl;
|
||||
tmStore->deleteData(message.getStorageId());
|
||||
return result;
|
||||
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
status = result;
|
||||
}
|
||||
else {
|
||||
tmStore->deleteData(message.getStorageId());
|
||||
packetSentCounter++;
|
||||
}
|
||||
tmStore->deleteData(message.getStorageId());
|
||||
}
|
||||
return RETURN_OK;
|
||||
return status;
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
|
||||
store_address_t storeId = 0;
|
||||
|
||||
if(tmFifo.full()) {
|
||||
sif::error << "TmTcBridge::storeDownlinkData: TM downlink max. number "
|
||||
<< "of stored packet IDs reached! "
|
||||
<< "Overwriting old data" << std::endl;
|
||||
tmFifo.retrieve(&storeId);
|
||||
tmStore->deleteData(storeId);
|
||||
if(tmFifo->full()) {
|
||||
sif::debug << "TmTcBridge::storeDownlinkData: TM downlink max. number "
|
||||
<< "of stored packet IDs reached! " << std::endl;
|
||||
if(overwriteOld) {
|
||||
tmFifo->retrieve(&storeId);
|
||||
tmStore->deleteData(storeId);
|
||||
}
|
||||
else {
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
|
||||
storeId = message->getStorageId();
|
||||
tmFifo.insert(storeId);
|
||||
tmFifo->insert(storeId);
|
||||
tmStored = true;
|
||||
return RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcBridge::handleStoredTm() {
|
||||
uint8_t counter = 0;
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
while(not tmFifo.empty() and counter < sentPacketsPerCycle) {
|
||||
//info << "TMTC Bridge: Sending stored TM data. There are "
|
||||
// << (int) fifo.size() << " left to send\r\n" << std::flush;
|
||||
ReturnValue_t status = RETURN_OK;
|
||||
while(not tmFifo->empty() and packetSentCounter < sentPacketsPerCycle) {
|
||||
//sif::info << "TMTC Bridge: Sending stored TM data. There are "
|
||||
// << (int) tmFifo->size() << " left to send\r\n" << std::flush;
|
||||
|
||||
store_address_t storeId;
|
||||
const uint8_t* data = nullptr;
|
||||
size_t size = 0;
|
||||
tmFifo.retrieve(&storeId);
|
||||
result = tmStore->getData(storeId, &data, &size);
|
||||
|
||||
sendTm(data,size);
|
||||
tmFifo->retrieve(&storeId);
|
||||
ReturnValue_t result = tmStore->getData(storeId, &data, &size);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
status = result;
|
||||
}
|
||||
|
||||
result = sendTm(data,size);
|
||||
if(result != RETURN_OK) {
|
||||
sif::error << "TMTC Bridge: Could not send stored downlink data"
|
||||
<< std::endl;
|
||||
result = RETURN_FAILED;
|
||||
status = result;
|
||||
}
|
||||
counter ++;
|
||||
packetSentCounter ++;
|
||||
|
||||
if(tmFifo.empty()) {
|
||||
if(tmFifo->empty()) {
|
||||
tmStored = false;
|
||||
}
|
||||
tmStore->deleteData(storeId);
|
||||
}
|
||||
return result;
|
||||
return status;
|
||||
}
|
||||
|
||||
void TmTcBridge::registerCommConnect() {
|
||||
if(not communicationLinkUp) {
|
||||
//info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
|
||||
//sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
|
||||
communicationLinkUp = true;
|
||||
}
|
||||
}
|
||||
|
||||
void TmTcBridge::registerCommDisconnect() {
|
||||
//info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
|
||||
//sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
|
||||
if(communicationLinkUp) {
|
||||
communicationLinkUp = false;
|
||||
}
|
||||
@ -209,3 +231,7 @@ MessageQueueId_t TmTcBridge::getRequestQueue() {
|
||||
// Default implementation: Relay TC messages to TC distributor directly.
|
||||
return tmTcReceptionQueue->getDefaultDestination();
|
||||
}
|
||||
|
||||
void TmTcBridge::setFifoToOverwriteOldData(bool overwriteOld) {
|
||||
this->overwriteOld = overwriteOld;
|
||||
}
|
||||
|
@ -1,15 +1,15 @@
|
||||
#ifndef FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
|
||||
#define FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_
|
||||
|
||||
|
||||
#include "../objectmanager/SystemObject.h"
|
||||
#include "AcceptsTelemetryIF.h"
|
||||
#include "../tmtcservices/AcceptsTelemetryIF.h"
|
||||
#include "../tasks/ExecutableObjectIF.h"
|
||||
#include "../ipc/MessageQueueIF.h"
|
||||
#include "../storagemanager/StorageManagerIF.h"
|
||||
#include "AcceptsTelecommandsIF.h"
|
||||
|
||||
#include "../container/FIFO.h"
|
||||
#include "TmTcMessage.h"
|
||||
#include "../tmtcservices/AcceptsTelecommandsIF.h"
|
||||
#include "../container/DynamicFIFO.h"
|
||||
#include "../tmtcservices/TmTcMessage.h"
|
||||
|
||||
class TmTcBridge : public AcceptsTelemetryIF,
|
||||
public AcceptsTelecommandsIF,
|
||||
@ -46,6 +46,12 @@ public:
|
||||
*/
|
||||
ReturnValue_t setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored);
|
||||
|
||||
/**
|
||||
* This will set up the bridge to overwrite old data in the FIFO.
|
||||
* @param overwriteOld
|
||||
*/
|
||||
void setFifoToOverwriteOldData(bool overwriteOld);
|
||||
|
||||
virtual void registerCommConnect();
|
||||
virtual void registerCommDisconnect();
|
||||
|
||||
@ -86,6 +92,8 @@ protected:
|
||||
//! by default, so telemetry will be handled immediately.
|
||||
bool communicationLinkUp = true;
|
||||
bool tmStored = false;
|
||||
bool overwriteOld = true;
|
||||
uint8_t packetSentCounter = 0;
|
||||
|
||||
/**
|
||||
* @brief Handle TC reception
|
||||
@ -145,7 +153,7 @@ protected:
|
||||
* This fifo can be used to store downlink data
|
||||
* which can not be sent at the moment.
|
||||
*/
|
||||
FIFO<store_address_t, LIMIT_DOWNLINK_PACKETS_STORED> tmFifo;
|
||||
DynamicFIFO<store_address_t>* tmFifo = nullptr;
|
||||
uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;
|
||||
uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED;
|
||||
};
|
||||
|
Loading…
Reference in New Issue
Block a user