Merge branch 'mueller/hostosal' of https://egit.irs.uni-stuttgart.de/KSat/fsfw into mueller/hostosal
This commit is contained in:
commit
51cf13428b
@ -72,11 +72,15 @@ public:
|
||||
return tmp;
|
||||
}
|
||||
|
||||
T operator*() {
|
||||
T& operator*(){
|
||||
return *value;
|
||||
}
|
||||
|
||||
T *operator->() {
|
||||
const T& operator*() const{
|
||||
return *value;
|
||||
}
|
||||
|
||||
T *operator->(){
|
||||
return value;
|
||||
}
|
||||
|
||||
|
@ -27,14 +27,27 @@ public:
|
||||
|
||||
/**
|
||||
* @brief Custom copy constructor which prevents setting the
|
||||
* underlying pointer wrong.
|
||||
* underlying pointer wrong. This function allocates memory!
|
||||
* @details This is a very heavy operation so try to avoid this!
|
||||
*
|
||||
*/
|
||||
DynamicFIFO(const DynamicFIFO& other): FIFOBase<T>(other),
|
||||
fifoVector(other.maxCapacity) {
|
||||
this->fifoVector = other.fifoVector;
|
||||
this->setContainer(fifoVector.data());
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Custom assignment operator
|
||||
* @details This is a very heavy operation so try to avoid this!
|
||||
* @param other DyamicFIFO to copy from
|
||||
*/
|
||||
DynamicFIFO& operator=(const DynamicFIFO& other){
|
||||
FIFOBase<T>::operator=(other);
|
||||
this->fifoVector = other.fifoVector;
|
||||
this->setContainer(fifoVector.data());
|
||||
return *this;
|
||||
}
|
||||
private:
|
||||
std::vector<T> fifoVector;
|
||||
};
|
||||
|
@ -25,9 +25,21 @@ public:
|
||||
* @param other
|
||||
*/
|
||||
FIFO(const FIFO& other): FIFOBase<T>(other) {
|
||||
this->fifoArray = other.fifoArray;
|
||||
this->setContainer(fifoArray.data());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Custom assignment operator
|
||||
* @param other
|
||||
*/
|
||||
FIFO& operator=(const FIFO& other){
|
||||
FIFOBase<T>::operator=(other);
|
||||
this->fifoArray = other.fifoArray;
|
||||
this->setContainer(fifoArray.data());
|
||||
return *this;
|
||||
}
|
||||
|
||||
private:
|
||||
std::array<T, capacity> fifoArray;
|
||||
};
|
||||
|
@ -1,15 +1,20 @@
|
||||
#ifndef FIXEDMAP_H_
|
||||
#define FIXEDMAP_H_
|
||||
#ifndef FSFW_CONTAINER_FIXEDMAP_H_
|
||||
#define FSFW_CONTAINER_FIXEDMAP_H_
|
||||
|
||||
#include "ArrayList.h"
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include <utility>
|
||||
#include <type_traits>
|
||||
|
||||
/**
|
||||
* \ingroup container
|
||||
* @warning Iterators return a non-const key_t in the pair.
|
||||
* @warning A User is not allowed to change the key, otherwise the map is corrupted.
|
||||
* @ingroup container
|
||||
*/
|
||||
template<typename key_t, typename T>
|
||||
class FixedMap: public SerializeIF {
|
||||
static_assert (std::is_trivially_copyable<T>::value or std::is_base_of<SerializeIF, T>::value,
|
||||
"Types used in FixedMap must either be trivial copy-able or a derived Class from SerializeIF to be serialize-able");
|
||||
public:
|
||||
static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP;
|
||||
static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01);
|
||||
@ -47,15 +52,6 @@ public:
|
||||
Iterator(std::pair<key_t, T> *pair) :
|
||||
ArrayList<std::pair<key_t, T>, uint32_t>::Iterator(pair) {
|
||||
}
|
||||
|
||||
T operator*() {
|
||||
return ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
|
||||
}
|
||||
|
||||
T *operator->() {
|
||||
return &ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
Iterator begin() const {
|
||||
@ -70,7 +66,7 @@ public:
|
||||
return _size;
|
||||
}
|
||||
|
||||
ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) {
|
||||
ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr) {
|
||||
if (exists(key) == HasReturnvaluesIF::RETURN_OK) {
|
||||
return KEY_ALREADY_EXISTS;
|
||||
}
|
||||
@ -79,7 +75,7 @@ public:
|
||||
}
|
||||
theMap[_size].first = key;
|
||||
theMap[_size].second = value;
|
||||
if (storedValue != NULL) {
|
||||
if (storedValue != nullptr) {
|
||||
*storedValue = Iterator(&theMap[_size]);
|
||||
}
|
||||
++_size;
|
||||
@ -87,7 +83,7 @@ public:
|
||||
}
|
||||
|
||||
ReturnValue_t insert(std::pair<key_t, T> pair) {
|
||||
return insert(pair.fist, pair.second);
|
||||
return insert(pair.first, pair.second);
|
||||
}
|
||||
|
||||
ReturnValue_t exists(key_t key) const {
|
||||
@ -196,4 +192,4 @@ public:
|
||||
|
||||
};
|
||||
|
||||
#endif /* FIXEDMAP_H_ */
|
||||
#endif /* FSFW_CONTAINER_FIXEDMAP_H_ */
|
||||
|
@ -48,7 +48,7 @@ private:
|
||||
if (_size <= position) {
|
||||
return;
|
||||
}
|
||||
memmove(&theMap[position], &theMap[position + 1],
|
||||
memmove(static_cast<void*>(&theMap[position]), static_cast<void*>(&theMap[position + 1]),
|
||||
(_size - position - 1) * sizeof(std::pair<key_t,T>));
|
||||
--_size;
|
||||
}
|
||||
@ -68,15 +68,6 @@ public:
|
||||
Iterator(std::pair<key_t, T> *pair) :
|
||||
ArrayList<std::pair<key_t, T>, uint32_t>::Iterator(pair) {
|
||||
}
|
||||
|
||||
T operator*() {
|
||||
return ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
|
||||
}
|
||||
|
||||
T *operator->() {
|
||||
return &ArrayList<std::pair<key_t, T>, uint32_t>::Iterator::value->second;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
Iterator begin() const {
|
||||
@ -91,17 +82,17 @@ public:
|
||||
return _size;
|
||||
}
|
||||
|
||||
ReturnValue_t insert(key_t key, T value, Iterator *storedValue = NULL) {
|
||||
ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr) {
|
||||
if (_size == theMap.maxSize()) {
|
||||
return MAP_FULL;
|
||||
}
|
||||
uint32_t position = findNicePlace(key);
|
||||
memmove(&theMap[position + 1], &theMap[position],
|
||||
memmove(static_cast<void*>(&theMap[position + 1]),static_cast<void*>(&theMap[position]),
|
||||
(_size - position) * sizeof(std::pair<key_t,T>));
|
||||
theMap[position].first = key;
|
||||
theMap[position].second = value;
|
||||
++_size;
|
||||
if (storedValue != NULL) {
|
||||
if (storedValue != nullptr) {
|
||||
*storedValue = Iterator(&theMap[position]);
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
@ -145,12 +136,6 @@ public:
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
//This is potentially unsafe
|
||||
// T *findValue(key_t key) const {
|
||||
// return &theMap[findFirstIndex(key)].second;
|
||||
// }
|
||||
|
||||
|
||||
Iterator find(key_t key) const {
|
||||
ReturnValue_t result = exists(key);
|
||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||
|
@ -1,41 +0,0 @@
|
||||
#ifndef ISDERIVEDFROM_H_
|
||||
#define ISDERIVEDFROM_H_
|
||||
|
||||
template<typename D, typename B>
|
||||
class IsDerivedFrom {
|
||||
class No {
|
||||
};
|
||||
class Yes {
|
||||
No no[3];
|
||||
};
|
||||
|
||||
static Yes Test(B*); // declared, but not defined
|
||||
static No Test(... ); // declared, but not defined
|
||||
|
||||
public:
|
||||
enum {
|
||||
Is = sizeof(Test(static_cast<D*>(0))) == sizeof(Yes)
|
||||
};
|
||||
};
|
||||
|
||||
template<typename, typename>
|
||||
struct is_same {
|
||||
static bool const value = false;
|
||||
};
|
||||
|
||||
template<typename A>
|
||||
struct is_same<A, A> {
|
||||
static bool const value = true;
|
||||
};
|
||||
|
||||
|
||||
template<bool C, typename T = void>
|
||||
struct enable_if {
|
||||
typedef T type;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
struct enable_if<false, T> { };
|
||||
|
||||
|
||||
#endif /* ISDERIVEDFROM_H_ */
|
@ -1,95 +1,124 @@
|
||||
#include "DleEncoder.h"
|
||||
#include "../globalfunctions/DleEncoder.h"
|
||||
|
||||
DleEncoder::DleEncoder() {
|
||||
}
|
||||
DleEncoder::DleEncoder() {}
|
||||
|
||||
DleEncoder::~DleEncoder() {
|
||||
DleEncoder::~DleEncoder() {}
|
||||
|
||||
ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
||||
size_t sourceLen, uint8_t* destStream, size_t maxDestLen,
|
||||
size_t* encodedLen, bool addStxEtx) {
|
||||
if (maxDestLen < 2) {
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
size_t encodedIndex = 0, sourceIndex = 0;
|
||||
uint8_t nextByte;
|
||||
if (addStxEtx) {
|
||||
destStream[0] = STX_CHAR;
|
||||
++encodedIndex;
|
||||
}
|
||||
|
||||
while (encodedIndex < maxDestLen and sourceIndex < sourceLen)
|
||||
{
|
||||
nextByte = sourceStream[sourceIndex];
|
||||
// STX, ETX and CR characters in the stream need to be escaped with DLE
|
||||
if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) {
|
||||
if (encodedIndex + 1 >= maxDestLen) {
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
else {
|
||||
destStream[encodedIndex] = DLE_CHAR;
|
||||
++encodedIndex;
|
||||
/* Escaped byte will be actual byte + 0x40. This prevents
|
||||
* STX, ETX, and carriage return characters from appearing
|
||||
* in the encoded data stream at all, so when polling an
|
||||
* encoded stream, the transmission can be stopped at ETX.
|
||||
* 0x40 was chosen at random with special requirements:
|
||||
* - Prevent going from one control char to another
|
||||
* - Prevent overflow for common characters */
|
||||
destStream[encodedIndex] = nextByte + 0x40;
|
||||
}
|
||||
}
|
||||
// DLE characters are simply escaped with DLE.
|
||||
else if (nextByte == DLE_CHAR) {
|
||||
if (encodedIndex + 1 >= maxDestLen) {
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
else {
|
||||
destStream[encodedIndex] = DLE_CHAR;
|
||||
++encodedIndex;
|
||||
destStream[encodedIndex] = DLE_CHAR;
|
||||
}
|
||||
}
|
||||
else {
|
||||
destStream[encodedIndex] = nextByte;
|
||||
}
|
||||
++encodedIndex;
|
||||
++sourceIndex;
|
||||
}
|
||||
|
||||
if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
|
||||
if (addStxEtx) {
|
||||
destStream[encodedIndex] = ETX_CHAR;
|
||||
++encodedIndex;
|
||||
}
|
||||
*encodedLen = encodedIndex;
|
||||
return RETURN_OK;
|
||||
}
|
||||
else {
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
|
||||
uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream,
|
||||
uint32_t maxDestStreamlen, uint32_t *decodedLen) {
|
||||
uint32_t encodedIndex = 0, decodedIndex = 0;
|
||||
size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
|
||||
size_t maxDestStreamlen, size_t *decodedLen) {
|
||||
size_t encodedIndex = 0, decodedIndex = 0;
|
||||
uint8_t nextByte;
|
||||
if (*sourceStream != STX) {
|
||||
return RETURN_FAILED;
|
||||
if (*sourceStream != STX_CHAR) {
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
++encodedIndex;
|
||||
|
||||
while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
|
||||
&& (sourceStream[encodedIndex] != ETX)
|
||||
&& (sourceStream[encodedIndex] != STX)) {
|
||||
if (sourceStream[encodedIndex] == DLE) {
|
||||
&& (sourceStream[encodedIndex] != ETX_CHAR)
|
||||
&& (sourceStream[encodedIndex] != STX_CHAR)) {
|
||||
if (sourceStream[encodedIndex] == DLE_CHAR) {
|
||||
nextByte = sourceStream[encodedIndex + 1];
|
||||
if (nextByte == 0x10) {
|
||||
// The next byte is a DLE character that was escaped by another
|
||||
// DLE character, so we can write it to the destination stream.
|
||||
if (nextByte == DLE_CHAR) {
|
||||
destStream[decodedIndex] = nextByte;
|
||||
} else {
|
||||
if ((nextByte == 0x42) || (nextByte == 0x43)
|
||||
|| (nextByte == 0x4D)) {
|
||||
}
|
||||
else {
|
||||
/* The next byte is a STX, DTX or 0x0D character which
|
||||
* was escaped by a DLE character. The actual byte was
|
||||
* also encoded by adding + 0x40 to prevent having control chars,
|
||||
* in the stream at all, so we convert it back. */
|
||||
if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) {
|
||||
destStream[decodedIndex] = nextByte - 0x40;
|
||||
} else {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
else {
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
}
|
||||
++encodedIndex;
|
||||
} else {
|
||||
}
|
||||
else {
|
||||
destStream[decodedIndex] = sourceStream[encodedIndex];
|
||||
}
|
||||
|
||||
++encodedIndex;
|
||||
++decodedIndex;
|
||||
}
|
||||
if (sourceStream[encodedIndex] != ETX) {
|
||||
return RETURN_FAILED;
|
||||
} else {
|
||||
|
||||
if (sourceStream[encodedIndex] != ETX_CHAR) {
|
||||
*readLen = ++encodedIndex;
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
else {
|
||||
*readLen = ++encodedIndex;
|
||||
*decodedLen = decodedIndex;
|
||||
return RETURN_OK;
|
||||
}
|
||||
}
|
||||
|
||||
ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
|
||||
uint32_t sourceLen, uint8_t* destStream, uint32_t maxDestLen,
|
||||
uint32_t* encodedLen, bool addStxEtx) {
|
||||
if (maxDestLen < 2) {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
uint32_t encodedIndex = 0, sourceIndex = 0;
|
||||
uint8_t nextByte;
|
||||
if (addStxEtx) {
|
||||
destStream[0] = STX;
|
||||
++encodedIndex;
|
||||
}
|
||||
while ((encodedIndex < maxDestLen) && (sourceIndex < sourceLen)) {
|
||||
nextByte = sourceStream[sourceIndex];
|
||||
if ((nextByte == STX) || (nextByte == ETX) || (nextByte == 0x0D)) {
|
||||
if (encodedIndex + 1 >= maxDestLen) {
|
||||
return RETURN_FAILED;
|
||||
} else {
|
||||
destStream[encodedIndex] = DLE;
|
||||
++encodedIndex;
|
||||
destStream[encodedIndex] = nextByte + 0x40;
|
||||
}
|
||||
} else if (nextByte == DLE) {
|
||||
if (encodedIndex + 1 >= maxDestLen) {
|
||||
return RETURN_FAILED;
|
||||
} else {
|
||||
destStream[encodedIndex] = DLE;
|
||||
++encodedIndex;
|
||||
destStream[encodedIndex] = DLE;
|
||||
}
|
||||
} else {
|
||||
destStream[encodedIndex] = nextByte;
|
||||
}
|
||||
++encodedIndex;
|
||||
++sourceIndex;
|
||||
}
|
||||
if ((sourceIndex == sourceLen) && (encodedIndex < maxDestLen)) {
|
||||
if (addStxEtx) {
|
||||
destStream[encodedIndex] = ETX;
|
||||
++encodedIndex;
|
||||
}
|
||||
*encodedLen = encodedIndex;
|
||||
return RETURN_OK;
|
||||
} else {
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
}
|
||||
|
@ -1,25 +1,79 @@
|
||||
#ifndef DLEENCODER_H_
|
||||
#define DLEENCODER_H_
|
||||
#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_
|
||||
#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_
|
||||
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include <cstddef>
|
||||
|
||||
/**
|
||||
* @brief This DLE Encoder (Data Link Encoder) can be used to encode and
|
||||
* decode arbitrary data with ASCII control characters
|
||||
* @details
|
||||
* List of control codes:
|
||||
* https://en.wikipedia.org/wiki/C0_and_C1_control_codes
|
||||
*
|
||||
* This encoder can be used to achieve a basic transport layer when using
|
||||
* char based transmission systems.
|
||||
* The passed source strean is converted into a encoded stream by adding
|
||||
* a STX marker at the start of the stream and an ETX marker at the end of
|
||||
* the stream. Any STX, ETX, DLE and CR occurrences in the source stream are
|
||||
* escaped by a DLE character. The encoder also replaces escaped control chars
|
||||
* by another char, so STX, ETX and CR should not appear anywhere in the actual
|
||||
* encoded data stream.
|
||||
*
|
||||
* When using a strictly char based reception of packets encoded with DLE,
|
||||
* STX can be used to notify a reader that actual data will start to arrive
|
||||
* while ETX can be used to notify the reader that the data has ended.
|
||||
*/
|
||||
class DleEncoder: public HasReturnvaluesIF {
|
||||
private:
|
||||
DleEncoder();
|
||||
virtual ~DleEncoder();
|
||||
|
||||
public:
|
||||
static const uint8_t STX = 0x02;
|
||||
static const uint8_t ETX = 0x03;
|
||||
static const uint8_t DLE = 0x10;
|
||||
static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER;
|
||||
static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01);
|
||||
static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02);
|
||||
|
||||
//! Start Of Text character. First character is encoded stream
|
||||
static constexpr uint8_t STX_CHAR = 0x02;
|
||||
//! End Of Text character. Last character in encoded stream
|
||||
static constexpr uint8_t ETX_CHAR = 0x03;
|
||||
//! Data Link Escape character. Used to escape STX, ETX and DLE occurrences
|
||||
//! in the source stream.
|
||||
static constexpr uint8_t DLE_CHAR = 0x10;
|
||||
static constexpr uint8_t CARRIAGE_RETURN = 0x0D;
|
||||
|
||||
/**
|
||||
* Encodes the give data stream by preceding it with the STX marker
|
||||
* and ending it with an ETX marker. STX, ETX and DLE characters inside
|
||||
* the stream are escaped by DLE characters and also replaced by adding
|
||||
* 0x40 (which is reverted in the decoding process).
|
||||
* @param sourceStream
|
||||
* @param sourceLen
|
||||
* @param destStream
|
||||
* @param maxDestLen
|
||||
* @param encodedLen
|
||||
* @param addStxEtx
|
||||
* Adding STX and ETX can be omitted, if they are added manually.
|
||||
* @return
|
||||
*/
|
||||
static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen,
|
||||
uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
|
||||
bool addStxEtx = true);
|
||||
|
||||
/**
|
||||
* Converts an encoded stream back.
|
||||
* @param sourceStream
|
||||
* @param sourceStreamLen
|
||||
* @param readLen
|
||||
* @param destStream
|
||||
* @param maxDestStreamlen
|
||||
* @param decodedLen
|
||||
* @return
|
||||
*/
|
||||
static ReturnValue_t decode(const uint8_t *sourceStream,
|
||||
uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream,
|
||||
uint32_t maxDestStreamlen, uint32_t *decodedLen);
|
||||
|
||||
static ReturnValue_t encode(const uint8_t *sourceStream, uint32_t sourceLen,
|
||||
uint8_t *destStream, uint32_t maxDestLen, uint32_t *encodedLen,
|
||||
bool addStxEtx = true);
|
||||
size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
|
||||
size_t maxDestStreamlen, size_t *decodedLen);
|
||||
};
|
||||
|
||||
#endif /* DLEENCODER_H_ */
|
||||
#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */
|
||||
|
34
globalfunctions/PeriodicOperationDivider.cpp
Normal file
34
globalfunctions/PeriodicOperationDivider.cpp
Normal file
@ -0,0 +1,34 @@
|
||||
#include "PeriodicOperationDivider.h"
|
||||
|
||||
|
||||
PeriodicOperationDivider::PeriodicOperationDivider(uint32_t divider,
|
||||
bool resetAutomatically): resetAutomatically(resetAutomatically),
|
||||
counter(divider), divider(divider) {
|
||||
}
|
||||
|
||||
bool PeriodicOperationDivider::checkAndIncrement() {
|
||||
if(counter >= divider) {
|
||||
if(resetAutomatically) {
|
||||
counter = 0;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
counter ++;
|
||||
return false;
|
||||
}
|
||||
|
||||
void PeriodicOperationDivider::resetCounter() {
|
||||
counter = 0;
|
||||
}
|
||||
|
||||
void PeriodicOperationDivider::setDivider(uint32_t newDivider) {
|
||||
divider = newDivider;
|
||||
}
|
||||
|
||||
uint32_t PeriodicOperationDivider::getCounter() const {
|
||||
return counter;
|
||||
}
|
||||
|
||||
uint32_t PeriodicOperationDivider::getDivider() const {
|
||||
return divider;
|
||||
}
|
55
globalfunctions/PeriodicOperationDivider.h
Normal file
55
globalfunctions/PeriodicOperationDivider.h
Normal file
@ -0,0 +1,55 @@
|
||||
#ifndef FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_
|
||||
#define FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
/**
|
||||
* @brief Lightweight helper class to facilitate periodic operation with
|
||||
* decreased frequencies.
|
||||
* @details
|
||||
* This class is useful to perform operations which have to be performed
|
||||
* with a reduced frequency, like debugging printouts in high periodic tasks
|
||||
* or low priority operations.
|
||||
*/
|
||||
class PeriodicOperationDivider {
|
||||
public:
|
||||
/**
|
||||
* Initialize with the desired divider and specify whether the internal
|
||||
* counter will be reset automatically.
|
||||
* @param divider
|
||||
* @param resetAutomatically
|
||||
*/
|
||||
PeriodicOperationDivider(uint32_t divider, bool resetAutomatically = true);
|
||||
|
||||
/**
|
||||
* Check whether operation is necessary.
|
||||
* If an operation is necessary and the class has been
|
||||
* configured to be reset automatically, the counter will be reset.
|
||||
* If not, the counter will be incremented.
|
||||
* @return
|
||||
* -@c true if the counter is larger or equal to the divider
|
||||
* -@c false otherwise
|
||||
*/
|
||||
bool checkAndIncrement();
|
||||
|
||||
/**
|
||||
* Can be used to reset the counter to 0 manually.
|
||||
*/
|
||||
void resetCounter();
|
||||
uint32_t getCounter() const;
|
||||
|
||||
/**
|
||||
* Can be used to set a new divider value.
|
||||
* @param newDivider
|
||||
*/
|
||||
void setDivider(uint32_t newDivider);
|
||||
uint32_t getDivider() const;
|
||||
private:
|
||||
bool resetAutomatically = true;
|
||||
uint32_t counter = 0;
|
||||
uint32_t divider = 0;
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif /* FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_ */
|
@ -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_ */
|
||||
|
@ -64,6 +64,11 @@ ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) {
|
||||
void PeriodicTask::taskFunctionality() {
|
||||
TickType_t xLastWakeTime;
|
||||
const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.);
|
||||
|
||||
for (auto const &object: objectList) {
|
||||
object->initializeAfterTaskCreation();
|
||||
}
|
||||
|
||||
/* The xLastWakeTime variable needs to be initialized with the current tick
|
||||
count. Note that this is the only time the variable is written to
|
||||
explicitly. After this assignment, xLastWakeTime is updated automatically
|
||||
|
@ -1,11 +1,10 @@
|
||||
#ifndef FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_
|
||||
#define FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_
|
||||
#ifndef FSFW_OSAL_FREERTOS_PERIODICTASK_H_
|
||||
#define FSFW_OSAL_FREERTOS_PERIODICTASK_H_
|
||||
|
||||
#include "FreeRTOSTaskIF.h"
|
||||
#include "../../objectmanager/ObjectManagerIF.h"
|
||||
#include "../../tasks/PeriodicTaskIF.h"
|
||||
#include "../../tasks/Typedef.h"
|
||||
#include "FreeRTOSTaskIF.h"
|
||||
|
||||
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/task.h>
|
||||
@ -24,7 +23,6 @@ public:
|
||||
/**
|
||||
* Keep in Mind that you need to call before this vTaskStartScheduler()!
|
||||
* A lot of task parameters are set in "FreeRTOSConfig.h".
|
||||
* TODO: why does this need to be called before vTaskStartScheduler?
|
||||
* @details
|
||||
* The class is initialized without allocated objects.
|
||||
* These need to be added with #addComponent.
|
||||
@ -125,4 +123,4 @@ protected:
|
||||
void handleMissedDeadline();
|
||||
};
|
||||
|
||||
#endif /* PERIODICTASK_H_ */
|
||||
#endif /* FSFW_OSAL_FREERTOS_PERIODICTASK_H_ */
|
||||
|
@ -5,8 +5,8 @@
|
||||
|
||||
PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_,
|
||||
size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()):
|
||||
PosixThread(name_,priority_,stackSize_),objectList(),started(false),
|
||||
periodMs(period_),deadlineMissedFunc(deadlineMissedFunc_) {
|
||||
PosixThread(name_, priority_, stackSize_), objectList(), started(false),
|
||||
periodMs(period_), deadlineMissedFunc(deadlineMissedFunc_) {
|
||||
}
|
||||
|
||||
PeriodicPosixTask::~PeriodicPosixTask() {
|
||||
@ -25,6 +25,8 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) {
|
||||
ExecutableObjectIF* newObject = objectManager->get<ExecutableObjectIF>(
|
||||
object);
|
||||
if (newObject == nullptr) {
|
||||
sif::error << "PeriodicTask::addComponent: Invalid object. Make sure"
|
||||
<< " it implements ExecutableObjectIF!" << std::endl;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
objectList.push_back(newObject);
|
||||
@ -38,35 +40,41 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) {
|
||||
}
|
||||
|
||||
|
||||
ReturnValue_t PeriodicPosixTask::startTask(void){
|
||||
ReturnValue_t PeriodicPosixTask::startTask(void) {
|
||||
started = true;
|
||||
//sif::info << stackSize << std::endl;
|
||||
PosixThread::createTask(&taskEntryPoint,this);
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void PeriodicPosixTask::taskFunctionality(void){
|
||||
if(!started){
|
||||
void PeriodicPosixTask::taskFunctionality(void) {
|
||||
if(not started) {
|
||||
suspend();
|
||||
}
|
||||
|
||||
for (auto const &object: objectList) {
|
||||
object->initializeAfterTaskCreation();
|
||||
}
|
||||
|
||||
uint64_t lastWakeTime = getCurrentMonotonicTimeMs();
|
||||
//The task's "infinite" inner loop is entered.
|
||||
while (1) {
|
||||
for (ObjectList::iterator it = objectList.begin();
|
||||
it != objectList.end(); ++it) {
|
||||
(*it)->performOperation();
|
||||
for (auto const &object: objectList) {
|
||||
object->performOperation();
|
||||
}
|
||||
if(!PosixThread::delayUntil(&lastWakeTime,periodMs)){
|
||||
|
||||
if(not PosixThread::delayUntil(&lastWakeTime, periodMs)){
|
||||
char name[20] = {0};
|
||||
int status = pthread_getname_np(pthread_self(),name,sizeof(name));
|
||||
if(status==0){
|
||||
int status = pthread_getname_np(pthread_self(), name, sizeof(name));
|
||||
if(status == 0) {
|
||||
sif::error << "PeriodicPosixTask " << name << ": Deadline "
|
||||
"missed." << std::endl;
|
||||
}else{
|
||||
}
|
||||
else {
|
||||
sif::error << "PeriodicPosixTask X: Deadline missed. " <<
|
||||
status << std::endl;
|
||||
}
|
||||
if (this->deadlineMissedFunc != NULL) {
|
||||
if (this->deadlineMissedFunc != nullptr) {
|
||||
this->deadlineMissedFunc();
|
||||
}
|
||||
}
|
||||
|
@ -32,7 +32,7 @@ public:
|
||||
* The address of the task object is passed as an argument
|
||||
* to the system call.
|
||||
*/
|
||||
ReturnValue_t startTask(void);
|
||||
ReturnValue_t startTask() override;
|
||||
/**
|
||||
* Adds an object to the list of objects to be executed.
|
||||
* The objects are executed in the order added.
|
||||
|
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_ */
|
@ -64,6 +64,7 @@ enum {
|
||||
LOCAL_POOL_OWNER_IF, //LPIF 58
|
||||
POOL_VARIABLE_IF, //PVA 59
|
||||
HOUSEKEEPING_MANAGER, //HKM 60
|
||||
DLE_ENCODER, //DLEE 61
|
||||
FW_CLASS_ID_COUNT //is actually count + 1 !
|
||||
|
||||
};
|
||||
|
@ -1,11 +1,11 @@
|
||||
#ifndef SERIALIZEADAPTER_H_
|
||||
#define SERIALIZEADAPTER_H_
|
||||
#ifndef _FSFW_SERIALIZE_SERIALIZEADAPTER_H_
|
||||
#define _FSFW_SERIALIZE_SERIALIZEADAPTER_H_
|
||||
|
||||
#include "../container/IsDerivedFrom.h"
|
||||
#include "../returnvalues/HasReturnvaluesIF.h"
|
||||
#include "EndianConverter.h"
|
||||
#include "SerializeIF.h"
|
||||
#include <string.h>
|
||||
#include <cstddef>
|
||||
#include <type_traits>
|
||||
|
||||
/**
|
||||
* \ingroup serialize
|
||||
@ -13,36 +13,91 @@
|
||||
|
||||
class SerializeAdapter {
|
||||
public:
|
||||
/***
|
||||
* This function can be used to serialize a trivial copy-able type or a child of SerializeIF.
|
||||
* The right template to be called is determined in the function itself.
|
||||
* For objects of non trivial copy-able type this function is almost never called by the user directly.
|
||||
* Instead helpers for specific types like SerialArrayListAdapter or SerialLinkedListAdapter is the right choice here.
|
||||
*
|
||||
* @param[in] object Object to serialize, the used type is deduced from this pointer
|
||||
* @param[in/out] buffer Buffer to serialize into. Will be moved by the function.
|
||||
* @param[in/out] size Size of current written buffer. Will be incremented by the function.
|
||||
* @param[in] maxSize Max size of Buffer
|
||||
* @param[in] streamEndianness Endianness of serialized element as in according to SerializeIF::Endianness
|
||||
* @return
|
||||
* - @c BUFFER_TOO_SHORT The given buffer in is too short
|
||||
* - @c RETURN_FAILED Generic Error
|
||||
* - @c RETURN_OK Successful serialization
|
||||
*/
|
||||
template<typename T>
|
||||
static ReturnValue_t serialize(const T *object, uint8_t **buffer,
|
||||
size_t *size, size_t maxSize, SerializeIF::Endianness streamEndianness) {
|
||||
InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
|
||||
size_t *size, size_t maxSize,
|
||||
SerializeIF::Endianness streamEndianness) {
|
||||
InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
|
||||
return adapter.serialize(object, buffer, size, maxSize,
|
||||
streamEndianness);
|
||||
}
|
||||
/**
|
||||
* Function to return the serialized size of the object in the pointer.
|
||||
* May be a trivially copy-able object or a Child of SerializeIF
|
||||
*
|
||||
* @param object Pointer to Object
|
||||
* @return Serialized size of object
|
||||
*/
|
||||
template<typename T>
|
||||
static uint32_t getSerializedSize(const T *object) {
|
||||
InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
|
||||
static size_t getSerializedSize(const T *object){
|
||||
InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
|
||||
return adapter.getSerializedSize(object);
|
||||
}
|
||||
/**
|
||||
* @brief
|
||||
* Deserializes a object from a given buffer of given size.
|
||||
* Object Must be trivially copy-able or a child of SerializeIF.
|
||||
*
|
||||
* @details
|
||||
* Buffer will be moved to the current read location. Size will be decreased by the function.
|
||||
*
|
||||
* @param[in/out] buffer Buffer to deSerialize from. Will be moved by the function.
|
||||
* @param[in/out] size Remaining size of the buffer to read from. Will be decreased by function.
|
||||
* @param[in] streamEndianness Endianness as in according to SerializeIF::Endianness
|
||||
* @return
|
||||
* - @c STREAM_TOO_SHORT The input stream is too short to deSerialize the object
|
||||
* - @c TOO_MANY_ELEMENTS The buffer has more inputs than expected
|
||||
* - @c RETURN_FAILED Generic Error
|
||||
* - @c RETURN_OK Successful deserialization
|
||||
*/
|
||||
template<typename T>
|
||||
static ReturnValue_t deSerialize(T *object, const uint8_t **buffer,
|
||||
size_t *size, SerializeIF::Endianness streamEndianness) {
|
||||
InternalSerializeAdapter<T, IsDerivedFrom<T, SerializeIF>::Is> adapter;
|
||||
InternalSerializeAdapter<T, std::is_base_of<SerializeIF, T>::value> adapter;
|
||||
return adapter.deSerialize(object, buffer, size, streamEndianness);
|
||||
}
|
||||
private:
|
||||
template<typename T, int>
|
||||
class InternalSerializeAdapter {
|
||||
/**
|
||||
* Internal template to deduce the right function calls at compile time
|
||||
*/
|
||||
template<typename T, bool> class InternalSerializeAdapter;
|
||||
|
||||
/**
|
||||
* Template to be used if T is not a child of SerializeIF
|
||||
*
|
||||
* @tparam T T must be trivially_copyable
|
||||
*/
|
||||
template<typename T>
|
||||
class InternalSerializeAdapter<T, false> {
|
||||
static_assert (std::is_trivially_copyable<T>::value,
|
||||
"If a type needs to be serialized it must be a child of SerializeIF or trivially copy-able");
|
||||
public:
|
||||
static ReturnValue_t serialize(const T *object, uint8_t **buffer,
|
||||
size_t *size, size_t max_size, SerializeIF::Endianness streamEndianness) {
|
||||
size_t *size, size_t max_size,
|
||||
SerializeIF::Endianness streamEndianness) {
|
||||
size_t ignoredSize = 0;
|
||||
if (size == NULL) {
|
||||
if (size == nullptr) {
|
||||
size = &ignoredSize;
|
||||
}
|
||||
//TODO check integer overflow of *size
|
||||
if (sizeof(T) + *size <= max_size) {
|
||||
//Check remaining size is large enough and check integer overflow of *size
|
||||
size_t newSize = sizeof(T) + *size;
|
||||
if ((newSize <= max_size) and (newSize > *size)) {
|
||||
T tmp;
|
||||
switch (streamEndianness) {
|
||||
case SerializeIF::Endianness::BIG:
|
||||
@ -94,22 +149,26 @@ private:
|
||||
uint32_t getSerializedSize(const T *object) {
|
||||
return sizeof(T);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/**
|
||||
* Template for objects that inherit from SerializeIF
|
||||
*
|
||||
* @tparam T A child of SerializeIF
|
||||
*/
|
||||
template<typename T>
|
||||
class InternalSerializeAdapter<T, 1> {
|
||||
class InternalSerializeAdapter<T, true> {
|
||||
public:
|
||||
ReturnValue_t serialize(const T *object, uint8_t **buffer,
|
||||
size_t *size, size_t max_size,
|
||||
ReturnValue_t serialize(const T *object, uint8_t **buffer, size_t *size,
|
||||
size_t max_size,
|
||||
SerializeIF::Endianness streamEndianness) const {
|
||||
size_t ignoredSize = 0;
|
||||
if (size == NULL) {
|
||||
if (size == nullptr) {
|
||||
size = &ignoredSize;
|
||||
}
|
||||
return object->serialize(buffer, size, max_size, streamEndianness);
|
||||
}
|
||||
uint32_t getSerializedSize(const T *object) const {
|
||||
size_t getSerializedSize(const T *object) const {
|
||||
return object->getSerializedSize();
|
||||
}
|
||||
|
||||
@ -120,4 +179,4 @@ private:
|
||||
};
|
||||
};
|
||||
|
||||
#endif /* SERIALIZEADAPTER_H_ */
|
||||
#endif /* _FSFW_SERIALIZE_SERIALIZEADAPTER_H_ */
|
||||
|
@ -43,7 +43,7 @@ public:
|
||||
* @param[in] maxSize The size of the buffer that is allowed to be used for serialize.
|
||||
* @param[in] streamEndianness Endianness of the serialized data according to SerializeIF::Endianness
|
||||
* @return
|
||||
* - @¢ BUFFER_TOO_SHORT The given buffer in is too short
|
||||
* - @c BUFFER_TOO_SHORT The given buffer in is too short
|
||||
* - @c RETURN_FAILED Generic error
|
||||
* - @c RETURN_OK Successful serialization
|
||||
*/
|
||||
|
@ -550,7 +550,7 @@ Mode_t Subsystem::getFallbackSequence(Mode_t sequence) {
|
||||
for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
|
||||
iter != modeSequences.end(); ++iter) {
|
||||
if (iter.value->first == sequence) {
|
||||
return iter->fallbackSequence;
|
||||
return iter->second.fallbackSequence;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
@ -559,7 +559,7 @@ Mode_t Subsystem::getFallbackSequence(Mode_t sequence) {
|
||||
bool Subsystem::isFallbackSequence(Mode_t SequenceId) {
|
||||
for (FixedMap<Mode_t, SequenceInfo>::Iterator iter = modeSequences.begin();
|
||||
iter != modeSequences.end(); iter++) {
|
||||
if (iter->fallbackSequence == SequenceId) {
|
||||
if (iter->second.fallbackSequence == SequenceId) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
@ -122,8 +122,8 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
|
||||
|
||||
|
||||
// Implemented by child class, specifies what to do with reply.
|
||||
ReturnValue_t result = handleReply(reply, iter->command, &iter->state,
|
||||
&nextCommand, iter->objectId, &isStep);
|
||||
ReturnValue_t result = handleReply(reply, iter->second.command, &iter->second.state,
|
||||
&nextCommand, iter->second.objectId, &isStep);
|
||||
|
||||
/* If the child implementation does not implement special handling for
|
||||
* rejected replies (RETURN_FAILED or INVALID_REPLY is returned), a
|
||||
@ -132,7 +132,7 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
|
||||
if((reply->getCommand() == CommandMessage::REPLY_REJECTED) and
|
||||
(result == RETURN_FAILED or result == INVALID_REPLY)) {
|
||||
result = reply->getReplyRejectedReason();
|
||||
failureParameter1 = iter->command;
|
||||
failureParameter1 = iter->second.command;
|
||||
}
|
||||
|
||||
switch (result) {
|
||||
@ -149,14 +149,14 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
|
||||
default:
|
||||
if (isStep) {
|
||||
verificationReporter.sendFailureReport(
|
||||
TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
|
||||
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
|
||||
result, ++iter->step, failureParameter1,
|
||||
TC_VERIFY::PROGRESS_FAILURE, iter->second.tcInfo.ackFlags,
|
||||
iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
|
||||
result, ++iter->second.step, failureParameter1,
|
||||
failureParameter2);
|
||||
} else {
|
||||
verificationReporter.sendFailureReport(
|
||||
TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
|
||||
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
|
||||
TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
|
||||
iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
|
||||
result, 0, failureParameter1, failureParameter2);
|
||||
}
|
||||
failureParameter1 = 0;
|
||||
@ -170,7 +170,7 @@ void CommandingServiceBase::handleCommandMessage(CommandMessage* reply) {
|
||||
void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
|
||||
CommandMapIter iter, CommandMessage* nextCommand,
|
||||
CommandMessage* reply, bool& isStep) {
|
||||
iter->command = nextCommand->getCommand();
|
||||
iter->second.command = nextCommand->getCommand();
|
||||
|
||||
// In case a new command is to be sent immediately, this is performed here.
|
||||
// If no new command is sent, only analyse reply result by initializing
|
||||
@ -185,14 +185,14 @@ void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
|
||||
if (isStep and result != NO_STEP_MESSAGE) {
|
||||
verificationReporter.sendSuccessReport(
|
||||
TC_VERIFY::PROGRESS_SUCCESS,
|
||||
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
|
||||
iter->tcInfo.tcSequenceControl, ++iter->step);
|
||||
iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
||||
iter->second.tcInfo.tcSequenceControl, ++iter->second.step);
|
||||
}
|
||||
else {
|
||||
verificationReporter.sendSuccessReport(
|
||||
TC_VERIFY::COMPLETION_SUCCESS,
|
||||
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
|
||||
iter->tcInfo.tcSequenceControl, 0);
|
||||
iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
||||
iter->second.tcInfo.tcSequenceControl, 0);
|
||||
checkAndExecuteFifo(iter);
|
||||
}
|
||||
}
|
||||
@ -200,16 +200,16 @@ void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result,
|
||||
if (isStep) {
|
||||
nextCommand->clearCommandMessage();
|
||||
verificationReporter.sendFailureReport(
|
||||
TC_VERIFY::PROGRESS_FAILURE, iter->tcInfo.ackFlags,
|
||||
iter->tcInfo.tcPacketId,
|
||||
iter->tcInfo.tcSequenceControl, sendResult,
|
||||
++iter->step, failureParameter1, failureParameter2);
|
||||
TC_VERIFY::PROGRESS_FAILURE, iter->second.tcInfo.ackFlags,
|
||||
iter->second.tcInfo.tcPacketId,
|
||||
iter->second.tcInfo.tcSequenceControl, sendResult,
|
||||
++iter->second.step, failureParameter1, failureParameter2);
|
||||
} else {
|
||||
nextCommand->clearCommandMessage();
|
||||
verificationReporter.sendFailureReport(
|
||||
TC_VERIFY::COMPLETION_FAILURE,
|
||||
iter->tcInfo.ackFlags, iter->tcInfo.tcPacketId,
|
||||
iter->tcInfo.tcSequenceControl, sendResult, 0,
|
||||
iter->second.tcInfo.ackFlags, iter->second.tcInfo.tcPacketId,
|
||||
iter->second.tcInfo.tcSequenceControl, sendResult, 0,
|
||||
failureParameter1, failureParameter2);
|
||||
}
|
||||
failureParameter1 = 0;
|
||||
@ -248,7 +248,7 @@ void CommandingServiceBase::handleRequestQueue() {
|
||||
iter = commandMap.find(queue);
|
||||
|
||||
if (iter != commandMap.end()) {
|
||||
result = iter->fifo.insert(address);
|
||||
result = iter->second.fifo.insert(address);
|
||||
if (result != RETURN_OK) {
|
||||
rejectPacket(TC_VERIFY::START_FAILURE, &packet, OBJECT_BUSY);
|
||||
}
|
||||
@ -316,11 +316,11 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
|
||||
CommandMapIter iter) {
|
||||
ReturnValue_t result = RETURN_OK;
|
||||
CommandMessage command;
|
||||
iter->subservice = storedPacket->getSubService();
|
||||
result = prepareCommand(&command, iter->subservice,
|
||||
iter->second.subservice = storedPacket->getSubService();
|
||||
result = prepareCommand(&command, iter->second.subservice,
|
||||
storedPacket->getApplicationData(),
|
||||
storedPacket->getApplicationDataSize(), &iter->state,
|
||||
iter->objectId);
|
||||
storedPacket->getApplicationDataSize(), &iter->second.state,
|
||||
iter->second.objectId);
|
||||
|
||||
ReturnValue_t sendResult = RETURN_OK;
|
||||
switch (result) {
|
||||
@ -330,13 +330,13 @@ void CommandingServiceBase::startExecution(TcPacketStored *storedPacket,
|
||||
&command);
|
||||
}
|
||||
if (sendResult == RETURN_OK) {
|
||||
Clock::getUptime(&iter->uptimeOfStart);
|
||||
iter->step = 0;
|
||||
iter->subservice = storedPacket->getSubService();
|
||||
iter->command = command.getCommand();
|
||||
iter->tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
|
||||
iter->tcInfo.tcPacketId = storedPacket->getPacketId();
|
||||
iter->tcInfo.tcSequenceControl =
|
||||
Clock::getUptime(&iter->second.uptimeOfStart);
|
||||
iter->second.step = 0;
|
||||
iter->second.subservice = storedPacket->getSubService();
|
||||
iter->second.command = command.getCommand();
|
||||
iter->second.tcInfo.ackFlags = storedPacket->getAcknowledgeFlags();
|
||||
iter->second.tcInfo.tcPacketId = storedPacket->getPacketId();
|
||||
iter->second.tcInfo.tcSequenceControl =
|
||||
storedPacket->getPacketSequenceControl();
|
||||
acceptPacket(TC_VERIFY::START_SUCCESS, storedPacket);
|
||||
} else {
|
||||
@ -386,7 +386,7 @@ void CommandingServiceBase::acceptPacket(uint8_t reportId,
|
||||
|
||||
void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter iter) {
|
||||
store_address_t address;
|
||||
if (iter->fifo.retrieve(&address) != RETURN_OK) {
|
||||
if (iter->second.fifo.retrieve(&address) != RETURN_OK) {
|
||||
commandMap.erase(&iter);
|
||||
} else {
|
||||
TcPacketStored newPacket(address);
|
||||
@ -412,10 +412,10 @@ void CommandingServiceBase::checkTimeout() {
|
||||
Clock::getUptime(&uptime);
|
||||
CommandMapIter iter;
|
||||
for (iter = commandMap.begin(); iter != commandMap.end(); ++iter) {
|
||||
if ((iter->uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
|
||||
if ((iter->second.uptimeOfStart + (timeoutSeconds * 1000)) < uptime) {
|
||||
verificationReporter.sendFailureReport(
|
||||
TC_VERIFY::COMPLETION_FAILURE, iter->tcInfo.ackFlags,
|
||||
iter->tcInfo.tcPacketId, iter->tcInfo.tcSequenceControl,
|
||||
TC_VERIFY::COMPLETION_FAILURE, iter->second.tcInfo.ackFlags,
|
||||
iter->second.tcInfo.tcPacketId, iter->second.tcInfo.tcSequenceControl,
|
||||
TIMEOUT);
|
||||
checkAndExecuteFifo(iter);
|
||||
}
|
||||
|
@ -211,8 +211,7 @@ protected:
|
||||
|
||||
virtual void doPeriodicOperation();
|
||||
|
||||
|
||||
struct CommandInfo {
|
||||
struct CommandInfo: public SerializeIF{
|
||||
struct tcInfo {
|
||||
uint8_t ackFlags;
|
||||
uint16_t tcPacketId;
|
||||
@ -225,6 +224,20 @@ protected:
|
||||
Command_t command;
|
||||
object_id_t objectId;
|
||||
FIFO<store_address_t, 3> fifo;
|
||||
|
||||
virtual ReturnValue_t serialize(uint8_t **buffer, size_t *size,
|
||||
size_t maxSize, Endianness streamEndianness) const override{
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
};
|
||||
|
||||
virtual size_t getSerializedSize() const override {
|
||||
return 0;
|
||||
};
|
||||
|
||||
virtual ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size,
|
||||
Endianness streamEndianness) override{
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
};
|
||||
};
|
||||
|
||||
using CommandMapIter = FixedMap<MessageQueueId_t,
|
||||
|
@ -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