KSat/fsfw
15
4
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests 1 Releases Wiki Activity

Merge branch 'mueller/hostosal' of https://egit.irs.uni-stuttgart.de/KSat/fsfw into mueller/hostosal

Browse Source
This commit is contained in:
Robin Müller 2020-09-15 15:28:37 +02:00
commit 51cf13428b
28 changed files with 1072 additions and 382 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
container/ArrayList.h
View File

@ -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;
}

17
container/DynamicFIFO.h
View File

@ -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;
};

12
container/FIFO.h
View File

@ -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;
};

28
container/FixedMap.h
View File

@ -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_ */

23
container/FixedOrderedMultimap.h
View File

@ -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) {

41
container/IsDerivedFrom.h
View File

@ -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_ */

167
globalfunctions/DleEncoder.cpp
View File

@ -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;
}
}

78
globalfunctions/DleEncoder.h
View File

@ -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
View 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
View 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_ */

7
globalfunctions/timevalOperations.cpp
View File

@ -90,3 +90,10 @@ 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;
}

1
globalfunctions/timevalOperations.h
View File

@ -41,6 +41,7 @@ namespace timevalOperations {
* @return seconds
*/
double toDouble(const timeval timeval);
timeval toTimeval(const double seconds);
}
#endif /* TIMEVALOPERATIONS_H_ */

5
osal/FreeRTOS/PeriodicTask.cpp
View File

@ -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

10
osal/FreeRTOS/PeriodicTask.h
View File

@ -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_ */

34
osal/linux/PeriodicPosixTask.cpp
View File

@ -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();
}
}

2
osal/linux/PeriodicPosixTask.h
View File

@ -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
View 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
View 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
View 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
View 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
returnvalues/FwClassIds.h
View File

@ -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 !
};

103
serialize/SerializeAdapter.h
View File

@ -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_ */

2
serialize/SerializeIF.h
View File

@ -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
*/

4
subsystem/Subsystem.cpp
View File

@ -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;
}
}

70
tmtcservices/CommandingServiceBase.cpp
View File

@ -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);
}

17
tmtcservices/CommandingServiceBase.h
View File

@ -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,

108
tmtcservices/TmTcBridge.cpp
View File

@ -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;
}

20
tmtcservices/TmTcBridge.h
View File

@ -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;
};