Compare commits
45 Commits
e227b5dead
...
v2.0.0
Author | SHA1 | Date | |
---|---|---|---|
ab906fa534 | |||
146e1e3282 | |||
f11957d827 | |||
a977302a53 | |||
53400c8bfa | |||
f2d0a0d9ee | |||
9e12f59707 | |||
2439613f21 | |||
1c8f86364d | |||
4e1c52f465 | |||
9f856761e2 | |||
2f119f102d | |||
afb472996c | |||
f76f462022 | |||
0f90d50065 | |||
b0cbd40e64 | |||
1c1433e797 | |||
7671c93095 | |||
ba4249d658 | |||
de7542c9c1 | |||
5a30dd969f | |||
0a2c912f29 | |||
2b15f9e644 | |||
42b5f8a79d | |||
358ee0fbf2 | |||
4f08b2d342 | |||
4b62c8aa81 | |||
1b38f84edc | |||
5064d44999 | |||
322c14d4bb | |||
8ec35f158c | |||
9a25f08fef | |||
f3caa122ae | |||
6e88f8f400 | |||
01762ad222 | |||
71d66c406f | |||
42df77ff32 | |||
85c04dee23 | |||
4c96db847d | |||
0246dccbe9 | |||
52b3d9473e | |||
fc9b85d5db | |||
bfae25ff2d | |||
ea3812fbbd | |||
f40f783cb4 |
@@ -28,7 +28,6 @@ enum GpioTypes {
|
||||
NONE,
|
||||
GPIO_REGULAR_BY_CHIP,
|
||||
GPIO_REGULAR_BY_LABEL,
|
||||
GPIO_REGULAR_BY_LINE_NAME,
|
||||
CALLBACK
|
||||
};
|
||||
|
||||
@@ -76,12 +75,6 @@ public:
|
||||
int initValue, int lineNum): GpioBase(gpioType, consumer, direction, initValue),
|
||||
lineNum(lineNum) {
|
||||
}
|
||||
|
||||
// line number will be configured at a later point for the open by line name configuration
|
||||
GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
|
||||
int initValue): GpioBase(gpioType, consumer, direction, initValue) {
|
||||
}
|
||||
|
||||
int lineNum = 0;
|
||||
struct gpiod_line* lineHandle = nullptr;
|
||||
};
|
||||
@@ -127,27 +120,6 @@ public:
|
||||
std::string label;
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Passing this GPIO configuration to the GPIO IF object will try to open the GPIO by its
|
||||
* line name. This line name can be set in the device tree and must be unique. Otherwise
|
||||
* the driver will open the first line with the given name.
|
||||
*/
|
||||
class GpiodRegularByLineName: public GpiodRegularBase {
|
||||
public:
|
||||
GpiodRegularByLineName(std::string lineName_, std::string consumer_, gpio::Direction direction_,
|
||||
int initValue_) :
|
||||
GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, direction_,
|
||||
initValue_), lineName(lineName_) {
|
||||
}
|
||||
|
||||
GpiodRegularByLineName(std::string lineName_, std::string consumer_) :
|
||||
GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN,
|
||||
gpio::LOW), lineName(lineName_) {
|
||||
}
|
||||
|
||||
std::string lineName;
|
||||
};
|
||||
|
||||
class GpioCallback: public GpioBase {
|
||||
public:
|
||||
GpioCallback(std::string consumer, gpio::Direction direction_, int initValue_,
|
||||
|
@@ -73,7 +73,7 @@ ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(
|
||||
switch (internalState) {
|
||||
case(InternalState::STATE_NONE):
|
||||
case(InternalState::STATE_NORMAL): {
|
||||
return DeviceHandlerBase::NOTHING_TO_SEND;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
case(InternalState::STATE_FIRST_CONTACT): {
|
||||
*id = MGMLIS3MDL::IDENTIFY_DEVICE;
|
||||
|
@@ -2,7 +2,6 @@
|
||||
#define MISSION_DEVICES_MGMRM3100HANDLER_H_
|
||||
|
||||
#include "fsfw/FSFW.h"
|
||||
#include "devices/powerSwitcherList.h"
|
||||
#include "devicedefinitions/MgmRM3100HandlerDefs.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
||||
|
||||
|
@@ -49,7 +49,7 @@ static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_DEFAULT_37HZ_VALUE;
|
||||
|
||||
static constexpr uint8_t MEASUREMENT_REG_START = 0x24;
|
||||
static constexpr uint8_t BIST_REGISTER = 0x33;
|
||||
static constexpr uint8_t DATA_READY_VAL = 0b1000'0000;
|
||||
static constexpr uint8_t DATA_READY_VAL = 0b10000000;
|
||||
static constexpr uint8_t STATUS_REGISTER = 0x34;
|
||||
static constexpr uint8_t REVID_REGISTER = 0x36;
|
||||
|
||||
|
@@ -66,14 +66,6 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) {
|
||||
configureGpioByLabel(gpioConfig.first, *regularGpio);
|
||||
break;
|
||||
}
|
||||
case(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME):{
|
||||
auto regularGpio = dynamic_cast<GpiodRegularByLineName*>(gpioConfig.second);
|
||||
if(regularGpio == nullptr) {
|
||||
return GPIO_INVALID_INSTANCE;
|
||||
}
|
||||
configureGpioByLineName(gpioConfig.first, *regularGpio);
|
||||
break;
|
||||
}
|
||||
case(gpio::GpioTypes::CALLBACK): {
|
||||
auto gpioCallback = dynamic_cast<GpioCallback*>(gpioConfig.second);
|
||||
if(gpioCallback->callback == nullptr) {
|
||||
@@ -92,13 +84,13 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLabel(gpioId_t gpioId,
|
||||
std::string& label = gpioByLabel.label;
|
||||
struct gpiod_chip* chip = gpiod_chip_open_by_label(label.c_str());
|
||||
if (chip == nullptr) {
|
||||
sif::warning << "LinuxLibgpioIF::configureGpioByLabel: Failed to open gpio from gpio "
|
||||
sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open gpio from gpio "
|
||||
<< "group with label " << label << ". Gpio ID: " << gpioId << std::endl;
|
||||
return RETURN_FAILED;
|
||||
|
||||
}
|
||||
std::string failOutput = "label: " + label;
|
||||
return configureRegularGpio(gpioId, chip, gpioByLabel, failOutput);
|
||||
return configureRegularGpio(gpioId, gpioByLabel.gpioType, chip, gpioByLabel, failOutput);
|
||||
}
|
||||
|
||||
ReturnValue_t LinuxLibgpioIF::configureGpioByChip(gpioId_t gpioId,
|
||||
@@ -106,41 +98,16 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByChip(gpioId_t gpioId,
|
||||
std::string& chipname = gpioByChip.chipname;
|
||||
struct gpiod_chip* chip = gpiod_chip_open_by_name(chipname.c_str());
|
||||
if (chip == nullptr) {
|
||||
sif::warning << "LinuxLibgpioIF::configureGpioByChip: Failed to open chip "
|
||||
sif::warning << "LinuxLibgpioIF::configureRegularGpio: Failed to open chip "
|
||||
<< chipname << ". Gpio ID: " << gpioId << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
std::string failOutput = "chipname: " + chipname;
|
||||
return configureRegularGpio(gpioId, chip, gpioByChip, failOutput);
|
||||
return configureRegularGpio(gpioId, gpioByChip.gpioType, chip, gpioByChip, failOutput);
|
||||
}
|
||||
|
||||
ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId,
|
||||
GpiodRegularByLineName &gpioByLineName) {
|
||||
std::string& lineName = gpioByLineName.lineName;
|
||||
char chipname[MAX_CHIPNAME_LENGTH];
|
||||
unsigned int lineOffset;
|
||||
|
||||
int result = gpiod_ctxless_find_line(lineName.c_str(), chipname, MAX_CHIPNAME_LENGTH,
|
||||
&lineOffset);
|
||||
if (result != LINE_FOUND) {
|
||||
parseFindeLineResult(result, lineName);
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
|
||||
gpioByLineName.lineNum = static_cast<int>(lineOffset);
|
||||
|
||||
struct gpiod_chip* chip = gpiod_chip_open_by_name(chipname);
|
||||
if (chip == nullptr) {
|
||||
sif::warning << "LinuxLibgpioIF::configureGpioByLineName: Failed to open chip "
|
||||
<< chipname << ". <Gpio ID: " << gpioId << std::endl;
|
||||
return RETURN_FAILED;
|
||||
}
|
||||
std::string failOutput = "line name: " + lineName;
|
||||
return configureRegularGpio(gpioId, chip, gpioByLineName, failOutput);
|
||||
}
|
||||
|
||||
ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod_chip* chip,
|
||||
GpiodRegularBase& regularGpio, std::string failOutput) {
|
||||
ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, gpio::GpioTypes gpioType,
|
||||
struct gpiod_chip* chip, GpiodRegularBase& regularGpio, std::string failOutput) {
|
||||
unsigned int lineNum;
|
||||
gpio::Direction direction;
|
||||
std::string consumer;
|
||||
@@ -206,9 +173,8 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) {
|
||||
}
|
||||
|
||||
auto gpioType = gpioMapIter->second->gpioType;
|
||||
if (gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME) {
|
||||
if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
|
||||
gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
|
||||
auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
|
||||
if(regularGpio == nullptr) {
|
||||
return GPIO_TYPE_FAILURE;
|
||||
@@ -235,9 +201,8 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) {
|
||||
}
|
||||
|
||||
auto& gpioType = gpioMapIter->second->gpioType;
|
||||
if (gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME) {
|
||||
if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
|
||||
gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
|
||||
auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
|
||||
if(regularGpio == nullptr) {
|
||||
return GPIO_TYPE_FAILURE;
|
||||
@@ -274,11 +239,9 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) {
|
||||
sif::warning << "LinuxLibgpioIF::readGpio: Unknown GPIOD ID " << gpioId << std::endl;
|
||||
return UNKNOWN_GPIO_ID;
|
||||
}
|
||||
|
||||
auto gpioType = gpioMapIter->second->gpioType;
|
||||
if (gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL
|
||||
or gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME) {
|
||||
if(gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_CHIP or
|
||||
gpioType == gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) {
|
||||
auto regularGpio = dynamic_cast<GpiodRegularBase*>(gpioMapIter->second);
|
||||
if(regularGpio == nullptr) {
|
||||
return GPIO_TYPE_FAILURE;
|
||||
@@ -379,21 +342,3 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsCallbackGpio(gpioId_t gpioIdToChe
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void LinuxLibgpioIF::parseFindeLineResult(int result, std::string& lineName) {
|
||||
switch (result) {
|
||||
case LINE_NOT_EXISTS:
|
||||
sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Line with name " << lineName
|
||||
<< " does not exist" << std::endl;
|
||||
break;
|
||||
case LINE_ERROR:
|
||||
sif::warning << "LinuxLibgpioIF::parseFindeLineResult: " << "Line with name "
|
||||
<< lineName << " does not exist" << std::endl;
|
||||
break;
|
||||
default:
|
||||
sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line with "
|
||||
<< "name " << lineName << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
@@ -38,12 +38,6 @@ public:
|
||||
ReturnValue_t readGpio(gpioId_t gpioId, int* gpioState) override;
|
||||
|
||||
private:
|
||||
|
||||
static const size_t MAX_CHIPNAME_LENGTH = 11;
|
||||
static const int LINE_NOT_EXISTS = 0;
|
||||
static const int LINE_ERROR = -1;
|
||||
static const int LINE_FOUND = 1;
|
||||
|
||||
/* Holds the information and configuration of all used GPIOs */
|
||||
GpioUnorderedMap gpioMap;
|
||||
GpioUnorderedMapIter gpioMapIter;
|
||||
@@ -59,10 +53,8 @@ private:
|
||||
|
||||
ReturnValue_t configureGpioByLabel(gpioId_t gpioId, GpiodRegularByLabel& gpioByLabel);
|
||||
ReturnValue_t configureGpioByChip(gpioId_t gpioId, GpiodRegularByChip& gpioByChip);
|
||||
ReturnValue_t configureGpioByLineName(gpioId_t gpioId,
|
||||
GpiodRegularByLineName &gpioByLineName);
|
||||
ReturnValue_t configureRegularGpio(gpioId_t gpioId, struct gpiod_chip* chip,
|
||||
GpiodRegularBase& regularGpio, std::string failOutput);
|
||||
ReturnValue_t configureRegularGpio(gpioId_t gpioId, gpio::GpioTypes gpioType,
|
||||
struct gpiod_chip* chip, GpiodRegularBase& regularGpio, std::string failOutput);
|
||||
|
||||
/**
|
||||
* @brief This function checks if GPIOs are already registered and whether
|
||||
@@ -85,7 +77,6 @@ private:
|
||||
*/
|
||||
ReturnValue_t configureGpios(GpioMap& mapToAdd);
|
||||
|
||||
void parseFindeLineResult(int result, std::string& lineName);
|
||||
};
|
||||
|
||||
#endif /* LINUX_GPIO_LINUXLIBGPIOIF_H_ */
|
||||
|
@@ -16,13 +16,9 @@
|
||||
#cmakedefine FSFW_ADD_MONITORING
|
||||
#cmakedefine FSFW_ADD_SGP4_PROPAGATOR
|
||||
|
||||
#ifndef FSFW_TCP_RECV_WIRETAPPING_ENABLED
|
||||
#define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0
|
||||
#endif
|
||||
|
||||
// Can be used for low-level debugging of the SPI bus
|
||||
#ifndef FSFW_HAL_SPI_WIRETAPPING
|
||||
#define FSFW_HAL_SPI_WIRETAPPING 0
|
||||
#define FSFW_HAL_SPI_WIRETAPPING 0
|
||||
#endif
|
||||
|
||||
#ifndef FSFW_HAL_L3GD20_GYRO_DEBUG
|
||||
|
@@ -3,8 +3,8 @@
|
||||
|
||||
const char* const FSFW_VERSION_NAME = "ASTP";
|
||||
|
||||
#define FSFW_VERSION 1
|
||||
#define FSFW_SUBVERSION 2
|
||||
#define FSFW_VERSION 2
|
||||
#define FSFW_SUBVERSION 0
|
||||
#define FSFW_REVISION 0
|
||||
|
||||
#endif /* FSFW_VERSION_H_ */
|
||||
|
@@ -7,7 +7,7 @@
|
||||
|
||||
/**
|
||||
* @brief Base class to implement reconfiguration and failure handling for
|
||||
* redundant devices by monitoring their modes and health states.
|
||||
* redundant devices by monitoring their modes health states.
|
||||
* @details
|
||||
* Documentation: Dissertation Baetz p.156, 157.
|
||||
*
|
||||
|
@@ -85,10 +85,9 @@ public:
|
||||
* Called by DHB in the GET_WRITE doGetWrite().
|
||||
* Get send confirmation that the data in sendMessage() was sent successfully.
|
||||
* @param cookie
|
||||
* @return
|
||||
* - @c RETURN_OK if data was sent successfully but a reply is expected
|
||||
* - NO_REPLY_EXPECTED if data was sent successfully and no reply is expected
|
||||
* - Everything else to indicate failure
|
||||
* @return - @c RETURN_OK if data was sent successfull
|
||||
* - Everything else triggers falure event with
|
||||
* returnvalue as parameter 1
|
||||
*/
|
||||
virtual ReturnValue_t getSendSuccess(CookieIF *cookie) = 0;
|
||||
|
||||
|
@@ -334,7 +334,8 @@ protected:
|
||||
* - @c RETURN_OK to send command after #rawPacket and #rawPacketLen
|
||||
* have been set.
|
||||
* - @c HasActionsIF::EXECUTION_COMPLETE to generate a finish reply immediately. This can
|
||||
* be used if no reply is expected
|
||||
* be used if no reply is expected. Otherwise, the developer can call #actionHelper.finish
|
||||
* to finish the command handling.
|
||||
* - Anything else triggers an event with the return code as a parameter as well as a
|
||||
* step reply failed with the return code
|
||||
*/
|
||||
|
@@ -120,8 +120,7 @@ public:
|
||||
static const ReturnValue_t WRONG_MODE_FOR_COMMAND = MAKE_RETURN_CODE(0xA5);
|
||||
static const ReturnValue_t TIMEOUT = MAKE_RETURN_CODE(0xA6);
|
||||
static const ReturnValue_t BUSY = MAKE_RETURN_CODE(0xA7);
|
||||
//!< Used to indicate that this is a command-only command.
|
||||
static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8);
|
||||
static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8); //!< Used to indicate that this is a command-only command.
|
||||
static const ReturnValue_t NON_OP_TEMPERATURE = MAKE_RETURN_CODE(0xA9);
|
||||
static const ReturnValue_t COMMAND_NOT_IMPLEMENTED = MAKE_RETURN_CODE(0xAA);
|
||||
|
||||
|
@@ -165,11 +165,9 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_
|
||||
if (sourceStream[encodedIndex++] != STX_CHAR) {
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
while ((encodedIndex < sourceStreamLen)
|
||||
and (decodedIndex < maxDestStreamlen)
|
||||
and (sourceStream[encodedIndex] != ETX_CHAR)
|
||||
and (sourceStream[encodedIndex] != STX_CHAR)) {
|
||||
if (sourceStream[encodedIndex] == DLE_CHAR) {
|
||||
while ((encodedIndex < sourceStreamLen) and (decodedIndex < maxDestStreamlen)) {
|
||||
switch(sourceStream[encodedIndex]) {
|
||||
case(DLE_CHAR): {
|
||||
if(encodedIndex + 1 >= sourceStreamLen) {
|
||||
//reached the end of the sourceStream
|
||||
*readLen = sourceStreamLen;
|
||||
@@ -197,29 +195,33 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_
|
||||
}
|
||||
}
|
||||
++encodedIndex;
|
||||
break;
|
||||
}
|
||||
else {
|
||||
case(STX_CHAR): {
|
||||
*readLen = encodedIndex;
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
case(ETX_CHAR): {
|
||||
*readLen = ++encodedIndex;
|
||||
*decodedLen = decodedIndex;
|
||||
return RETURN_OK;
|
||||
}
|
||||
default: {
|
||||
destStream[decodedIndex] = sourceStream[encodedIndex];
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
++encodedIndex;
|
||||
++decodedIndex;
|
||||
}
|
||||
if (sourceStream[encodedIndex] != ETX_CHAR) {
|
||||
if(decodedIndex == maxDestStreamlen) {
|
||||
//so far we did not find anything wrong here, so let user try again
|
||||
*readLen = 0;
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
else {
|
||||
*readLen = ++encodedIndex;
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
}
|
||||
else {
|
||||
*readLen = ++encodedIndex;
|
||||
*decodedLen = decodedIndex;
|
||||
return RETURN_OK;
|
||||
|
||||
if(decodedIndex == maxDestStreamlen) {
|
||||
//so far we did not find anything wrong here, so let user try again
|
||||
*readLen = 0;
|
||||
return STREAM_TOO_SHORT;
|
||||
} else {
|
||||
*readLen = encodedIndex;
|
||||
return DECODING_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
|
@@ -2,42 +2,43 @@
|
||||
|
||||
|
||||
PeriodicOperationDivider::PeriodicOperationDivider(uint32_t divider,
|
||||
bool resetAutomatically): resetAutomatically(resetAutomatically),
|
||||
counter(divider), divider(divider) {
|
||||
bool resetAutomatically): resetAutomatically(resetAutomatically),
|
||||
counter(divider), divider(divider) {
|
||||
}
|
||||
|
||||
bool PeriodicOperationDivider::checkAndIncrement() {
|
||||
counter++;
|
||||
bool opNecessary = check();
|
||||
if(opNecessary) {
|
||||
if(resetAutomatically) {
|
||||
resetCounter();
|
||||
}
|
||||
}
|
||||
return opNecessary;
|
||||
bool opNecessary = check();
|
||||
if(opNecessary) {
|
||||
if(resetAutomatically) {
|
||||
counter = 0;
|
||||
}
|
||||
return opNecessary;
|
||||
}
|
||||
counter ++;
|
||||
return opNecessary;
|
||||
}
|
||||
|
||||
bool PeriodicOperationDivider::check() {
|
||||
if(counter >= divider) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
if(counter >= divider) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PeriodicOperationDivider::resetCounter() {
|
||||
counter = 0;
|
||||
counter = 0;
|
||||
}
|
||||
|
||||
void PeriodicOperationDivider::setDivider(uint32_t newDivider) {
|
||||
divider = newDivider;
|
||||
divider = newDivider;
|
||||
}
|
||||
|
||||
uint32_t PeriodicOperationDivider::getCounter() const {
|
||||
return counter;
|
||||
return counter;
|
||||
}
|
||||
|
||||
uint32_t PeriodicOperationDivider::getDivider() const {
|
||||
return divider;
|
||||
return divider;
|
||||
}
|
||||
|
@@ -13,51 +13,51 @@
|
||||
*/
|
||||
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);
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* @return
|
||||
* -@c true if the counter is larger or equal to the divider
|
||||
* -@c false otherwise
|
||||
*/
|
||||
bool checkAndIncrement();
|
||||
/**
|
||||
* 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.
|
||||
*
|
||||
* @return
|
||||
* -@c true if the counter is larger or equal to the divider
|
||||
* -@c false otherwise
|
||||
*/
|
||||
bool checkAndIncrement();
|
||||
|
||||
/**
|
||||
* Checks whether an operation is necessary.
|
||||
* This function will not increment the counter!
|
||||
* @return
|
||||
* -@c true if the counter is larger or equal to the divider
|
||||
* -@c false otherwise
|
||||
*/
|
||||
bool check();
|
||||
/**
|
||||
* Checks whether an operation is necessary.
|
||||
* This function will not increment the counter!
|
||||
* @return
|
||||
* -@c true if the counter is larger or equal to the divider
|
||||
* -@c false otherwise
|
||||
*/
|
||||
bool check();
|
||||
|
||||
/**
|
||||
* Can be used to reset the counter to 0 manually.
|
||||
*/
|
||||
void resetCounter();
|
||||
uint32_t getCounter() const;
|
||||
/**
|
||||
* 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;
|
||||
/**
|
||||
* 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;
|
||||
bool resetAutomatically = true;
|
||||
uint32_t counter = 0;
|
||||
uint32_t divider = 0;
|
||||
};
|
||||
|
||||
|
||||
|
@@ -81,7 +81,7 @@ public:
|
||||
* @param args Any other arguments which an implementation might require
|
||||
* @return
|
||||
*/
|
||||
virtual ReturnValue_t removeFile(const char* repositoryPath,
|
||||
virtual ReturnValue_t deleteFile(const char* repositoryPath,
|
||||
const char* filename, void* args = nullptr) = 0;
|
||||
|
||||
/**
|
||||
|
@@ -1,10 +1,8 @@
|
||||
#include "fsfw/osal/common/TcpTmTcServer.h"
|
||||
#include "fsfw/osal/common/TcpTmTcBridge.h"
|
||||
#include "fsfw/osal/common/tcpipHelpers.h"
|
||||
|
||||
#include "fsfw/platform.h"
|
||||
#include "fsfw/FSFW.h"
|
||||
|
||||
#include "TcpTmTcServer.h"
|
||||
#include "TcpTmTcBridge.h"
|
||||
#include "tcpipHelpers.h"
|
||||
|
||||
#include "fsfw/container/SharedRingBuffer.h"
|
||||
#include "fsfw/ipc/MessageQueueSenderIF.h"
|
||||
#include "fsfw/ipc/MutexGuard.h"
|
||||
|
@@ -13,10 +13,8 @@ target_sources(${LIB_FSFW_NAME}
|
||||
QueueFactory.cpp
|
||||
SemaphoreFactory.cpp
|
||||
TaskFactory.cpp
|
||||
Timer.cpp
|
||||
tcpipHelpers.cpp
|
||||
unixUtility.cpp
|
||||
CommandExecutor.cpp
|
||||
)
|
||||
|
||||
find_package(Threads REQUIRED)
|
||||
|
@@ -1,185 +0,0 @@
|
||||
#include "CommandExecutor.h"
|
||||
|
||||
#include "fsfw/serviceinterface.h"
|
||||
#include "fsfw/container/SimpleRingBuffer.h"
|
||||
#include "fsfw/container/DynamicFIFO.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
CommandExecutor::CommandExecutor(const size_t maxSize):
|
||||
readVec(maxSize) {
|
||||
waiter.events = POLLIN;
|
||||
}
|
||||
|
||||
ReturnValue_t CommandExecutor::load(std::string command, bool blocking, bool printOutput) {
|
||||
if(state == States::PENDING) {
|
||||
return COMMAND_PENDING;
|
||||
}
|
||||
|
||||
currentCmd = command;
|
||||
this->blocking = blocking;
|
||||
this->printOutput = printOutput;
|
||||
if(state == States::IDLE) {
|
||||
state = States::COMMAND_LOADED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CommandExecutor::execute() {
|
||||
if(state == States::IDLE) {
|
||||
return NO_COMMAND_LOADED_OR_PENDING;
|
||||
}
|
||||
else if(state == States::PENDING) {
|
||||
return COMMAND_PENDING;
|
||||
}
|
||||
currentCmdFile = popen(currentCmd.c_str(), "r");
|
||||
if(currentCmdFile == nullptr) {
|
||||
lastError = errno;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
if(blocking) {
|
||||
return executeBlocking();
|
||||
}
|
||||
else {
|
||||
currentFd = fileno(currentCmdFile);
|
||||
waiter.fd = currentFd;
|
||||
}
|
||||
state = States::PENDING;
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
ReturnValue_t CommandExecutor::close() {
|
||||
if(state == States::PENDING) {
|
||||
// Attempt to close process, irrespective of if it is running or not
|
||||
if(currentCmdFile != nullptr) {
|
||||
pclose(currentCmdFile);
|
||||
}
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void CommandExecutor::printLastError(std::string funcName) const {
|
||||
if(lastError != 0) {
|
||||
sif::error << funcName << " pclose failed with code " <<
|
||||
lastError << ": " << strerror(lastError) << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
void CommandExecutor::setRingBuffer(SimpleRingBuffer *ringBuffer,
|
||||
DynamicFIFO<uint16_t>* sizesFifo) {
|
||||
this->ringBuffer = ringBuffer;
|
||||
this->sizesFifo = sizesFifo;
|
||||
}
|
||||
|
||||
ReturnValue_t CommandExecutor::check(bool& bytesRead) {
|
||||
if(blocking) {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
switch(state) {
|
||||
case(States::IDLE):
|
||||
case(States::COMMAND_LOADED): {
|
||||
return NO_COMMAND_LOADED_OR_PENDING;
|
||||
}
|
||||
case(States::PENDING): {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
int result = poll(&waiter, 1, 0);
|
||||
switch(result) {
|
||||
case(0): {
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
break;
|
||||
}
|
||||
case(1): {
|
||||
if (waiter.revents & POLLIN) {
|
||||
ssize_t readBytes = read(currentFd, readVec.data(), readVec.size());
|
||||
if(readBytes == 0) {
|
||||
// Should not happen
|
||||
sif::warning << "CommandExecutor::check: "
|
||||
"No bytes read after poll event.." << std::endl;
|
||||
break;
|
||||
}
|
||||
else if(readBytes > 0) {
|
||||
bytesRead = true;
|
||||
if(printOutput) {
|
||||
// It is assumed the command output is line terminated
|
||||
sif::info << currentCmd << " | " << readVec.data();
|
||||
}
|
||||
if(ringBuffer != nullptr) {
|
||||
ringBuffer->writeData(reinterpret_cast<const uint8_t*>(
|
||||
readVec.data()), readBytes);
|
||||
}
|
||||
if(sizesFifo != nullptr) {
|
||||
if(not sizesFifo->full()) {
|
||||
sizesFifo->insert(readBytes);
|
||||
}
|
||||
}
|
||||
return BYTES_READ;
|
||||
}
|
||||
else {
|
||||
// Should also not happen
|
||||
sif::warning << "CommandExecutor::check: Error " << errno << ": " <<
|
||||
strerror(errno) << std::endl;
|
||||
}
|
||||
}
|
||||
else if(waiter.revents & POLLERR) {
|
||||
sif::warning << "CommandExecuter::check: Poll error" << std::endl;
|
||||
return COMMAND_ERROR;
|
||||
}
|
||||
else if(waiter.revents & POLLHUP) {
|
||||
int result = pclose(currentCmdFile);
|
||||
if(result != 0) {
|
||||
lastError = result;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
state = States::IDLE;
|
||||
currentCmdFile = nullptr;
|
||||
currentFd = 0;
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void CommandExecutor::reset() {
|
||||
CommandExecutor::close();
|
||||
currentCmdFile = nullptr;
|
||||
currentFd = 0;
|
||||
state = States::IDLE;
|
||||
}
|
||||
|
||||
int CommandExecutor::getLastError() const {
|
||||
return this->lastError;
|
||||
}
|
||||
|
||||
CommandExecutor::States CommandExecutor::getCurrentState() const {
|
||||
return state;
|
||||
}
|
||||
|
||||
ReturnValue_t CommandExecutor::executeBlocking() {
|
||||
while(fgets(readVec.data(), readVec.size(), currentCmdFile) != nullptr) {
|
||||
std::string output(readVec.data());
|
||||
if(printOutput) {
|
||||
sif::info << currentCmd << " | " << output;
|
||||
}
|
||||
if(ringBuffer != nullptr) {
|
||||
ringBuffer->writeData(reinterpret_cast<const uint8_t*>(output.data()), output.size());
|
||||
}
|
||||
if(sizesFifo != nullptr) {
|
||||
if(not sizesFifo->full()) {
|
||||
sizesFifo->insert(output.size());
|
||||
}
|
||||
}
|
||||
}
|
||||
int result = pclose(currentCmdFile);
|
||||
if(result != 0) {
|
||||
lastError = result;
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
@@ -1,134 +0,0 @@
|
||||
#ifndef FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_
|
||||
#define FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_
|
||||
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
#include "fsfw/returnvalues/FwClassIds.h"
|
||||
|
||||
#include <poll.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
class SimpleRingBuffer;
|
||||
template <typename T> class DynamicFIFO;
|
||||
|
||||
/**
|
||||
* @brief Helper class to execute shell commands in blocking and non-blocking mode
|
||||
* @details
|
||||
* This class is able to execute processes by using the Linux popen call. It also has the
|
||||
* capability of writing the read output of a process into a provided ring buffer.
|
||||
*
|
||||
* The executor works by first loading the command which should be executed and specifying
|
||||
* whether it should be executed blocking or non-blocking. After that, execution can be started
|
||||
* with the execute command. In blocking mode, the execute command will block until the command
|
||||
* has finished
|
||||
*/
|
||||
class CommandExecutor {
|
||||
public:
|
||||
enum class States {
|
||||
IDLE,
|
||||
COMMAND_LOADED,
|
||||
PENDING
|
||||
};
|
||||
|
||||
static constexpr uint8_t CLASS_ID = CLASS_ID::LINUX_OSAL;
|
||||
|
||||
//! [EXPORT] : [COMMENT] Execution of the current command has finished
|
||||
static constexpr ReturnValue_t EXECUTION_FINISHED =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0);
|
||||
|
||||
//! [EXPORT] : [COMMENT] Command is pending. This will also be returned if the user tries
|
||||
//! to load another command but a command is still pending
|
||||
static constexpr ReturnValue_t COMMAND_PENDING =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1);
|
||||
//! [EXPORT] : [COMMENT] Some bytes have been read from the executing process
|
||||
static constexpr ReturnValue_t BYTES_READ =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2);
|
||||
//! [EXPORT] : [COMMENT] Command execution failed
|
||||
static constexpr ReturnValue_t COMMAND_ERROR =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 3);
|
||||
//! [EXPORT] : [COMMENT]
|
||||
static constexpr ReturnValue_t NO_COMMAND_LOADED_OR_PENDING =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 4);
|
||||
static constexpr ReturnValue_t PCLOSE_CALL_ERROR =
|
||||
HasReturnvaluesIF::makeReturnCode(CLASS_ID, 6);
|
||||
|
||||
/**
|
||||
* Constructor. Is initialized with maximum size of internal buffer to read data from the
|
||||
* executed process.
|
||||
* @param maxSize
|
||||
*/
|
||||
CommandExecutor(const size_t maxSize);
|
||||
|
||||
/**
|
||||
* Load a new command which should be executed
|
||||
* @param command
|
||||
* @param blocking
|
||||
* @param printOutput
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t load(std::string command, bool blocking, bool printOutput = true);
|
||||
/**
|
||||
* Execute the loaded command.
|
||||
* @return
|
||||
* - In blocking mode, it will return RETURN_FAILED if
|
||||
* the result of the system call was not 0. The error value can be accessed using
|
||||
* getLastError
|
||||
* - In non-blocking mode, this call will start
|
||||
* the execution and then return RETURN_OK
|
||||
*/
|
||||
ReturnValue_t execute();
|
||||
/**
|
||||
* Only used in non-blocking mode. Checks the currently running command.
|
||||
* @param bytesRead Will be set to the number of bytes read, if bytes have been read
|
||||
* @return
|
||||
* - BYTES_READ if bytes have been read from the executing process. It is recommended to call
|
||||
* check again after this
|
||||
* - RETURN_OK execution is pending, but no bytes have been read from the executing process
|
||||
* - RETURN_FAILED if execution has failed, error value can be accessed using getLastError
|
||||
* - EXECUTION_FINISHED if the process was executed successfully
|
||||
* - COMMAND_ERROR internal poll error
|
||||
*/
|
||||
ReturnValue_t check(bool& bytesRead);
|
||||
/**
|
||||
* Abort the current command. Should normally not be necessary, check can be used to find
|
||||
* out whether command execution was successful
|
||||
* @return RETURN_OK
|
||||
*/
|
||||
ReturnValue_t close();
|
||||
|
||||
States getCurrentState() const;
|
||||
int getLastError() const;
|
||||
void printLastError(std::string funcName) const;
|
||||
|
||||
/**
|
||||
* Assign a ring buffer and a FIFO which will be filled by the executor with the output
|
||||
* read from the started process
|
||||
* @param ringBuffer
|
||||
* @param sizesFifo
|
||||
*/
|
||||
void setRingBuffer(SimpleRingBuffer* ringBuffer, DynamicFIFO<uint16_t>* sizesFifo);
|
||||
|
||||
/**
|
||||
* Reset the executor. This calls close internally and then reset the state machine so new
|
||||
* commands can be loaded and executed
|
||||
*/
|
||||
void reset();
|
||||
private:
|
||||
std::string currentCmd;
|
||||
bool blocking = true;
|
||||
FILE* currentCmdFile = nullptr;
|
||||
int currentFd = 0;
|
||||
bool printOutput = true;
|
||||
std::vector<char> readVec;
|
||||
struct pollfd waiter {};
|
||||
SimpleRingBuffer* ringBuffer = nullptr;
|
||||
DynamicFIFO<uint16_t>* sizesFifo = nullptr;
|
||||
|
||||
States state = States::IDLE;
|
||||
int lastError = 0;
|
||||
|
||||
ReturnValue_t executeBlocking();
|
||||
};
|
||||
|
||||
#endif /* FSFW_SRC_FSFW_OSAL_LINUX_COMMANDEXECUTOR_H_ */
|
@@ -285,10 +285,10 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
||||
|
||||
utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF");
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "mq_send to " << sendTo << " sent from "
|
||||
<< sentFrom << " failed" << std::endl;
|
||||
sif::warning << "mq_send to: " << sendTo << " sent from "
|
||||
<< sentFrom << "failed" << std::endl;
|
||||
#else
|
||||
sif::printWarning("mq_send to %d sent from %d failed\n", sendTo, sentFrom);
|
||||
sif::printWarning("mq_send to: %d sent from %d failed\n", sendTo, sentFrom);
|
||||
#endif
|
||||
return DESTINATION_INVALID;
|
||||
}
|
||||
|
@@ -1,57 +0,0 @@
|
||||
#include "fsfw/osal/linux/Timer.h"
|
||||
#include "fsfw/serviceinterface/ServiceInterfaceStream.h"
|
||||
#include <errno.h>
|
||||
|
||||
|
||||
Timer::Timer() {
|
||||
sigevent sigEvent;
|
||||
sigEvent.sigev_notify = SIGEV_NONE;
|
||||
sigEvent.sigev_signo = 0;
|
||||
sigEvent.sigev_value.sival_ptr = &timerId;
|
||||
int status = timer_create(CLOCK_MONOTONIC, &sigEvent, &timerId);
|
||||
if(status!=0){
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "Timer creation failed with: " << status <<
|
||||
" errno: " << errno << std::endl;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
Timer::~Timer() {
|
||||
timer_delete(timerId);
|
||||
}
|
||||
|
||||
int Timer::setTimer(uint32_t intervalMs) {
|
||||
itimerspec timer;
|
||||
timer.it_value.tv_sec = intervalMs / 1000;
|
||||
timer.it_value.tv_nsec = (intervalMs * 1000000) % (1000000000);
|
||||
timer.it_interval.tv_sec = 0;
|
||||
timer.it_interval.tv_nsec = 0;
|
||||
set = true;
|
||||
return timer_settime(timerId, 0, &timer, NULL);
|
||||
}
|
||||
|
||||
|
||||
int Timer::getTimer(uint32_t* remainingTimeMs){
|
||||
itimerspec timer;
|
||||
timer.it_value.tv_sec = 0;
|
||||
timer.it_value.tv_nsec = 0;
|
||||
timer.it_interval.tv_sec = 0;
|
||||
timer.it_interval.tv_nsec = 0;
|
||||
int status = timer_gettime(timerId, &timer);
|
||||
|
||||
*remainingTimeMs = timer.it_value.tv_sec * 1000 + timer.it_value.tv_nsec / 1000000;
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
bool Timer::isSet() const {
|
||||
return this->set;
|
||||
}
|
||||
|
||||
void Timer::resetTimer() {
|
||||
if(not this->set) {
|
||||
set = false;
|
||||
}
|
||||
setTimer(0);
|
||||
}
|
@@ -1,49 +0,0 @@
|
||||
#ifndef FRAMEWORK_OSAL_LINUX_TIMER_H_
|
||||
#define FRAMEWORK_OSAL_LINUX_TIMER_H_
|
||||
|
||||
#include <signal.h>
|
||||
#include <time.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/**
|
||||
* This class is a helper for the creation of a Clock Monotonic timer which does not trigger a signal
|
||||
*/
|
||||
class Timer {
|
||||
public:
|
||||
/**
|
||||
* Creates the Timer sets the timerId Member
|
||||
*/
|
||||
Timer();
|
||||
/**
|
||||
* Deletes the timer
|
||||
*
|
||||
* Careful! According to POSIX documentation:
|
||||
* The treatment of any pending signal generated by the deleted timer is unspecified.
|
||||
*/
|
||||
virtual ~Timer();
|
||||
|
||||
/**
|
||||
* Set the timer given in timerId to the given interval
|
||||
*
|
||||
* @param intervalMs Interval in ms to be set
|
||||
* @return 0 on Success 1 else
|
||||
*/
|
||||
int setTimer(uint32_t intervalMs);
|
||||
|
||||
/**
|
||||
* Get the remaining time of the timer
|
||||
*
|
||||
* @param remainingTimeMs Pointer to integer value which is used to return the remaining time
|
||||
* @return 0 on Success 1 else (see timer_getime documentation of posix function)
|
||||
*/
|
||||
int getTimer(uint32_t* remainingTimeMs);
|
||||
|
||||
bool isSet() const;
|
||||
void resetTimer();
|
||||
|
||||
private:
|
||||
bool set = true;
|
||||
timer_t timerId;
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_OSAL_LINUX_TIMER_H_ */
|
@@ -41,7 +41,8 @@ ReturnValue_t Service5EventReporting::performService() {
|
||||
}
|
||||
}
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service5EventReporting::generateEventReport: Too many events" << std::endl;
|
||||
sif::debug << "Service5EventReporting::generateEventReport:"
|
||||
" Too many events" << std::endl;
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
@@ -63,11 +64,8 @@ ReturnValue_t Service5EventReporting::generateEventReport(
|
||||
requestQueue->getDefaultDestination(),requestQueue->getId());
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "Service5EventReporting::generateEventReport: "
|
||||
"Could not send TM packet" << std::endl;
|
||||
#else
|
||||
sif::printWarning("Service5EventReporting::generateEventReport: "
|
||||
"Could not send TM packet\n");
|
||||
sif::debug << "Service5EventReporting::generateEventReport:"
|
||||
" Could not send TM packet" << std::endl;
|
||||
#endif
|
||||
}
|
||||
return result;
|
||||
|
@@ -33,8 +33,8 @@ ReturnValue_t Service8FunctionManagement::getMessageQueueAndObject(
|
||||
if(tcDataLen < sizeof(object_id_t)) {
|
||||
return CommandingServiceBase::INVALID_TC;
|
||||
}
|
||||
// Can't fail, size was checked before
|
||||
SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
||||
SerializeAdapter::deSerialize(objectId, &tcData,
|
||||
&tcDataLen, SerializeIF::Endianness::BIG);
|
||||
|
||||
return checkInterfaceAndAcquireMessageQueue(id,objectId);
|
||||
}
|
||||
|
@@ -22,9 +22,9 @@ public:
|
||||
* @param number
|
||||
* @return
|
||||
*/
|
||||
static constexpr ReturnValue_t makeReturnCode(uint8_t classId,
|
||||
static constexpr ReturnValue_t makeReturnCode(uint8_t interfaceId,
|
||||
uint8_t number) {
|
||||
return (static_cast<ReturnValue_t>(classId) << 8) + number;
|
||||
return (static_cast<ReturnValue_t>(interfaceId) << 8) + number;
|
||||
}
|
||||
};
|
||||
|
||||
|
@@ -62,8 +62,7 @@ protected:
|
||||
struct ChildInfo {
|
||||
MessageQueueId_t commandQueue;
|
||||
Mode_t mode;
|
||||
Submode_t submode;
|
||||
bool healthChanged;
|
||||
Submode_t submode;bool healthChanged;
|
||||
};
|
||||
|
||||
Mode_t mode;
|
||||
|
@@ -6,16 +6,14 @@ Countdown::Countdown(uint32_t initialTimeout): timeout(initialTimeout) {
|
||||
Countdown::~Countdown() {
|
||||
}
|
||||
|
||||
ReturnValue_t Countdown::setTimeout(uint32_t miliseconds) {
|
||||
ReturnValue_t return_value = Clock::getUptime( &startTime );
|
||||
timeout = miliseconds;
|
||||
return return_value;
|
||||
ReturnValue_t Countdown::setTimeout(uint32_t milliseconds) {
|
||||
ReturnValue_t returnValue = Clock::getUptime( &startTime );
|
||||
timeout = milliseconds;
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
bool Countdown::hasTimedOut() const {
|
||||
uint32_t current_time;
|
||||
Clock::getUptime( ¤t_time );
|
||||
if ( uint32_t(current_time - startTime) >= timeout) {
|
||||
if ( uint32_t( this->getCurrentTime() - startTime) >= timeout) {
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
@@ -31,7 +29,23 @@ ReturnValue_t Countdown::resetTimer() {
|
||||
}
|
||||
|
||||
void Countdown::timeOut() {
|
||||
uint32_t current_time;
|
||||
Clock::getUptime( ¤t_time );
|
||||
startTime= current_time - timeout;
|
||||
startTime = this->getCurrentTime() - timeout;
|
||||
}
|
||||
|
||||
uint32_t Countdown::getRemainingMillis() const {
|
||||
// We fetch the time before the if-statement
|
||||
// to be sure that the return is in
|
||||
// range 0 <= number <= timeout
|
||||
uint32_t currentTime = this->getCurrentTime();
|
||||
if (this->hasTimedOut()){
|
||||
return 0;
|
||||
}else{
|
||||
return (startTime + timeout) - currentTime;
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t Countdown::getCurrentTime() const {
|
||||
uint32_t currentTime;
|
||||
Clock::getUptime( ¤tTime );
|
||||
return currentTime;
|
||||
}
|
||||
|
@@ -4,28 +4,77 @@
|
||||
#include "Clock.h"
|
||||
|
||||
/**
|
||||
* @brief This file defines the Countdown class.
|
||||
* @author baetz
|
||||
*
|
||||
* Countdown keeps track of a timespan.
|
||||
*
|
||||
* Countdown::resetTimer restarts the timer.
|
||||
* Countdown::setTimeout sets a new countdown duration and resets.
|
||||
*
|
||||
* Can be checked with Countdown::hasTimedOut or
|
||||
* Countdown::isBusy.
|
||||
*
|
||||
* Countdown::timeOut will force the timer to time out.
|
||||
*
|
||||
*/
|
||||
class Countdown {
|
||||
public:
|
||||
uint32_t timeout;
|
||||
/**
|
||||
* Constructor which sets the countdown duration in milliseconds
|
||||
*
|
||||
* It does not start the countdown!
|
||||
* Call resetTimer or setTimeout before usage!
|
||||
* Otherwise a call to hasTimedOut might return True.
|
||||
*
|
||||
* @param initialTimeout Countdown duration in milliseconds
|
||||
*/
|
||||
Countdown(uint32_t initialTimeout = 0);
|
||||
~Countdown();
|
||||
ReturnValue_t setTimeout(uint32_t miliseconds);
|
||||
|
||||
/**
|
||||
* Call to set a new countdown duration.
|
||||
*
|
||||
* Resets the countdown!
|
||||
*
|
||||
* @param milliseconds new countdown duration in milliseconds
|
||||
* @return Returnvalue from Clock::getUptime
|
||||
*/
|
||||
ReturnValue_t setTimeout(uint32_t milliseconds);
|
||||
/**
|
||||
* Returns true if the countdown duration has passed.
|
||||
*
|
||||
* @return True if the countdown has passed
|
||||
* False if it is still running
|
||||
*/
|
||||
bool hasTimedOut() const;
|
||||
|
||||
/**
|
||||
* Complementary to hasTimedOut.
|
||||
*
|
||||
* @return True if the countdown is till running
|
||||
* False if it is still running
|
||||
*/
|
||||
bool isBusy() const;
|
||||
|
||||
//!< Use last set timeout value and restart timer.
|
||||
/**
|
||||
* Uses last set timeout value and restarts timer.
|
||||
*/
|
||||
ReturnValue_t resetTimer();
|
||||
|
||||
//!< Make hasTimedOut() return true
|
||||
/**
|
||||
* Returns the remaining milliseconds (0 if timeout)
|
||||
*/
|
||||
uint32_t getRemainingMillis() const;
|
||||
/**
|
||||
* Makes hasTimedOut() return true
|
||||
*/
|
||||
void timeOut();
|
||||
|
||||
/**
|
||||
* Internal countdown duration in milliseconds
|
||||
*/
|
||||
uint32_t timeout;
|
||||
private:
|
||||
/**
|
||||
* Last time the timer was started (uptime)
|
||||
*/
|
||||
uint32_t startTime = 0;
|
||||
|
||||
uint32_t getCurrentTime() const;
|
||||
};
|
||||
|
||||
#endif /* FSFW_TIMEMANAGER_COUNTDOWN_H_ */
|
||||
|
@@ -3,8 +3,8 @@
|
||||
|
||||
#include <cstring>
|
||||
|
||||
SpacePacketBase::SpacePacketBase(const uint8_t* setAddress) {
|
||||
this->data = reinterpret_cast<SpacePacketPointer*>(const_cast<uint8_t*>(setAddress));
|
||||
SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) {
|
||||
this->data = (SpacePacketPointer*) set_address;
|
||||
}
|
||||
|
||||
SpacePacketBase::~SpacePacketBase() {
|
||||
@@ -15,21 +15,10 @@ uint8_t SpacePacketBase::getPacketVersionNumber( void ) {
|
||||
return (this->data->header.packet_id_h & 0b11100000) >> 5;
|
||||
}
|
||||
|
||||
ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
|
||||
void SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
|
||||
bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) {
|
||||
if(data == nullptr) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "SpacePacketBase::initSpacePacketHeader: Data pointer is invalid"
|
||||
<< std::endl;
|
||||
#else
|
||||
sif::printWarning("SpacePacketBase::initSpacePacketHeader: Data pointer is invalid!\n");
|
||||
#endif
|
||||
#endif
|
||||
return HasReturnvaluesIF::RETURN_FAILED;
|
||||
}
|
||||
//reset header to zero:
|
||||
memset(data, 0, sizeof(this->data->header) );
|
||||
memset(data,0, sizeof(this->data->header) );
|
||||
//Set TC/TM bit.
|
||||
data->header.packet_id_h = ((isTelecommand? 1 : 0)) << 4;
|
||||
//Set secondaryHeader bit
|
||||
@@ -38,7 +27,7 @@ ReturnValue_t SpacePacketBase::initSpacePacketHeader(bool isTelecommand,
|
||||
//Always initialize as standalone packets.
|
||||
data->header.sequence_control_h = 0b11000000;
|
||||
setPacketSequenceCount(sequenceCount);
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
|
||||
}
|
||||
|
||||
bool SpacePacketBase::isTelecommand( void ) {
|
||||
@@ -65,11 +54,6 @@ void SpacePacketBase::setAPID( uint16_t new_apid ) {
|
||||
this->data->header.packet_id_l = ( new_apid & 0x00FF );
|
||||
}
|
||||
|
||||
void SpacePacketBase::setSequenceFlags( uint8_t sequenceflags ) {
|
||||
this->data->header.sequence_control_h &= 0x3F;
|
||||
this->data->header.sequence_control_h |= sequenceflags << 6;
|
||||
}
|
||||
|
||||
uint16_t SpacePacketBase::getPacketSequenceControl( void ) {
|
||||
return ( (this->data->header.sequence_control_h) << 8 )
|
||||
+ this->data->header.sequence_control_l;
|
||||
|
@@ -2,8 +2,6 @@
|
||||
#define FSFW_TMTCPACKET_SPACEPACKETBASE_H_
|
||||
|
||||
#include "ccsds_header.h"
|
||||
#include "fsfw/returnvalues/HasReturnvaluesIF.h"
|
||||
|
||||
#include <cstddef>
|
||||
|
||||
/**
|
||||
@@ -84,7 +82,7 @@ public:
|
||||
*/
|
||||
bool isTelecommand( void );
|
||||
|
||||
ReturnValue_t initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
|
||||
void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader,
|
||||
uint16_t apid, uint16_t sequenceCount = 0);
|
||||
/**
|
||||
* The CCSDS header provides a secondary header flag (the fifth-highest bit),
|
||||
@@ -111,13 +109,6 @@ public:
|
||||
* ignored.
|
||||
*/
|
||||
void setAPID( uint16_t setAPID );
|
||||
|
||||
/**
|
||||
* Sets the sequence flags of a packet, which are bit 17 and 18 in the space packet header.
|
||||
* @param The sequence flags to set
|
||||
*/
|
||||
void setSequenceFlags( uint8_t sequenceflags );
|
||||
|
||||
/**
|
||||
* Returns the CCSDS packet sequence control field, which are the third and
|
||||
* the fourth byte of the CCSDS primary header.
|
||||
|
@@ -53,14 +53,11 @@ uint8_t* TmPacketPusC::getPacketTimeRaw() const{
|
||||
|
||||
}
|
||||
|
||||
ReturnValue_t TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
|
||||
void TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
|
||||
uint8_t subservice, uint16_t packetSubcounter, uint16_t destinationId,
|
||||
uint8_t timeRefField) {
|
||||
//Set primary header:
|
||||
ReturnValue_t result = initSpacePacketHeader(false, true, apid);
|
||||
if(result != HasReturnvaluesIF::RETURN_OK) {
|
||||
return result;
|
||||
}
|
||||
initSpacePacketHeader(false, true, apid);
|
||||
//Set data Field Header:
|
||||
//First, set to zero.
|
||||
memset(&tmData->dataField, 0, sizeof(tmData->dataField));
|
||||
@@ -79,7 +76,6 @@ ReturnValue_t TmPacketPusC::initializeTmPacket(uint16_t apid, uint8_t service,
|
||||
timeStamper->addTimeStamp(tmData->dataField.time,
|
||||
sizeof(tmData->dataField.time));
|
||||
}
|
||||
return HasReturnvaluesIF::RETURN_OK;
|
||||
}
|
||||
|
||||
void TmPacketPusC::setSourceDataSize(uint16_t size) {
|
||||
|
@@ -100,7 +100,7 @@ protected:
|
||||
* @param subservice PUS Subservice
|
||||
* @param packetSubcounter Additional subcounter used.
|
||||
*/
|
||||
ReturnValue_t initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
|
||||
void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice,
|
||||
uint16_t packetSubcounter, uint16_t destinationId = 0, uint8_t timeRefField = 0);
|
||||
|
||||
/**
|
||||
|
@@ -43,55 +43,27 @@ TmPacketStoredPusC::TmPacketStoredPusC(uint16_t apid, uint8_t service,
|
||||
return;
|
||||
}
|
||||
size_t sourceDataSize = 0;
|
||||
if (content != nullptr) {
|
||||
if (content != NULL) {
|
||||
sourceDataSize += content->getSerializedSize();
|
||||
}
|
||||
if (header != nullptr) {
|
||||
if (header != NULL) {
|
||||
sourceDataSize += header->getSerializedSize();
|
||||
}
|
||||
uint8_t *pData = nullptr;
|
||||
size_t sizeToReserve = getPacketMinimumSize() + sourceDataSize;
|
||||
ReturnValue_t returnValue = store->getFreeElement(&storeAddress, sizeToReserve, &pData);
|
||||
uint8_t *p_data = NULL;
|
||||
ReturnValue_t returnValue = store->getFreeElement(&storeAddress,
|
||||
(getPacketMinimumSize() + sourceDataSize), &p_data);
|
||||
if (returnValue != store->RETURN_OK) {
|
||||
#if FSFW_VERBOSE_LEVEL >= 1
|
||||
switch(returnValue) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
case(StorageManagerIF::DATA_STORAGE_FULL): {
|
||||
sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
|
||||
"size " << sizeToReserve << std::endl;
|
||||
break;
|
||||
}
|
||||
case(StorageManagerIF::DATA_TOO_LARGE): {
|
||||
sif::warning << "TmPacketStoredPusC::TmPacketStoredPusC: Data with size " <<
|
||||
sizeToReserve << " too large" << std::endl;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
case(StorageManagerIF::DATA_STORAGE_FULL): {
|
||||
sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Store full for packet with "
|
||||
"size %d\n", sizeToReserve);
|
||||
break;
|
||||
}
|
||||
case(StorageManagerIF::DATA_TOO_LARGE): {
|
||||
sif::printWarning("TmPacketStoredPusC::TmPacketStoredPusC: Data with size "
|
||||
"%d too large\n", sizeToReserve);
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
TmPacketStoredBase::checkAndReportLostTm();
|
||||
return;
|
||||
}
|
||||
setData(pData);
|
||||
setData(p_data);
|
||||
initializeTmPacket(apid, service, subservice, packetSubcounter, destinationId, timeRefField);
|
||||
uint8_t *putDataHere = getSourceData();
|
||||
size_t size = 0;
|
||||
if (header != nullptr) {
|
||||
if (header != NULL) {
|
||||
header->serialize(&putDataHere, &size, sourceDataSize,
|
||||
SerializeIF::Endianness::BIG);
|
||||
}
|
||||
if (content != nullptr) {
|
||||
if (content != NULL) {
|
||||
content->serialize(&putDataHere, &size, sourceDataSize,
|
||||
SerializeIF::Endianness::BIG);
|
||||
}
|
||||
|
@@ -18,4 +18,5 @@ add_subdirectory(serialize)
|
||||
add_subdirectory(datapoollocal)
|
||||
add_subdirectory(storagemanager)
|
||||
add_subdirectory(globalfunctions)
|
||||
add_subdirectory(timemanager)
|
||||
add_subdirectory(tmtcpacket)
|
||||
|
@@ -103,7 +103,7 @@ TEST_CASE("DleEncoder" , "[DleEncoder]") {
|
||||
for(size_t faultyDestSize = 0; faultyDestSize < expectedVec.size(); faultyDestSize ++) {
|
||||
result = dleEncoder.encode(vecToEncode.data(), vecToEncode.size(),
|
||||
buffer.data(), faultyDestSize, &encodedLen);
|
||||
REQUIRE(result == DleEncoder::STREAM_TOO_SHORT);
|
||||
REQUIRE(result == static_cast<int>(DleEncoder::STREAM_TOO_SHORT));
|
||||
}
|
||||
};
|
||||
|
||||
@@ -218,5 +218,10 @@ TEST_CASE("DleEncoder" , "[DleEncoder]") {
|
||||
REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
|
||||
|
||||
dleEncoder.setEscapeMode(true);
|
||||
testArray1EncodedFaulty = TEST_ARRAY_1_ENCODED_ESCAPED;
|
||||
testArray1EncodedFaulty[5] = 0;
|
||||
result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
|
||||
&readLen, buffer.data(), buffer.size(), &encodedLen);
|
||||
REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
|
||||
}
|
||||
}
|
||||
|
3
tests/src/fsfw_tests/unit/timemanager/CMakeLists.txt
Normal file
3
tests/src/fsfw_tests/unit/timemanager/CMakeLists.txt
Normal file
@@ -0,0 +1,3 @@
|
||||
target_sources(${TARGET_NAME} PRIVATE
|
||||
TestCountdown.cpp
|
||||
)
|
27
tests/src/fsfw_tests/unit/timemanager/TestCountdown.cpp
Normal file
27
tests/src/fsfw_tests/unit/timemanager/TestCountdown.cpp
Normal file
@@ -0,0 +1,27 @@
|
||||
#include "fsfw_tests/unit/CatchDefinitions.h"
|
||||
#include <fsfw/timemanager/Countdown.h>
|
||||
#include <catch2/catch_test_macros.hpp>
|
||||
|
||||
|
||||
TEST_CASE( "Countdown Tests", "[TestCountdown]") {
|
||||
INFO("Countdown Tests");
|
||||
Countdown count(20);
|
||||
REQUIRE(count.timeout == 20);
|
||||
REQUIRE(count.setTimeout(100) == static_cast<uint16_t>(HasReturnvaluesIF::RETURN_OK));
|
||||
REQUIRE(count.timeout == 100);
|
||||
REQUIRE(count.setTimeout(150) == static_cast<uint16_t>(HasReturnvaluesIF::RETURN_OK));
|
||||
REQUIRE(count.isBusy());
|
||||
REQUIRE(not count.hasTimedOut());
|
||||
uint32_t number = count.getRemainingMillis();
|
||||
REQUIRE(number > 0);
|
||||
bool blocked = false;
|
||||
while(not count.hasTimedOut()){
|
||||
blocked = true;
|
||||
};
|
||||
REQUIRE(blocked);
|
||||
number = count.getRemainingMillis();
|
||||
REQUIRE(number==0);
|
||||
count.resetTimer();
|
||||
REQUIRE(not count.hasTimedOut());
|
||||
REQUIRE(count.isBusy());
|
||||
}
|
Reference in New Issue
Block a user