Compare commits
82 Commits
cb1aaea6cd
...
improve-se
Author | SHA1 | Date | |
---|---|---|---|
3b0ee7ca31
|
|||
f307a86d9a
|
|||
8b21dd276d | |||
e00da212cd | |||
a229748aa4 | |||
e6e3753324 | |||
94bd1ba2ab | |||
e12a8cfa29 | |||
efbcddc2e5 | |||
31d4b85523 | |||
aff6bb673b
|
|||
f8e3777c43
|
|||
0e2fa8dc83
|
|||
0cfe559b93
|
|||
c5159fb645
|
|||
203c0bac5c
|
|||
43ea29cb84 | |||
27c8a97d45 | |||
47b21caf5f | |||
2673070204 | |||
518a07d05b | |||
516357d855 | |||
ac28b7e00d | |||
0d4a862c1a | |||
b2576d3422 | |||
bccaf4a9ea | |||
7d4e77843b | |||
cd2cd61ba5 | |||
90f3f8ef1f | |||
9894935e99 | |||
a3a6c0720c | |||
d5a52eadbb | |||
b5e7179af1 | |||
5879eb7f7b | |||
e64e8b274d | |||
48bcce65b1 | |||
7105e199c6 | |||
94de483836 | |||
c648229c99 | |||
5b661551a8 | |||
3978524181 | |||
7187f2b5cd | |||
8e367abb47 | |||
b28174db24 | |||
c906acd659 | |||
8dfc84c0b5 | |||
74775bb59d | |||
c02d9e009e | |||
0021aa29f5 | |||
41d67bff63 | |||
8d7bb51d44
|
|||
7673d8b396
|
|||
91b194d8eb | |||
698897bfc5 | |||
d554062b86
|
|||
bf7fac071c | |||
35be7da353
|
|||
18cc870c8e
|
|||
133ca51d18
|
|||
0a40391a75
|
|||
cc3e64e70d | |||
6021257a87
|
|||
63c238005e | |||
0f604b35c6 | |||
e3a815444e | |||
19093866ee | |||
497f947237
|
|||
5bd020193b
|
|||
420c0625a5
|
|||
42a0b15303
|
|||
c199cbedaa
|
|||
c4e18cc2f3
|
|||
c143198494
|
|||
448fbd0d38
|
|||
470f589bde
|
|||
dfcfb035be
|
|||
6e53582dc9
|
|||
fb1500e041
|
|||
ea2e58249d
|
|||
9ef63825f3
|
|||
073cb4b3d5
|
|||
8c11685240
|
@@ -28,9 +28,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
|
||||
- add CFDP subsystem ID
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742
|
||||
- `PusTmZcWriter` now exposes API to set message counter field.
|
||||
- Relative timeshift in the PUS time service.
|
||||
|
||||
## Changed
|
||||
|
||||
- The PUS time service now dumps the time before setting a new time and after having set the
|
||||
time.
|
||||
- HK generation is now countdown based.
|
||||
- Bump ETL version to 20.35.14
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/748
|
||||
@@ -41,6 +44,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/).
|
||||
- Assert that `FixedArrayList` is larger than 0 at compile time.
|
||||
https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/740
|
||||
- Health functions are virtual now.
|
||||
- PUS Service Base request queue depth and maximum number of handled packets per cycle is now
|
||||
configurable.
|
||||
|
||||
# [v6.0.0] 2023-02-10
|
||||
|
||||
|
@@ -17,7 +17,7 @@ static constexpr char CFDP_VERSION_2_NAME[] = "CCSDS 727.0-B-5";
|
||||
static constexpr uint8_t CFDP_VERSION_2 = 0b001;
|
||||
static constexpr uint8_t VERSION_BITS = CFDP_VERSION_2 << 5;
|
||||
|
||||
static constexpr uint8_t CFDP_CLASS_ID = CLASS_ID::CFDP;
|
||||
static constexpr uint8_t CFDP_CLASS_ID = CLASS_ID::CFDP_BASE;
|
||||
|
||||
static constexpr ReturnValue_t INVALID_TLV_TYPE = returnvalue::makeCode(CFDP_CLASS_ID, 1);
|
||||
static constexpr ReturnValue_t INVALID_DIRECTIVE_FIELD = returnvalue::makeCode(CFDP_CLASS_ID, 2);
|
||||
|
@@ -146,7 +146,7 @@ ReturnValue_t cfdp::DestHandler::handleMetadataPdu(const PacketInfo& info) {
|
||||
return handleMetadataParseError(result, constAccessorPair.second.data(),
|
||||
constAccessorPair.second.size());
|
||||
}
|
||||
return startTransaction(reader, metadataInfo);
|
||||
return startTransaction(reader);
|
||||
}
|
||||
|
||||
ReturnValue_t cfdp::DestHandler::handleFileDataPdu(const cfdp::PacketInfo& info) {
|
||||
@@ -274,8 +274,7 @@ ReturnValue_t cfdp::DestHandler::handleMetadataParseError(ReturnValue_t result,
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
|
||||
ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader,
|
||||
MetadataGenericInfo& info) {
|
||||
ReturnValue_t cfdp::DestHandler::startTransaction(const MetadataPduReader& reader) {
|
||||
if (fsmRes.state != CfdpState::IDLE) {
|
||||
// According to standard, discard metadata PDU if we are busy
|
||||
return OK;
|
||||
@@ -283,25 +282,31 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader,
|
||||
ReturnValue_t result = OK;
|
||||
size_t sourceNameSize = 0;
|
||||
|
||||
const uint8_t* sourceNamePtr = reader.getSourceFileName().getValue(&sourceNameSize);
|
||||
if (sourceNameSize + 1 > transactionParams.sourceName.size()) {
|
||||
fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "source filename too large");
|
||||
return FAILED;
|
||||
if (not reader.getSourceFileName().isEmpty()) {
|
||||
const uint8_t* sourceNamePtr = reader.getSourceFileName().getValue(&sourceNameSize);
|
||||
if (sourceNameSize + 1 > transactionParams.sourceName.size()) {
|
||||
fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "source filename too large");
|
||||
return FAILED;
|
||||
}
|
||||
std::memcpy(transactionParams.sourceName.data(), sourceNamePtr, sourceNameSize);
|
||||
transactionParams.sourceName[sourceNameSize] = '\0';
|
||||
}
|
||||
std::memcpy(transactionParams.sourceName.data(), sourceNamePtr, sourceNameSize);
|
||||
transactionParams.sourceName[sourceNameSize] = '\0';
|
||||
size_t destNameSize = 0;
|
||||
const uint8_t* destNamePtr = reader.getDestFileName().getValue(&destNameSize);
|
||||
if (destNameSize + 1 > transactionParams.destName.size()) {
|
||||
fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "dest filename too large");
|
||||
return FAILED;
|
||||
if (not reader.getDestFileName().isEmpty()) {
|
||||
const uint8_t* destNamePtr = reader.getDestFileName().getValue(&destNameSize);
|
||||
if (destNameSize + 1 > transactionParams.destName.size()) {
|
||||
fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "dest filename too large");
|
||||
return FAILED;
|
||||
}
|
||||
std::memcpy(transactionParams.destName.data(), destNamePtr, destNameSize);
|
||||
transactionParams.destName[destNameSize] = '\0';
|
||||
}
|
||||
std::memcpy(transactionParams.destName.data(), destNamePtr, destNameSize);
|
||||
transactionParams.destName[destNameSize] = '\0';
|
||||
|
||||
transactionParams.metadataOnly = true;
|
||||
// If both dest name size and source name size are 0, we are dealing with a metadata only PDU,
|
||||
// so there is no need to create a file or truncate an existing file
|
||||
if (destNameSize > 0 and sourceNameSize > 0) {
|
||||
transactionParams.metadataOnly = false;
|
||||
FilesystemParams fparams(transactionParams.destName.data());
|
||||
// handling to allow only specifying target directory. Example:
|
||||
// Source path /test/hello.txt, dest path /tmp -> dest path /tmp/hello.txt
|
||||
@@ -338,12 +343,19 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader,
|
||||
#endif
|
||||
return FAILED;
|
||||
}
|
||||
fsmRes.step = TransactionStep::TRANSACTION_START;
|
||||
if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) {
|
||||
fsmRes.state = CfdpState::BUSY_CLASS_1_NACKED;
|
||||
} else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) {
|
||||
fsmRes.state = CfdpState::BUSY_CLASS_2_ACKED;
|
||||
}
|
||||
if (transactionParams.metadataOnly) {
|
||||
fsmRes.step = TransactionStep::TRANSFER_COMPLETION;
|
||||
} else {
|
||||
// Kind of ugly, make FSM working on packet per packet basis..
|
||||
fsmRes.step = TransactionStep::TRANSACTION_START;
|
||||
fsmRes.step = TransactionStep::RECEIVING_FILE_DATA_PDUS;
|
||||
}
|
||||
auto& info = reader.getGenericInfo();
|
||||
transactionParams.checksumType = info.getChecksumType();
|
||||
transactionParams.closureRequested = info.isClosureRequested();
|
||||
reader.fillConfig(transactionParams.pduConf);
|
||||
@@ -351,20 +363,19 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader,
|
||||
transactionParams.transactionId.entityId = transactionParams.pduConf.sourceId;
|
||||
transactionParams.transactionId.seqNum = transactionParams.pduConf.seqNum;
|
||||
transactionParams.fileSize = info.getFileSize();
|
||||
fsmRes.step = TransactionStep::RECEIVING_FILE_DATA_PDUS;
|
||||
MetadataRecvdParams params(transactionParams.transactionId, transactionParams.pduConf.sourceId,
|
||||
transactionParams.fileSize);
|
||||
params.destFileName = transactionParams.destName.data();
|
||||
params.sourceFileName = transactionParams.sourceName.data();
|
||||
unsigned tlvIdx = 0;
|
||||
params.numberOfMsgsToUser = 0;
|
||||
for (const auto& opt : tlvVec) {
|
||||
if (opt.getType() == TlvType::MSG_TO_USER) {
|
||||
msgToUserVec[tlvIdx] = MessageToUserTlv(opt.getValue(), opt.getLengthField());
|
||||
tlvIdx++;
|
||||
msgToUserVec[params.numberOfMsgsToUser] =
|
||||
MessageToUserTlv(opt.getValue(), opt.getLengthField());
|
||||
params.numberOfMsgsToUser++;
|
||||
}
|
||||
}
|
||||
params.msgsToUserArray = msgToUserVec.data();
|
||||
params.msgsToUserLen = tlvIdx;
|
||||
destParams.user.metadataRecvdIndication(params);
|
||||
return result;
|
||||
}
|
||||
|
@@ -125,11 +125,13 @@ class DestHandler {
|
||||
crc = 0;
|
||||
progress = 0;
|
||||
remoteCfg = nullptr;
|
||||
metadataOnly = false;
|
||||
closureRequested = false;
|
||||
vfsErrorCount = 0;
|
||||
checksumType = ChecksumType::NULL_CHECKSUM;
|
||||
}
|
||||
|
||||
bool metadataOnly = false;
|
||||
ChecksumType checksumType = ChecksumType::NULL_CHECKSUM;
|
||||
bool closureRequested = false;
|
||||
uint16_t vfsErrorCount = 0;
|
||||
@@ -153,7 +155,7 @@ class DestHandler {
|
||||
cfdp::FsfwParams fsfwParams;
|
||||
FsmResult fsmRes;
|
||||
|
||||
ReturnValue_t startTransaction(MetadataPduReader& reader, MetadataGenericInfo& info);
|
||||
ReturnValue_t startTransaction(const MetadataPduReader& reader);
|
||||
ReturnValue_t handleMetadataPdu(const PacketInfo& info);
|
||||
ReturnValue_t handleFileDataPdu(const PacketInfo& info);
|
||||
ReturnValue_t handleEofPdu(const PacketInfo& info);
|
||||
|
@@ -16,7 +16,7 @@ class RemoteConfigTableIF {
|
||||
*/
|
||||
class OneRemoteConfigProvider : public RemoteConfigTableIF {
|
||||
public:
|
||||
explicit OneRemoteConfigProvider(RemoteEntityCfg cfg) : cfg(std::move(cfg)) {}
|
||||
explicit OneRemoteConfigProvider(RemoteEntityCfg& cfg) : cfg(cfg) {}
|
||||
|
||||
bool getRemoteCfg(const EntityId& remoteId, cfdp::RemoteEntityCfg** cfg_) override {
|
||||
if (remoteId != cfg.remoteId) {
|
||||
@@ -27,7 +27,7 @@ class OneRemoteConfigProvider : public RemoteConfigTableIF {
|
||||
}
|
||||
|
||||
private:
|
||||
RemoteEntityCfg cfg;
|
||||
RemoteEntityCfg& cfg;
|
||||
};
|
||||
|
||||
} // namespace cfdp
|
||||
|
@@ -13,27 +13,24 @@ cfdp::ReservedMessageParser::ReservedMessageParser(StorageManagerIF& ipcStore,
|
||||
MessageQueueId_t userDestination)
|
||||
: msgQueue(msgQueue), ipcStore(ipcStore), userDestination(userDestination) {}
|
||||
|
||||
ReturnValue_t cfdp::ReservedMessageParser::parse(const uint8_t* msgsToUserPtr,
|
||||
size_t sizeOfMessages) {
|
||||
ReturnValue_t result;
|
||||
size_t currentIdx = 0;
|
||||
const uint8_t* currentPtr = msgsToUserPtr;
|
||||
MessageToUserTlv tlv;
|
||||
size_t deserSize = sizeOfMessages;
|
||||
ReturnValue_t cfdp::ReservedMessageParser::parse(const MessageToUserTlv* msgsToUserArray,
|
||||
size_t numMsgToUser) {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
cfdp::StringLv sourceFileName;
|
||||
cfdp::StringLv destFileName;
|
||||
PutRequest putRequest;
|
||||
const uint8_t* currentPtr = nullptr;
|
||||
size_t deserSize = 0;
|
||||
bool needToSendPutRequest = false;
|
||||
while (currentIdx < sizeOfMessages) {
|
||||
result = tlv.deSerialize(¤tPtr, &deserSize, SerializeIF::Endianness::NETWORK);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
for (size_t idx = 0; idx < numMsgToUser; idx++) {
|
||||
if (&msgsToUserArray[idx] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
uint8_t messageType = 0;
|
||||
if (tlv.isReservedCfdpMessage(messageType, ¤tPtr, deserSize)) {
|
||||
if (msgsToUserArray[idx].isReservedCfdpMessage(messageType, ¤tPtr, deserSize)) {
|
||||
if (messageType == static_cast<uint8_t>(ProxyOpMessageType::PUT_REQUEST)) {
|
||||
EntityId entityIdLv;
|
||||
entityIdLv.deSerializeFromLv(¤tPtr, &deserSize);
|
||||
result = entityIdLv.deSerializeFromLv(¤tPtr, &deserSize);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
@@ -52,7 +49,6 @@ ReturnValue_t cfdp::ReservedMessageParser::parse(const uint8_t* msgsToUserPtr,
|
||||
needToSendPutRequest = true;
|
||||
}
|
||||
}
|
||||
currentIdx += tlv.getSerializedSize();
|
||||
}
|
||||
if (needToSendPutRequest) {
|
||||
store_address_t storeId;
|
||||
|
@@ -1,5 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "fsfw/cfdp/tlv/MessageToUserTlv.h"
|
||||
#include "fsfw/ipc/MessageQueueIF.h"
|
||||
#include "fsfw/storagemanager/StorageManagerIF.h"
|
||||
|
||||
@@ -15,7 +16,7 @@ class ReservedMessageParser {
|
||||
ReservedMessageParser(StorageManagerIF& ipcStore, MessageQueueIF& msgQueue,
|
||||
MessageQueueId_t userDestination);
|
||||
|
||||
ReturnValue_t parse(const uint8_t* msgsToUserPtr, size_t sizeOfMessages);
|
||||
ReturnValue_t parse(const MessageToUserTlv* msgsToUserArray, size_t numMsgsToUser);
|
||||
|
||||
private:
|
||||
MessageQueueIF& msgQueue;
|
||||
|
@@ -11,6 +11,7 @@
|
||||
#include "fsfw/globalfunctions/arrayprinter.h"
|
||||
#include "fsfw/objectmanager.h"
|
||||
#include "fsfw/serviceinterface.h"
|
||||
#include "fsfw/tasks/TaskFactory.h"
|
||||
#include "fsfw/tmtcservices/TmTcMessage.h"
|
||||
|
||||
using namespace returnvalue;
|
||||
@@ -49,19 +50,16 @@ cfdp::SourceHandler::FsmResult& cfdp::SourceHandler::fsmNacked() {
|
||||
}
|
||||
if (step == TransactionStep::TRANSACTION_START) {
|
||||
sourceParams.user.transactionIndication(transactionParams.id);
|
||||
step = TransactionStep::CRC_PROCEDURE;
|
||||
}
|
||||
if (step == TransactionStep::CRC_PROCEDURE) {
|
||||
result = checksumGeneration();
|
||||
if (result != OK) {
|
||||
// TODO: Some error handling
|
||||
addError(result);
|
||||
}
|
||||
step = TransactionStep::SENDING_METADATA;
|
||||
}
|
||||
if (step == TransactionStep::SENDING_METADATA) {
|
||||
result = prepareAndSendMetadataPdu();
|
||||
if (result != OK) {
|
||||
// TODO: Error handling
|
||||
addError(result);
|
||||
}
|
||||
fsmResult.callStatus = CallStatus::CALL_AGAIN;
|
||||
return fsmResult;
|
||||
@@ -69,10 +67,7 @@ cfdp::SourceHandler::FsmResult& cfdp::SourceHandler::fsmNacked() {
|
||||
if (step == TransactionStep::SENDING_FILE_DATA) {
|
||||
bool noFdPdu = false;
|
||||
result = prepareAndSendNextFileDataPdu(noFdPdu);
|
||||
if (result != OK) {
|
||||
// TODO: Error handling
|
||||
}
|
||||
if (!noFdPdu) {
|
||||
if (result == OK and !noFdPdu) {
|
||||
fsmResult.callStatus = CallStatus::CALL_AGAIN;
|
||||
return fsmResult;
|
||||
}
|
||||
@@ -80,7 +75,7 @@ cfdp::SourceHandler::FsmResult& cfdp::SourceHandler::fsmNacked() {
|
||||
if (step == TransactionStep::SENDING_EOF) {
|
||||
result = prepareAndSendEofPdu();
|
||||
if (result != OK) {
|
||||
// TODO: Error handling
|
||||
addError(result);
|
||||
}
|
||||
if (sourceParams.cfg.indicCfg.eofSentIndicRequired) {
|
||||
sourceParams.user.eofSentIndication(transactionParams.id);
|
||||
@@ -141,8 +136,7 @@ ReturnValue_t cfdp::SourceHandler::checksumGeneration() {
|
||||
params.size = readLen;
|
||||
auto result = sourceParams.user.vfs.readFromFile(params, buf.data(), buf.size());
|
||||
if (result != OK) {
|
||||
// TODO: I think this is a case for a filestore rejection, but it might sense to print
|
||||
// a warning or trigger an event because this should generally not happen
|
||||
addError(result);
|
||||
return FAILED;
|
||||
}
|
||||
crcCalc.add(buf.begin(), buf.begin() + readLen);
|
||||
@@ -171,6 +165,10 @@ ReturnValue_t cfdp::SourceHandler::transactionStart(PutRequest& putRequest, Remo
|
||||
const char* destNamePtr = putRequest.getDestName().getCString(transactionParams.destNameSize);
|
||||
std::strncpy(transactionParams.sourceName.data(), srcNamePtr, transactionParams.sourceNameSize);
|
||||
std::strncpy(transactionParams.destName.data(), destNamePtr, transactionParams.destNameSize);
|
||||
// Add 0 termination. The source and dest name size can not be larger than UINT8_MAX, so this
|
||||
// operation is safe.
|
||||
transactionParams.sourceName[transactionParams.sourceNameSize] = '\0';
|
||||
transactionParams.destName[transactionParams.destNameSize] = '\0';
|
||||
FilesystemParams params(transactionParams.sourceName.data());
|
||||
if (!sourceParams.user.vfs.fileExists(params)) {
|
||||
return FILE_DOES_NOT_EXIST;
|
||||
@@ -322,6 +320,10 @@ ReturnValue_t cfdp::SourceHandler::sendGenericPdu(const SerializeIF& pdu) {
|
||||
fsfwParams.tmStore->getFreeElement(&storeId, pdu.getSerializedSize(), &dataPtr);
|
||||
if (result != OK) {
|
||||
addError(result);
|
||||
fsmResult.callStatus = CallStatus::CALL_AFTER_DELAY;
|
||||
if (result == StorageManagerIF::DATA_STORAGE_FULL) {
|
||||
return TM_STORE_FULL;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
size_t serializedLen = 0;
|
||||
@@ -333,7 +335,12 @@ ReturnValue_t cfdp::SourceHandler::sendGenericPdu(const SerializeIF& pdu) {
|
||||
TmTcMessage tmMsg(storeId);
|
||||
result =
|
||||
fsfwParams.msgQueue->sendMessage(fsfwParams.packetDest.getReportReceptionQueue(), &tmMsg);
|
||||
if (result == OK) {
|
||||
if (result != OK) {
|
||||
fsmResult.callStatus = CallStatus::CALL_AFTER_DELAY;
|
||||
}
|
||||
if (result == MessageQueueIF::FULL) {
|
||||
return TARGET_MSG_QUEUE_FULL;
|
||||
} else if (result == OK) {
|
||||
fsmResult.packetsSent += 1;
|
||||
}
|
||||
return result;
|
||||
@@ -364,5 +371,6 @@ void cfdp::SourceHandler::addError(ReturnValue_t error) {
|
||||
if (fsmResult.errors < fsmResult.errorCodes.size()) {
|
||||
fsmResult.errorCodes[fsmResult.errors] = error;
|
||||
fsmResult.errors++;
|
||||
fsmResult.result = error;
|
||||
}
|
||||
}
|
||||
|
@@ -30,7 +30,6 @@ class SourceHandler {
|
||||
enum class TransactionStep : uint8_t {
|
||||
IDLE = 0,
|
||||
TRANSACTION_START = 1,
|
||||
CRC_PROCEDURE = 2,
|
||||
SENDING_METADATA = 3,
|
||||
SENDING_FILE_DATA = 4,
|
||||
SENDING_EOF = 5,
|
||||
|
@@ -35,7 +35,7 @@ struct MetadataRecvdParams {
|
||||
Fss fileSize{};
|
||||
const char* sourceFileName = "";
|
||||
const char* destFileName = "";
|
||||
size_t msgsToUserLen = 0;
|
||||
size_t numberOfMsgsToUser = 0;
|
||||
const MessageToUserTlv* msgsToUserArray = nullptr;
|
||||
};
|
||||
|
||||
|
@@ -12,7 +12,7 @@ namespace cfdp {
|
||||
enum class CfdpState { IDLE, BUSY_CLASS_1_NACKED, BUSY_CLASS_2_ACKED, SUSPENDED };
|
||||
|
||||
static constexpr uint8_t SSID = SUBSYSTEM_ID::CFDP;
|
||||
static constexpr uint8_t CID = CLASS_ID::CFDP;
|
||||
static constexpr uint8_t CID = CLASS_ID::CFDP_HANDLER;
|
||||
|
||||
struct PacketInfo {
|
||||
PacketInfo(PduType type, store_address_t storeId,
|
||||
@@ -58,6 +58,8 @@ static constexpr Event SERIALIZATION_ERROR = event::makeEvent(SSID, 2, severity:
|
||||
static constexpr Event FILESTORE_ERROR = event::makeEvent(SSID, 3, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name
|
||||
static constexpr Event FILENAME_TOO_LARGE_ERROR = event::makeEvent(SSID, 4, severity::LOW);
|
||||
//! [EXPORT] : [COMMENT] CFDP request handling failed. P2: Returncode.
|
||||
static constexpr Event HANDLING_CFDP_REQUEST_FAILED = event::makeEvent(SSID, 5, severity::LOW);
|
||||
|
||||
} // namespace events
|
||||
|
||||
@@ -67,6 +69,8 @@ static constexpr ReturnValue_t FILE_SEGMENT_LEN_INVALID = returnvalue::makeCode(
|
||||
static constexpr ReturnValue_t SOURCE_NAME_EMPTY = returnvalue::makeCode(CID, 3);
|
||||
static constexpr ReturnValue_t DEST_NAME_EMPTY = returnvalue::makeCode(CID, 4);
|
||||
static constexpr ReturnValue_t WRONG_REMOTE_CFG_ENTITY_ID = returnvalue::makeCode(CID, 5);
|
||||
static constexpr ReturnValue_t TARGET_MSG_QUEUE_FULL = returnvalue::makeCode(CID, 6);
|
||||
static constexpr ReturnValue_t TM_STORE_FULL = returnvalue::makeCode(CID, 7);
|
||||
|
||||
} // namespace cfdp
|
||||
#endif // FSFW_CFDP_HANDLER_DEFS_H
|
||||
|
@@ -30,7 +30,7 @@ struct RemoteEntityCfg {
|
||||
RemoteEntityCfg() = default;
|
||||
explicit RemoteEntityCfg(EntityId id) : remoteId(std::move(id)) {}
|
||||
EntityId remoteId;
|
||||
size_t maxFileSegmentLen = 2048;
|
||||
size_t maxFileSegmentLen = 1024;
|
||||
bool closureRequested = false;
|
||||
bool crcOnTransmission = false;
|
||||
TransmissionMode defaultTransmissionMode = TransmissionMode::UNACKNOWLEDGED;
|
||||
|
@@ -21,7 +21,7 @@ void MetadataGenericInfo::setClosureRequested(bool closureRequested_) {
|
||||
closureRequested = closureRequested_;
|
||||
}
|
||||
|
||||
cfdp::Fss& MetadataGenericInfo::getFileSize() { return fileSize; }
|
||||
const cfdp::Fss& MetadataGenericInfo::getFileSize() const { return fileSize; }
|
||||
|
||||
size_t MetadataGenericInfo::getSerializedSize(bool fssLarge) {
|
||||
// 1 byte + minimal FSS 4 bytes
|
||||
@@ -31,3 +31,5 @@ size_t MetadataGenericInfo::getSerializedSize(bool fssLarge) {
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
cfdp::Fss& MetadataGenericInfo::getMutFileSize() { return fileSize; }
|
||||
|
@@ -22,7 +22,9 @@ class MetadataGenericInfo {
|
||||
[[nodiscard]] bool isClosureRequested() const;
|
||||
void setClosureRequested(bool closureRequested = false);
|
||||
|
||||
cfdp::Fss& getFileSize();
|
||||
[[nodiscard]] const cfdp::Fss& getFileSize() const;
|
||||
|
||||
cfdp::Fss& getMutFileSize();
|
||||
|
||||
private:
|
||||
bool closureRequested = false;
|
||||
|
@@ -25,7 +25,7 @@ ReturnValue_t MetadataPduReader::parseData() {
|
||||
remSize -= 1;
|
||||
buf += 1;
|
||||
auto endianness = getEndianness();
|
||||
result = info.getFileSize().deSerialize(&buf, &remSize, endianness);
|
||||
result = info.getMutFileSize().deSerialize(&buf, &remSize, endianness);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
@@ -63,3 +63,5 @@ size_t MetadataPduReader::getNumberOfParsedOptions() const { return parsedOption
|
||||
const cfdp::StringLv& MetadataPduReader::getSourceFileName() const { return srcFileName; }
|
||||
|
||||
const cfdp::StringLv& MetadataPduReader::getDestFileName() const { return destFileName; }
|
||||
|
||||
const MetadataGenericInfo& MetadataPduReader::getGenericInfo() const { return info; }
|
||||
|
@@ -11,6 +11,7 @@ class MetadataPduReader : public FileDirectiveReader {
|
||||
|
||||
ReturnValue_t parseData() override;
|
||||
|
||||
[[nodiscard]] const MetadataGenericInfo& getGenericInfo() const;
|
||||
[[nodiscard]] const cfdp::StringLv& getSourceFileName() const;
|
||||
[[nodiscard]] const cfdp::StringLv& getDestFileName() const;
|
||||
|
||||
|
@@ -95,3 +95,5 @@ cfdp::Lv& cfdp::Lv::operator=(cfdp::Lv&& other) noexcept {
|
||||
}
|
||||
|
||||
size_t cfdp::Lv::getValueLen() const { return getSerializedSize() - 1; }
|
||||
|
||||
bool cfdp::Lv::isEmpty() const { return zeroLen; }
|
||||
|
@@ -50,6 +50,8 @@ class Lv : public SerializeIF {
|
||||
*/
|
||||
const uint8_t* getValue(size_t* size) const;
|
||||
|
||||
bool isEmpty() const;
|
||||
|
||||
private:
|
||||
bool zeroLen = true;
|
||||
SerialBufferAdapter<uint8_t> value;
|
||||
|
@@ -16,7 +16,7 @@ bool MessageToUserTlv::isReservedCfdpMessage(uint8_t& messageType, const uint8_t
|
||||
if (cfdp::Tlv::getLengthField() < 5) {
|
||||
return false;
|
||||
}
|
||||
if (std::strcmp(reinterpret_cast<const char*>(getValue()), "cfdp") == 0) {
|
||||
if (std::strncmp(reinterpret_cast<const char*>(getValue()), "cfdp", 4) == 0) {
|
||||
messageType = getValue()[4];
|
||||
if (msgDataStart != nullptr) {
|
||||
*msgDataStart = getValue() + 5;
|
||||
|
@@ -54,6 +54,7 @@ ReturnValue_t cfdp::Tlv::deSerialize(const uint8_t **buffer, size_t *size,
|
||||
return STREAM_TOO_SHORT;
|
||||
}
|
||||
|
||||
rawData = *buffer;
|
||||
uint8_t rawType = **buffer;
|
||||
if (not checkType(rawType)) {
|
||||
return INVALID_TLV_TYPE;
|
||||
@@ -102,3 +103,5 @@ void cfdp::Tlv::setValue(uint8_t *value, size_t len) {
|
||||
uint8_t cfdp::Tlv::getLengthField() const { return this->value.getSerializedSize() - 1; }
|
||||
|
||||
void cfdp::Tlv::setType(TlvType type) { this->type = type; }
|
||||
|
||||
const uint8_t *cfdp::Tlv::getRawData() const { return rawData; }
|
||||
|
@@ -47,6 +47,7 @@ class Tlv : public TlvIF {
|
||||
void setValue(uint8_t *value, size_t len);
|
||||
|
||||
[[nodiscard]] const uint8_t *getValue() const;
|
||||
[[nodiscard]] const uint8_t *getRawData() const;
|
||||
void setType(TlvType type);
|
||||
[[nodiscard]] TlvType getType() const override;
|
||||
[[nodiscard]] uint8_t getLengthField() const override;
|
||||
@@ -55,6 +56,7 @@ class Tlv : public TlvIF {
|
||||
bool checkType(uint8_t rawType);
|
||||
|
||||
bool zeroLen = true;
|
||||
const uint8_t *rawData = nullptr;
|
||||
TlvType type = TlvType::INVALID_TLV;
|
||||
SerialBufferAdapter<uint8_t> value;
|
||||
};
|
||||
|
@@ -72,18 +72,17 @@ void ControllerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
|
||||
*submode_ = this->submode;
|
||||
}
|
||||
|
||||
void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
|
||||
|
||||
void ControllerBase::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
|
||||
|
||||
void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
|
||||
|
||||
void ControllerBase::setToExternalControl() { healthHelper.setHealth(EXTERNAL_CONTROL); }
|
||||
|
||||
ReturnValue_t ControllerBase::performOperation(uint8_t opCode) {
|
||||
handleQueue();
|
||||
performControlOperation();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void ControllerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
|
||||
|
||||
ReturnValue_t ControllerBase::setHealth(HealthState health) {
|
||||
switch (health) {
|
||||
case HEALTHY:
|
||||
|
@@ -6,6 +6,8 @@
|
||||
#include "fsfw/globalfunctions/constants.h"
|
||||
#include "fsfw/globalfunctions/math/MatrixOperations.h"
|
||||
#include "fsfw/globalfunctions/math/VectorOperations.h"
|
||||
#include "fsfw/globalfunctions/sign.h"
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
void CoordinateTransformations::positionEcfToEci(const double* ecfPosition, double* eciPosition,
|
||||
timeval* timeUTC) {
|
||||
@@ -97,7 +99,14 @@ void CoordinateTransformations::ecfToEci(const double* ecfCoordinates, double* e
|
||||
|
||||
double CoordinateTransformations::getJuleanCenturiesTT(timeval timeUTC) {
|
||||
timeval timeTT;
|
||||
Clock::convertUTCToTT(timeUTC, &timeTT);
|
||||
ReturnValue_t result = Clock::convertUTCToTT(timeUTC, &timeTT);
|
||||
if (result != returnvalue::OK) {
|
||||
// i think it is better to continue here than to abort
|
||||
timeTT = timeUTC;
|
||||
sif::error << "CoordinateTransformations::Conversion from UTC to TT failed. Continuing "
|
||||
"calculations with UTC."
|
||||
<< std::endl;
|
||||
}
|
||||
double jD2000TT;
|
||||
Clock::convertTimevalToJD2000(timeTT, &jD2000TT);
|
||||
|
||||
@@ -207,3 +216,61 @@ void CoordinateTransformations::getTransMatrixECITOECF(timeval timeUTC, double T
|
||||
|
||||
MatrixOperations<double>::multiply(mTheta[0], Ttemp[0], Tfi[0], 3, 3, 3);
|
||||
};
|
||||
|
||||
void CoordinateTransformations::cartesianFromLatLongAlt(const double lat, const double longi,
|
||||
const double alt, double* cartesianOutput) {
|
||||
/* @brief: cartesianFromLatLongAlt() - calculates cartesian coordinates in ECEF from latitude,
|
||||
* longitude and altitude
|
||||
* @param: lat geodetic latitude [rad]
|
||||
* longi longitude [rad]
|
||||
* alt altitude [m]
|
||||
* cartesianOutput Cartesian Coordinates in ECEF (3x1)
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.34ff
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
double radiusPolar = 6356752.314;
|
||||
double radiusEqua = 6378137;
|
||||
|
||||
double eccentricity = sqrt(1 - pow(radiusPolar, 2) / pow(radiusEqua, 2));
|
||||
double auxRadius = radiusEqua / sqrt(1 - pow(eccentricity, 2) * pow(sin(lat), 2));
|
||||
|
||||
cartesianOutput[0] = (auxRadius + alt) * cos(lat) * cos(longi);
|
||||
cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
|
||||
cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
|
||||
};
|
||||
|
||||
void CoordinateTransformations::latLongAltFromCartesian(const double* vector, double& latitude,
|
||||
double& longitude, double& altitude) {
|
||||
/* @brief: latLongAltFromCartesian() - calculates latitude, longitude and altitude from
|
||||
* cartesian coordinates in ECEF
|
||||
* @param: x x-value of position vector [m]
|
||||
* y y-value of position vector [m]
|
||||
* z z-value of position vector [m]
|
||||
* latitude geodetic latitude [rad]
|
||||
* longitude longitude [rad]
|
||||
* altitude altitude [m]
|
||||
* @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
|
||||
* Landis Markley and John L. Crassidis*/
|
||||
// From World Geodetic System the Earth Radii
|
||||
double a = 6378137.0; // semimajor axis [m]
|
||||
double b = 6356752.3142; // semiminor axis [m]
|
||||
|
||||
// Calculation
|
||||
double e2 = 1 - pow(b, 2) / pow(a, 2);
|
||||
double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
|
||||
double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
|
||||
double p = std::abs(vector[2]) / epsilon2;
|
||||
double s = pow(rho, 2) / (e2 * epsilon2);
|
||||
double q = pow(p, 2) - pow(b, 2) + s;
|
||||
double u = p / sqrt(q);
|
||||
double v = pow(b, 2) * pow(u, 2) / q;
|
||||
double P = 27 * v * s / q;
|
||||
double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
|
||||
double t = (1 + Q + 1 / Q) / 6;
|
||||
double c = sqrt(pow(u, 2) - 1 + 2 * t);
|
||||
double w = (c - u) / 2;
|
||||
double d = sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
|
||||
double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
|
||||
latitude = asin((epsilon2 + 1) * d / N);
|
||||
altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
|
||||
longitude = atan2(vector[1], vector[0]);
|
||||
}
|
||||
|
@@ -23,6 +23,12 @@ class CoordinateTransformations {
|
||||
|
||||
static void getEarthRotationMatrix(timeval timeUTC, double matrix[][3]);
|
||||
|
||||
static void cartesianFromLatLongAlt(const double lat, const double longi, const double alt,
|
||||
double* cartesianOutput);
|
||||
|
||||
static void latLongAltFromCartesian(const double* vector, double& latitude, double& longitude,
|
||||
double& altitude);
|
||||
|
||||
private:
|
||||
CoordinateTransformations();
|
||||
static void ecfToEci(const double* ecfCoordinates, double* eciCoordinates,
|
||||
|
@@ -166,9 +166,9 @@ ReturnValue_t Sgp4Propagator::propagate(double* position, double* velocity, time
|
||||
timeval timeSinceEpoch = time - epoch;
|
||||
double minutesSinceEpoch = timeSinceEpoch.tv_sec / 60. + timeSinceEpoch.tv_usec / 60000000.;
|
||||
|
||||
double monthsSinceEpoch = minutesSinceEpoch / 60 / 24 / 30;
|
||||
double daysSinceEpoch = minutesSinceEpoch / 60 / 24;
|
||||
|
||||
if ((monthsSinceEpoch > 1) || (monthsSinceEpoch < -1)) {
|
||||
if ((daysSinceEpoch > 7) || (daysSinceEpoch < -7)) {
|
||||
return TLE_TOO_OLD;
|
||||
}
|
||||
|
||||
|
@@ -7,4 +7,5 @@ target_sources(
|
||||
DeviceHandlerFailureIsolation.cpp
|
||||
DeviceHandlerMessage.cpp
|
||||
DeviceTmReportingWrapper.cpp
|
||||
FreshDeviceHandlerBase.cpp
|
||||
HealthDevice.cpp)
|
||||
|
@@ -26,6 +26,11 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
|
||||
if (isFdirInActionOrAreWeFaulty(event)) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
// As mentioned in the function documentation, no FDIR reaction are performed when the device
|
||||
// is in external control.
|
||||
if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
ReturnValue_t result = returnvalue::FAILED;
|
||||
switch (event->getEvent()) {
|
||||
case HasModesIF::MODE_TRANSITION_FAILED:
|
||||
@@ -186,15 +191,6 @@ void DeviceHandlerFailureIsolation::setFdirState(FDIRState state) {
|
||||
fdirState = state;
|
||||
}
|
||||
|
||||
void DeviceHandlerFailureIsolation::triggerEvent(Event event, uint32_t parameter1,
|
||||
uint32_t parameter2) {
|
||||
// Do not throw error events if fdirState != none.
|
||||
// This will still forward MODE and HEALTH INFO events in any case.
|
||||
if (fdirState == NONE || event::getSeverity(event) == severity::INFO) {
|
||||
FailureIsolationBase::triggerEvent(event, parameter1, parameter2);
|
||||
}
|
||||
}
|
||||
|
||||
bool DeviceHandlerFailureIsolation::isFdirActionInProgress() { return (fdirState != NONE); }
|
||||
|
||||
void DeviceHandlerFailureIsolation::startRecovery(Event reason) {
|
||||
|
@@ -17,7 +17,6 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase {
|
||||
uint8_t eventQueueDepth = 10);
|
||||
~DeviceHandlerFailureIsolation();
|
||||
ReturnValue_t initialize();
|
||||
void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
|
||||
bool isFdirActionInProgress();
|
||||
virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
@@ -41,6 +40,19 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase {
|
||||
static const uint32_t DEFAULT_MAX_MISSED_REPLY_COUNT = 5;
|
||||
static const uint32_t DEFAULT_MISSED_REPLY_TIME_MS = 10000;
|
||||
|
||||
/**
|
||||
* This is the default implementation of the eventReceived function.
|
||||
*
|
||||
* It will perform recoveries or failures on a pre-defined set of events. If the user wants
|
||||
* to add handling for custom events, this function should be overriden.
|
||||
*
|
||||
* It should be noted that the default implementation will not perform FDIR reactions if the
|
||||
* handler is faulty or in external control by default. If the user commands the device
|
||||
* manually, this might be related to debugging to testing the device in a low-level way. FDIR
|
||||
* reactions might get in the way of this process by restarting the device or putting it in
|
||||
* the faulty state. If the user still requires FDIR handling in the EXTERNAL_CONTROL case,
|
||||
* this function should be overriden.
|
||||
*/
|
||||
virtual ReturnValue_t eventReceived(EventMessage* event);
|
||||
virtual void eventConfirmed(EventMessage* event);
|
||||
void wasParentsFault(EventMessage* event);
|
||||
|
@@ -85,6 +85,7 @@ class DeviceHandlerIF {
|
||||
static const ReturnValue_t NO_REPLY_EXPECTED = MAKE_RETURN_CODE(0xA8);
|
||||
static const ReturnValue_t NON_OP_TEMPERATURE = MAKE_RETURN_CODE(0xA9);
|
||||
static const ReturnValue_t COMMAND_NOT_IMPLEMENTED = MAKE_RETURN_CODE(0xAA);
|
||||
static const ReturnValue_t NON_OP_STATE_OF_CHARGE = MAKE_RETURN_CODE(0xAB);
|
||||
|
||||
// Standard codes used in scanForReply
|
||||
static const ReturnValue_t CHECKSUM_ERROR = MAKE_RETURN_CODE(0xB0);
|
||||
|
201
src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp
Normal file
201
src/fsfw/devicehandlers/FreshDeviceHandlerBase.cpp
Normal file
@@ -0,0 +1,201 @@
|
||||
#include "FreshDeviceHandlerBase.h"
|
||||
|
||||
#include "fsfw/devicehandlers/DeviceHandlerFailureIsolation.h"
|
||||
#include "fsfw/ipc/QueueFactory.h"
|
||||
#include "fsfw/subsystem/helper.h"
|
||||
|
||||
FreshDeviceHandlerBase::FreshDeviceHandlerBase(DhbConfig config)
|
||||
: SystemObject(config.objectId),
|
||||
actionHelper(this, nullptr),
|
||||
modeHelper(this),
|
||||
healthHelper(this, getObjectId()),
|
||||
paramHelper(this),
|
||||
poolManager(this, nullptr),
|
||||
defaultFdirParent(config.defaultFdirParent) {
|
||||
auto mqArgs = MqArgs(config.objectId, static_cast<void*>(this));
|
||||
messageQueue = QueueFactory::instance()->createMessageQueue(
|
||||
config.msgQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
||||
}
|
||||
FreshDeviceHandlerBase::~FreshDeviceHandlerBase() {
|
||||
QueueFactory::instance()->deleteMessageQueue(messageQueue);
|
||||
if (not hasCustomFdir) {
|
||||
delete fdirInstance;
|
||||
}
|
||||
}
|
||||
|
||||
[[nodiscard]] object_id_t FreshDeviceHandlerBase::getObjectId() const {
|
||||
return SystemObject::getObjectId();
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::performOperation(uint8_t opCode) {
|
||||
performDeviceOperationPreQueueHandling(opCode);
|
||||
handleQueue();
|
||||
performDeviceOperation(opCode);
|
||||
poolManager.performHkOperation();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::performDeviceOperationPreQueueHandling(uint8_t opCode) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::startTransition(Mode_t mode_, Submode_t submode_) {
|
||||
triggerEvent(CHANGING_MODE, mode_, submode_);
|
||||
// Complete mode transition immediately by default.
|
||||
setMode(mode_, submode_);
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::setMode(Mode_t newMode, Submode_t newSubmode) {
|
||||
mode = newMode;
|
||||
submode = newSubmode;
|
||||
modeHelper.modeChanged(mode, submode);
|
||||
modeChanged(mode, submode);
|
||||
announceMode(false);
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::setMode(Mode_t newMode) { setMode(newMode, submode); }
|
||||
|
||||
void FreshDeviceHandlerBase::getMode(Mode_t* mode_, Submode_t* submode_) {
|
||||
*mode_ = this->mode;
|
||||
*submode_ = this->submode;
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::announceMode(bool recursive) {
|
||||
triggerEvent(MODE_INFO, mode, submode);
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::modeChanged(Mode_t mode_, Submode_t submode_) {}
|
||||
|
||||
[[nodiscard]] MessageQueueId_t FreshDeviceHandlerBase::getCommandQueue() const {
|
||||
return messageQueue->getId();
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::handleQueue() {
|
||||
CommandMessage command;
|
||||
ReturnValue_t result;
|
||||
for (result = messageQueue->receiveMessage(&command); result == returnvalue::OK;
|
||||
result = messageQueue->receiveMessage(&command)) {
|
||||
result = actionHelper.handleActionMessage(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = modeHelper.handleModeCommand(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = healthHelper.handleHealthCommand(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = paramHelper.handleParameterMessage(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = poolManager.handleHousekeepingMessage(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
|
||||
result = handleCommandMessage(&command);
|
||||
if (result == returnvalue::OK) {
|
||||
continue;
|
||||
}
|
||||
command.setToUnknownCommand();
|
||||
messageQueue->reply(&command);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
HasHealthIF::HealthState FreshDeviceHandlerBase::getHealth() { return healthHelper.getHealth(); }
|
||||
|
||||
const HasHealthIF* FreshDeviceHandlerBase::getOptHealthIF() const { return this; }
|
||||
|
||||
const HasModesIF& FreshDeviceHandlerBase::getModeIF() const { return *this; }
|
||||
|
||||
ModeTreeChildIF& FreshDeviceHandlerBase::getModeTreeChildIF() { return *this; }
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
|
||||
return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper);
|
||||
}
|
||||
|
||||
// Executable Overrides.
|
||||
void FreshDeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; }
|
||||
|
||||
// Pool Manager overrides.
|
||||
LocalDataPoolManager* FreshDeviceHandlerBase::getHkManagerHandle() { return &poolManager; }
|
||||
|
||||
[[nodiscard]] uint32_t FreshDeviceHandlerBase::getPeriodicOperationFrequency() const {
|
||||
return this->executingTask->getPeriodMs();
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::initializeAfterTaskCreation() {
|
||||
return poolManager.initializeAfterTaskCreation();
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::setHealth(HasHealthIF::HealthState health) {
|
||||
// Assembly should handle commanding to OFF.
|
||||
healthHelper.setHealth(health);
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
||||
fdirInstance->triggerEvent(event, parameter1, parameter2);
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::forwardEvent(Event event, uint32_t parameter1,
|
||||
uint32_t parameter2) const {
|
||||
fdirInstance->triggerEvent(event, parameter1, parameter2);
|
||||
}
|
||||
|
||||
void FreshDeviceHandlerBase::setToExternalControl() { setHealth(HealthState::EXTERNAL_CONTROL); }
|
||||
|
||||
// System Object overrides.
|
||||
ReturnValue_t FreshDeviceHandlerBase::initialize() {
|
||||
ReturnValue_t result = modeHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = healthHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = actionHelper.initialize(messageQueue);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
result = paramHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
result = poolManager.initialize(messageQueue);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
if (fdirInstance == nullptr) {
|
||||
hasCustomFdir = false;
|
||||
fdirInstance = new DeviceHandlerFailureIsolation(getObjectId(), defaultFdirParent);
|
||||
}
|
||||
|
||||
result = fdirInstance->initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return SystemObject::initialize();
|
||||
}
|
||||
|
||||
ReturnValue_t FreshDeviceHandlerBase::getParameter(uint8_t domainId, uint8_t uniqueId,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) {
|
||||
ReturnValue_t result =
|
||||
fdirInstance->getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex);
|
||||
if (result != INVALID_DOMAIN_ID) {
|
||||
return result;
|
||||
}
|
||||
return INVALID_DOMAIN_ID;
|
||||
}
|
167
src/fsfw/devicehandlers/FreshDeviceHandlerBase.h
Normal file
167
src/fsfw/devicehandlers/FreshDeviceHandlerBase.h
Normal file
@@ -0,0 +1,167 @@
|
||||
#pragma once
|
||||
|
||||
#include "fsfw/action.h"
|
||||
#include "fsfw/datapoollocal/HasLocalDataPoolIF.h"
|
||||
#include "fsfw/datapoollocal/LocalDataPoolManager.h"
|
||||
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
||||
#include "fsfw/fdir/FailureIsolationBase.h"
|
||||
#include "fsfw/health/HasHealthIF.h"
|
||||
#include "fsfw/health/HealthHelper.h"
|
||||
#include "fsfw/modes/HasModesIF.h"
|
||||
#include "fsfw/objectmanager.h"
|
||||
#include "fsfw/parameters/ParameterHelper.h"
|
||||
#include "fsfw/parameters/ReceivesParameterMessagesIF.h"
|
||||
#include "fsfw/retval.h"
|
||||
#include "fsfw/subsystem/ModeTreeChildIF.h"
|
||||
#include "fsfw/subsystem/ModeTreeConnectionIF.h"
|
||||
#include "fsfw/tasks/ExecutableObjectIF.h"
|
||||
#include "fsfw/tasks/PeriodicTaskIF.h"
|
||||
|
||||
struct DhbConfig {
|
||||
explicit DhbConfig(object_id_t objectId) : objectId(objectId) {}
|
||||
object_id_t objectId;
|
||||
FailureIsolationBase* fdirInstance = nullptr;
|
||||
object_id_t defaultFdirParent = objects::NO_OBJECT;
|
||||
uint32_t msgQueueDepth = 10;
|
||||
};
|
||||
|
||||
class FreshDeviceHandlerBase : public SystemObject,
|
||||
public DeviceHandlerIF,
|
||||
public HasModesIF,
|
||||
public HasHealthIF,
|
||||
public ExecutableObjectIF,
|
||||
public ModeTreeChildIF,
|
||||
public ModeTreeConnectionIF,
|
||||
public HasActionsIF,
|
||||
public ReceivesParameterMessagesIF,
|
||||
public HasLocalDataPoolIF {
|
||||
public:
|
||||
explicit FreshDeviceHandlerBase(DhbConfig config);
|
||||
~FreshDeviceHandlerBase() override;
|
||||
|
||||
/**
|
||||
* Periodic helper executed function, implemented by child class.
|
||||
*/
|
||||
virtual void performDeviceOperation(uint8_t opCode) = 0;
|
||||
|
||||
[[nodiscard]] object_id_t getObjectId() const override;
|
||||
|
||||
[[nodiscard]] MessageQueueId_t getCommandQueue() const override;
|
||||
|
||||
HasHealthIF::HealthState getHealth() override;
|
||||
|
||||
// Mode Tree Overrides.
|
||||
[[nodiscard]] const HasHealthIF* getOptHealthIF() const override;
|
||||
[[nodiscard]] const HasModesIF& getModeIF() const override;
|
||||
ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) override;
|
||||
ModeTreeChildIF& getModeTreeChildIF() override;
|
||||
|
||||
[[nodiscard]] uint32_t getPeriodicOperationFrequency() const override;
|
||||
|
||||
protected:
|
||||
// Pool Manager overrides.
|
||||
LocalDataPoolManager* getHkManagerHandle() override;
|
||||
ActionHelper actionHelper;
|
||||
ModeHelper modeHelper;
|
||||
HealthHelper healthHelper;
|
||||
ParameterHelper paramHelper;
|
||||
LocalDataPoolManager poolManager;
|
||||
|
||||
bool hasCustomFdir = false;
|
||||
FailureIsolationBase* fdirInstance;
|
||||
object_id_t defaultFdirParent;
|
||||
|
||||
/**
|
||||
* Pointer to the task which executes this component,
|
||||
* is invalid before setTaskIF was called.
|
||||
*/
|
||||
PeriodicTaskIF* executingTask = nullptr;
|
||||
|
||||
Mode_t mode = HasModesIF::MODE_UNDEFINED;
|
||||
Submode_t submode = 0;
|
||||
|
||||
MessageQueueIF* messageQueue;
|
||||
|
||||
/**
|
||||
* The default queue handler will process all messages for the interfaces implemented
|
||||
* by this class. If there are special requirements, for example that action commands are
|
||||
* received on a different queue, the user can override this function for those special
|
||||
* requirements.
|
||||
*/
|
||||
virtual ReturnValue_t handleQueue();
|
||||
|
||||
// Mode Helpers.
|
||||
virtual void modeChanged(Mode_t mode, Submode_t submode);
|
||||
/**
|
||||
* The default implementation sets the new mode immediately. If this is not applicable for
|
||||
* certain modes, the user should provide a custom implementation, which performs rougly
|
||||
* the same functionality of this function, when all the steps have been taken to reach the
|
||||
* new mode.
|
||||
*/
|
||||
void startTransition(Mode_t mode, Submode_t submode) override;
|
||||
virtual void setMode(Mode_t newMode, Submode_t newSubmode);
|
||||
virtual void setMode(Mode_t newMode);
|
||||
void getMode(Mode_t* mode, Submode_t* submode) override;
|
||||
void setToExternalControl() override;
|
||||
void announceMode(bool recursive) override;
|
||||
// System Object overrides.
|
||||
ReturnValue_t initialize() override;
|
||||
|
||||
/**
|
||||
* Implemented by child class. Handle all command messages which are
|
||||
* not health, mode, action or housekeeping messages.
|
||||
* @param message
|
||||
* @return
|
||||
*/
|
||||
virtual ReturnValue_t handleCommandMessage(CommandMessage* message) = 0;
|
||||
|
||||
// HK manager abstract functions.
|
||||
LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override = 0;
|
||||
ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap,
|
||||
LocalDataPoolManager& poolManager) override = 0;
|
||||
|
||||
// Mode abstract functions
|
||||
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t* msToReachTheMode) override = 0;
|
||||
// Health Overrides.
|
||||
ReturnValue_t setHealth(HealthState health) override;
|
||||
// Action override. Forward to user.
|
||||
ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
||||
const uint8_t* data, size_t size) override = 0;
|
||||
// Executable overrides.
|
||||
ReturnValue_t performOperation(uint8_t opCode) override;
|
||||
ReturnValue_t initializeAfterTaskCreation() override;
|
||||
|
||||
/**
|
||||
* This calls the FDIR instance event trigger function.
|
||||
* @param event
|
||||
* @param parameter1
|
||||
* @param parameter2
|
||||
*/
|
||||
void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) override;
|
||||
/**
|
||||
* This calls the FDIR instance event forward function.
|
||||
* @param event
|
||||
* @param parameter1
|
||||
* @param parameter2
|
||||
*/
|
||||
void forwardEvent(Event event, uint32_t parameter1, uint32_t parameter2) const override;
|
||||
/**
|
||||
* This implementation handles the FDIR parameters. The user can override this to handle
|
||||
* custom parameters.
|
||||
* @param domainId
|
||||
* @param uniqueId
|
||||
* @param parameterWrapper
|
||||
* @param newValues
|
||||
* @param startAtIndex
|
||||
* @return
|
||||
*/
|
||||
ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues, uint16_t startAtIndex) override;
|
||||
|
||||
virtual ReturnValue_t performDeviceOperationPreQueueHandling(uint8_t opCode);
|
||||
|
||||
private:
|
||||
// Executable Overrides.
|
||||
void setTaskIF(PeriodicTaskIF* task) override;
|
||||
};
|
@@ -148,25 +148,16 @@ void FailureIsolationBase::doConfirmFault(EventMessage* event) {
|
||||
ReturnValue_t FailureIsolationBase::confirmFault(EventMessage* event) { return YOUR_FAULT; }
|
||||
|
||||
void FailureIsolationBase::triggerEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
||||
// With this mechanism, all events are disabled for a certain device.
|
||||
// That's not so good for visibility.
|
||||
if (isFdirDisabledForSeverity(event::getSeverity(event))) {
|
||||
return;
|
||||
}
|
||||
// By default, we trigger all events and also call the handler function to handle FDIR reactions
|
||||
// which might occur due to these events. This makes all events visible. If the handling of
|
||||
// FDIR reaction should be disabled, this should be done through dedicated logic inside the
|
||||
// eventReceived function.
|
||||
EventMessage message(event, ownerId, parameter1, parameter2);
|
||||
EventManagerIF::triggerEvent(&message, eventQueue->getId());
|
||||
eventReceived(&message);
|
||||
}
|
||||
|
||||
bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) {
|
||||
if ((owner != NULL) && (severity != severity::INFO)) {
|
||||
if (owner->getHealth() == HasHealthIF::EXTERNAL_CONTROL) {
|
||||
// External control disables handling of fault messages.
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
bool FailureIsolationBase::isFdirDisabledForSeverity(EventSeverity_t severity) { return false; }
|
||||
|
||||
void FailureIsolationBase::throwFdirEvent(Event event, uint32_t parameter1, uint32_t parameter2) {
|
||||
EventMessage message(event, ownerId, parameter1, parameter2);
|
||||
|
@@ -44,13 +44,13 @@ class FailureIsolationBase : public ConfirmsFailuresIF, public HasParametersIF {
|
||||
virtual void wasParentsFault(EventMessage* event);
|
||||
virtual ReturnValue_t confirmFault(EventMessage* event);
|
||||
virtual void decrementFaultCounters() = 0;
|
||||
virtual bool isFdirDisabledForSeverity(EventSeverity_t severity);
|
||||
ReturnValue_t sendConfirmationRequest(EventMessage* event,
|
||||
MessageQueueId_t destination = MessageQueueIF::NO_QUEUE);
|
||||
void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0);
|
||||
|
||||
private:
|
||||
void doConfirmFault(EventMessage* event);
|
||||
bool isFdirDisabledForSeverity(EventSeverity_t severity);
|
||||
};
|
||||
|
||||
#endif /* FRAMEWORK_FDIR_FAILUREISOLATIONBASE_H_ */
|
||||
#endif /* FRAMEWORK_FDIR */
|
||||
|
@@ -1,9 +1,12 @@
|
||||
#ifndef MATRIXOPERATIONS_H_
|
||||
#define MATRIXOPERATIONS_H_
|
||||
|
||||
#include <fsfw/retval.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <utility>
|
||||
|
||||
template <typename T1, typename T2 = T1, typename T3 = T2>
|
||||
class MatrixOperations {
|
||||
@@ -95,6 +98,139 @@ class MatrixOperations {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool isFinite(const T1 *inputMatrix, uint8_t rows, uint8_t cols) {
|
||||
for (uint8_t col = 0; col < cols; col++) {
|
||||
for (uint8_t row = 0; row < rows; row++) {
|
||||
if (not std::isfinite(inputMatrix[row * cols + cols])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
static void writeSubmatrix(T1 *mainMatrix, T1 *subMatrix, uint8_t subRows, uint8_t subCols,
|
||||
uint8_t mainRows, uint8_t mainCols, uint8_t startRow,
|
||||
uint8_t startCol) {
|
||||
if ((startRow + subRows > mainRows) or (startCol + subCols > mainCols)) {
|
||||
return;
|
||||
}
|
||||
for (uint8_t row = 0; row < subRows; row++) {
|
||||
for (uint8_t col = 0; col < subCols; col++) {
|
||||
mainMatrix[(startRow + row) * mainCols + (startCol + col)] = subMatrix[row * subCols + col];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static ReturnValue_t inverseMatrix(const T1 *inputMatrix, T1 *inverse, uint8_t size) {
|
||||
// Stopwatch stopwatch;
|
||||
T1 matrix[size][size], identity[size][size];
|
||||
// reformat array to matrix
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
for (uint8_t col = 0; col < size; col++) {
|
||||
matrix[row][col] = inputMatrix[row * size + col];
|
||||
}
|
||||
}
|
||||
// init identity matrix
|
||||
std::memset(identity, 0.0, sizeof(identity));
|
||||
for (uint8_t diag = 0; diag < size; diag++) {
|
||||
identity[diag][diag] = 1;
|
||||
}
|
||||
// gauss-jordan algo
|
||||
// sort matrix such as no diag entry shall be 0
|
||||
for (uint8_t row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
bool swaped = false;
|
||||
uint8_t rowIndex = 0;
|
||||
while ((rowIndex < size) && !swaped) {
|
||||
if ((matrix[rowIndex][row] != 0.0) && (matrix[row][rowIndex] != 0.0)) {
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
swaped = true;
|
||||
}
|
||||
rowIndex++;
|
||||
}
|
||||
if (!swaped) {
|
||||
return returnvalue::FAILED; // matrix not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int row = 0; row < size; row++) {
|
||||
if (matrix[row][row] == 0.0) {
|
||||
uint8_t rowIndex;
|
||||
if (row == 0) {
|
||||
rowIndex = size - 1;
|
||||
} else {
|
||||
rowIndex = row - 1;
|
||||
}
|
||||
for (uint8_t colIndex = 0; colIndex < size; colIndex++) {
|
||||
std::swap(matrix[row][colIndex], matrix[rowIndex][colIndex]);
|
||||
std::swap(identity[row][colIndex], identity[rowIndex][colIndex]);
|
||||
}
|
||||
row--;
|
||||
if (row < 0) {
|
||||
return returnvalue::FAILED; // Matrix is not invertible
|
||||
}
|
||||
}
|
||||
}
|
||||
// remove non diag elements in matrix (jordan)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int rowIndex = 0; rowIndex < size; rowIndex++) {
|
||||
if (row != rowIndex) {
|
||||
double ratio = matrix[rowIndex][row] / matrix[row][row];
|
||||
for (int colIndex = 0; colIndex < size; colIndex++) {
|
||||
matrix[rowIndex][colIndex] -= ratio * matrix[row][colIndex];
|
||||
identity[rowIndex][colIndex] -= ratio * identity[row][colIndex];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// normalize rows in matrix (gauss)
|
||||
for (int row = 0; row < size; row++) {
|
||||
for (int col = 0; col < size; col++) {
|
||||
identity[row][col] = identity[row][col] / matrix[row][row];
|
||||
}
|
||||
}
|
||||
std::memcpy(inverse, identity, sizeof(identity));
|
||||
return returnvalue::OK; // successful inversion
|
||||
}
|
||||
|
||||
static void inverseMatrixDimThree(const T1 *matrix, T1 *output) {
|
||||
int i, j;
|
||||
double determinant = 0;
|
||||
double mat[3][3] = {{matrix[0], matrix[1], matrix[2]},
|
||||
{matrix[3], matrix[4], matrix[5]},
|
||||
{matrix[6], matrix[7], matrix[8]}};
|
||||
|
||||
for (i = 0; i < 3; i++) {
|
||||
determinant = determinant + (mat[0][i] * (mat[1][(i + 1) % 3] * mat[2][(i + 2) % 3] -
|
||||
mat[1][(i + 2) % 3] * mat[2][(i + 1) % 3]));
|
||||
}
|
||||
for (i = 0; i < 3; i++) {
|
||||
for (j = 0; j < 3; j++) {
|
||||
output[i * 3 + j] = ((mat[(j + 1) % 3][(i + 1) % 3] * mat[(j + 2) % 3][(i + 2) % 3]) -
|
||||
(mat[(j + 1) % 3][(i + 2) % 3] * mat[(j + 2) % 3][(i + 1) % 3])) /
|
||||
determinant;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void skewMatrix(const T1 *vector, T2 *result) {
|
||||
// Input Dimension [3], Output [3][3]
|
||||
result[0] = 0;
|
||||
result[1] = -vector[2];
|
||||
result[2] = vector[1];
|
||||
result[3] = vector[2];
|
||||
result[4] = 0;
|
||||
result[5] = -vector[0];
|
||||
result[6] = -vector[1];
|
||||
result[7] = vector[0];
|
||||
result[8] = 0;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* MATRIXOPERATIONS_H_ */
|
||||
|
@@ -39,6 +39,48 @@ void QuaternionOperations::inverse(const double* quaternion, double* inverseQuat
|
||||
VectorOperations<double>::mulScalar(inverseQuaternion, -1, inverseQuaternion, 3);
|
||||
}
|
||||
|
||||
void QuaternionOperations::slerp(const double q1[4], const double q2[4], const double weight,
|
||||
double q[4]) {
|
||||
double q1s[4] = {0, 0, 0, 0}, q2I[4] = {0, 0, 0, 0}, qD[4] = {0, 0, 0, 0}, left[4] = {0, 0, 0, 0},
|
||||
right[4] = {0, 0, 0, 0}, angle = 0;
|
||||
|
||||
// we need to be able to invert this quaternion
|
||||
std::memcpy(q1s, q1, 4 * sizeof(double));
|
||||
// calculate angle between orientations
|
||||
inverse(q2, q2I);
|
||||
multiply(q1s, q2I, qD);
|
||||
angle = std::acos(qD[3]);
|
||||
|
||||
if (std::cos(angle) < 0.0) {
|
||||
// we need to invert one quaternion
|
||||
VectorOperations<double>::mulScalar(q1s, -1, q1s, 4);
|
||||
multiply(q1s, q2I, qD);
|
||||
angle = std::acos(qD[3]);
|
||||
}
|
||||
|
||||
if (std::sin(angle) == 0.0) {
|
||||
// nothing to calculate here
|
||||
std::memcpy(q, q1s, 4 * sizeof(double));
|
||||
return;
|
||||
}
|
||||
|
||||
VectorOperations<double>::mulScalar(q1s, std::sin((1 - weight) * angle) / std::sin(angle), left,
|
||||
4);
|
||||
VectorOperations<double>::mulScalar(q2, std::sin(weight * angle) / std::sin(angle), right, 4);
|
||||
VectorOperations<double>::add(left, right, q, 4);
|
||||
|
||||
normalize(q);
|
||||
}
|
||||
|
||||
void QuaternionOperations::preventSignJump(double qNew[4], const double qOld[4]) {
|
||||
double qDiff[4] = {0, 0, 0, 0}, qSum[4] = {0, 0, 0, 0};
|
||||
VectorOperations<double>::subtract(qOld, qNew, qDiff, 4);
|
||||
VectorOperations<double>::add(qOld, qNew, qSum, 4);
|
||||
if (VectorOperations<double>::norm(qDiff, 4) > VectorOperations<double>::norm(qSum, 4)) {
|
||||
VectorOperations<double>::mulScalar(qNew, -1, qNew, 4);
|
||||
}
|
||||
}
|
||||
|
||||
QuaternionOperations::QuaternionOperations() {}
|
||||
|
||||
void QuaternionOperations::normalize(const double* quaternion, double* unitQuaternion) {
|
||||
@@ -120,3 +162,25 @@ double QuaternionOperations::getAngle(const double* quaternion, bool abs) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void QuaternionOperations::rotationFromQuaternions(const double qNew[4], const double qOld[4],
|
||||
const double timeDelta, double rotRate[3]) {
|
||||
double qOldInv[4] = {0, 0, 0, 0};
|
||||
double qDelta[4] = {0, 0, 0, 0};
|
||||
|
||||
inverse(qOld, qOldInv);
|
||||
multiply(qNew, qOldInv, qDelta);
|
||||
if (VectorOperations<double>::norm(qDelta, 4) != 0.0) {
|
||||
normalize(qDelta);
|
||||
}
|
||||
if (VectorOperations<double>::norm(qDelta, 3) == 0.0) {
|
||||
rotRate[0] = 0.0;
|
||||
rotRate[1] = 0.0;
|
||||
rotRate[2] = 0.0;
|
||||
return;
|
||||
}
|
||||
double rotVec[3] = {0, 0, 0};
|
||||
double angle = getAngle(qDelta);
|
||||
VectorOperations<double>::normalize(qDelta, rotVec, 3);
|
||||
VectorOperations<double>::mulScalar(rotVec, angle / timeDelta, rotRate, 3);
|
||||
}
|
||||
|
@@ -23,6 +23,13 @@ class QuaternionOperations {
|
||||
|
||||
static void inverse(const double *quaternion, double *inverseQuaternion);
|
||||
|
||||
static void slerp(const double q1[4], const double q2[4], const double weight, double q[4]);
|
||||
|
||||
static void rotationFromQuaternions(const double qNew[4], const double qOld[4],
|
||||
const double timeDelta, double rotRate[3]);
|
||||
|
||||
static void preventSignJump(double qNew[4], const double qOld[4]);
|
||||
|
||||
/**
|
||||
* returns angle in ]-Pi;Pi] or [0;Pi] if abs == true
|
||||
*/
|
||||
|
@@ -99,6 +99,15 @@ class VectorOperations {
|
||||
|
||||
static void copy(const T *in, T *out, uint8_t size) { mulScalar(in, 1, out, size); }
|
||||
|
||||
static bool isFinite(const T *inputVector, uint8_t size) {
|
||||
for (uint8_t i = 0; i < size; i++) {
|
||||
if (not std::isfinite(inputVector[i])) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
VectorOperations();
|
||||
};
|
||||
|
@@ -21,7 +21,7 @@ TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination,
|
||||
: TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, msgQueueDepth, tmStoreId, tcStoreId) {
|
||||
mutex = MutexFactory::instance()->createMutex();
|
||||
// Connection is always up, TM is requested by connecting to server and receiving packets
|
||||
registerCommConnect();
|
||||
TmTcBridge::registerCommConnect();
|
||||
}
|
||||
|
||||
ReturnValue_t TcpTmTcBridge::initialize() {
|
||||
|
@@ -123,6 +123,7 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb
|
||||
StorageManagerIF* tmStore = nullptr;
|
||||
|
||||
private:
|
||||
//! [EXPORT] : [SKIP]
|
||||
static constexpr ReturnValue_t CONN_BROKEN = returnvalue::makeCode(1, 0);
|
||||
//! TMTC bridge is cached.
|
||||
object_id_t tmtcBridgeId = objects::NO_OBJECT;
|
||||
|
@@ -38,6 +38,8 @@ class PowerSwitcherComponent : public SystemObject,
|
||||
|
||||
protected:
|
||||
PowerSwitcher switcher;
|
||||
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t *msToReachTheMode) override;
|
||||
|
||||
private:
|
||||
MessageQueueIF *queue = nullptr;
|
||||
@@ -56,8 +58,6 @@ class PowerSwitcherComponent : public SystemObject,
|
||||
|
||||
[[nodiscard]] MessageQueueId_t getCommandQueue() const override;
|
||||
void getMode(Mode_t *mode, Submode_t *submode) override;
|
||||
ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode,
|
||||
uint32_t *msToReachTheMode) override;
|
||||
void startTransition(Mode_t mode, Submode_t submode) override;
|
||||
virtual void performFaultyOperation();
|
||||
void setToExternalControl() override;
|
||||
|
@@ -43,6 +43,7 @@ class Service11TelecommandScheduling final : public PusServiceBase {
|
||||
static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4);
|
||||
static constexpr ReturnValue_t CONTAINED_TC_TOO_SMALL = returnvalue::makeCode(CLASS_ID, 5);
|
||||
static constexpr ReturnValue_t CONTAINED_TC_CRC_MISSMATCH = returnvalue::makeCode(CLASS_ID, 6);
|
||||
static constexpr ReturnValue_t MAP_IS_FULL = returnvalue::makeCode(CLASS_ID, 7);
|
||||
|
||||
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11;
|
||||
|
||||
|
@@ -150,6 +150,9 @@ inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::handleResetCom
|
||||
template <size_t MAX_NUM_TCS>
|
||||
inline ReturnValue_t Service11TelecommandScheduling<MAX_NUM_TCS>::doInsertActivity(
|
||||
const uint8_t *data, size_t size) {
|
||||
if (telecommandMap.full()) {
|
||||
return MAP_IS_FULL;
|
||||
}
|
||||
uint32_t timestamp = 0;
|
||||
ReturnValue_t result = SerializeAdapter::deSerialize(×tamp, &data, &size, DEF_END);
|
||||
if (result != returnvalue::OK) {
|
||||
|
@@ -2,9 +2,9 @@
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "fsfw/events/EventManagerIF.h"
|
||||
#include "fsfw/pus/servicepackets/Service9Packets.h"
|
||||
#include "fsfw/serviceinterface/ServiceInterface.h"
|
||||
#include "fsfw/returnvalues/returnvalue.h"
|
||||
#include "fsfw/serialize/SerializeAdapter.h"
|
||||
#include "fsfw/timemanager/CCSDSTime.h"
|
||||
|
||||
Service9TimeManagement::Service9TimeManagement(PsbParams params) : PusServiceBase(params) {
|
||||
@@ -18,16 +18,53 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK;
|
||||
ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) {
|
||||
switch (subservice) {
|
||||
case Subservice::SET_TIME: {
|
||||
return setTime();
|
||||
reportCurrentTime(CLOCK_DUMP_BEFORE_SETTING_TIME);
|
||||
ReturnValue_t result = setTime();
|
||||
reportCurrentTime(CLOCK_DUMP_AFTER_SETTING_TIME);
|
||||
return result;
|
||||
}
|
||||
case Subservice::DUMP_TIME: {
|
||||
timeval newTime;
|
||||
Clock::getClock_timeval(&newTime);
|
||||
uint32_t subsecondMs =
|
||||
static_cast<uint32_t>(std::floor(static_cast<double>(newTime.tv_usec) / 1000.0));
|
||||
triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs);
|
||||
reportCurrentTime();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
case Subservice::RELATIVE_TIMESHIFT: {
|
||||
timeval currentTime;
|
||||
ReturnValue_t result = Clock::getClock(¤tTime);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
reportTime(CLOCK_DUMP_BEFORE_SETTING_TIME, currentTime);
|
||||
|
||||
if (currentPacket.getUserDataLen() != 8) {
|
||||
return AcceptsTelecommandsIF::ILLEGAL_APPLICATION_DATA;
|
||||
}
|
||||
size_t deserLen = 8;
|
||||
int64_t timeshiftNanos = 0;
|
||||
result = SerializeAdapter::deSerialize(×hiftNanos, currentPacket.getUserData(),
|
||||
&deserLen, SerializeIF::Endianness::NETWORK);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
bool positiveShift = true;
|
||||
if (timeshiftNanos < 0) {
|
||||
positiveShift = false;
|
||||
}
|
||||
timeval offset{};
|
||||
offset.tv_sec = std::abs(timeshiftNanos) / NANOS_PER_SECOND;
|
||||
offset.tv_usec = (std::abs(timeshiftNanos) % NANOS_PER_SECOND) / 1000;
|
||||
|
||||
timeval newTime;
|
||||
if (positiveShift) {
|
||||
newTime = currentTime + offset;
|
||||
} else {
|
||||
newTime = currentTime - offset;
|
||||
}
|
||||
result = Clock::setClock(&newTime);
|
||||
if (result == returnvalue::OK) {
|
||||
reportTime(CLOCK_DUMP_AFTER_SETTING_TIME, newTime);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
default:
|
||||
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
||||
}
|
||||
@@ -43,17 +80,20 @@ ReturnValue_t Service9TimeManagement::setTime() {
|
||||
return result;
|
||||
}
|
||||
|
||||
timeval time;
|
||||
Clock::getClock_timeval(&time);
|
||||
result = Clock::setClock(&timeToSet);
|
||||
|
||||
if (result == returnvalue::OK) {
|
||||
timeval newTime;
|
||||
Clock::getClock_timeval(&newTime);
|
||||
triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec);
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
if (result != returnvalue::OK) {
|
||||
triggerEvent(CLOCK_SET_FAILURE, result, 0);
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void Service9TimeManagement::reportCurrentTime(Event event) {
|
||||
timeval currentTime{};
|
||||
Clock::getClock(¤tTime);
|
||||
triggerEvent(event, currentTime.tv_sec, currentTime.tv_usec);
|
||||
}
|
||||
|
||||
void Service9TimeManagement::reportTime(Event event, timeval time) {
|
||||
triggerEvent(event, time.tv_sec, time.tv_usec);
|
||||
}
|
||||
|
@@ -1,18 +1,25 @@
|
||||
#ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
|
||||
#define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_
|
||||
|
||||
#include "fsfw/returnvalues/returnvalue.h"
|
||||
#include "fsfw/tmtcservices/PusServiceBase.h"
|
||||
|
||||
class Service9TimeManagement : public PusServiceBase {
|
||||
public:
|
||||
static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9;
|
||||
|
||||
//!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
|
||||
static constexpr uint32_t NANOS_PER_SECOND = 1'000'000'000;
|
||||
|
||||
//!< [EXPORT] : [COMMENT] Clock has been set. P1: old timeval seconds. P2: new timeval seconds.
|
||||
static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO);
|
||||
//!< Clock dump event. P1: timeval seconds P2: timeval milliseconds.
|
||||
static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO);
|
||||
//!< Clock could not be set. P1: Returncode.
|
||||
//!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval milliseconds.
|
||||
static constexpr Event CLOCK_DUMP_LEGACY = MAKE_EVENT(1, severity::INFO);
|
||||
//!< [EXPORT] : [COMMENT] Clock could not be set. P1: Returncode.
|
||||
static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW);
|
||||
//!< [EXPORT] : [COMMENT] Clock dump event. P1: timeval seconds P2: timeval microseconds.
|
||||
static constexpr Event CLOCK_DUMP = MAKE_EVENT(3, severity::INFO);
|
||||
static constexpr Event CLOCK_DUMP_BEFORE_SETTING_TIME = MAKE_EVENT(4, severity::INFO);
|
||||
static constexpr Event CLOCK_DUMP_AFTER_SETTING_TIME = MAKE_EVENT(5, severity::INFO);
|
||||
|
||||
static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9;
|
||||
|
||||
@@ -30,12 +37,16 @@ class Service9TimeManagement : public PusServiceBase {
|
||||
*/
|
||||
ReturnValue_t handleRequest(uint8_t subservice) override;
|
||||
|
||||
void reportCurrentTime(Event eventType = CLOCK_DUMP);
|
||||
void reportTime(Event event, timeval time);
|
||||
|
||||
virtual ReturnValue_t setTime();
|
||||
|
||||
private:
|
||||
enum Subservice {
|
||||
SET_TIME = 128, //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format
|
||||
DUMP_TIME = 129,
|
||||
RELATIVE_TIMESHIFT = 130,
|
||||
};
|
||||
};
|
||||
|
||||
|
@@ -61,7 +61,8 @@ enum : uint8_t {
|
||||
HAS_ACTIONS_IF, // HF
|
||||
DEVICE_COMMUNICATION_IF, // DC
|
||||
BSP, // BSP
|
||||
CFDP, // CFDP
|
||||
CFDP_BASE, // CFDP
|
||||
CFDP_HANDLER, // CFDP
|
||||
TIME_STAMPER_IF, // TSI
|
||||
SGP4PROPAGATOR_CLASS, // SGP4
|
||||
MUTEX_IF, // MUX
|
||||
|
@@ -107,14 +107,6 @@ uint8_t* SerialBufferAdapter<count_t>::getBuffer() {
|
||||
|
||||
template <typename count_t>
|
||||
const uint8_t* SerialBufferAdapter<count_t>::getConstBuffer() const {
|
||||
if (constBuffer == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "SerialBufferAdapter::getConstBuffer:"
|
||||
" Buffers are unitialized!"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return nullptr;
|
||||
}
|
||||
return constBuffer;
|
||||
}
|
||||
|
||||
|
@@ -63,7 +63,17 @@ class SerialBufferAdapter : public SerializeIF {
|
||||
ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
|
||||
Endianness streamEndianness) override;
|
||||
|
||||
/**
|
||||
* Please note that this function can also return a nullpointer in case the length field contains
|
||||
* 0.
|
||||
* @return
|
||||
*/
|
||||
uint8_t* getBuffer();
|
||||
/**
|
||||
* Please note that this function can also return a nullpointer in case the length field contains
|
||||
* 0.
|
||||
* @return
|
||||
*/
|
||||
[[nodiscard]] const uint8_t* getConstBuffer() const;
|
||||
void setConstBuffer(const uint8_t* buf, count_t bufLen);
|
||||
|
||||
|
@@ -89,7 +89,7 @@ ReturnValue_t LocalPool::deleteData(store_address_t storeId) {
|
||||
ReturnValue_t status = returnvalue::OK;
|
||||
size_type pageSize = getSubpoolElementSize(storeId.poolIndex);
|
||||
if ((pageSize != 0) and (storeId.packetIndex < numberOfElements[storeId.poolIndex])) {
|
||||
uint16_t packetPosition = getRawPosition(storeId);
|
||||
size_type packetPosition = getRawPosition(storeId);
|
||||
uint8_t* ptr = &store[storeId.poolIndex][packetPosition];
|
||||
std::memset(ptr, 0, pageSize);
|
||||
// Set free list
|
||||
|
@@ -40,7 +40,7 @@ void PusServiceBase::setTaskIF(PeriodicTaskIF* taskHandle_) { this->taskHandle =
|
||||
void PusServiceBase::handleRequestQueue() {
|
||||
TmTcMessage message;
|
||||
ReturnValue_t result;
|
||||
for (uint8_t count = 0; count < PUS_SERVICE_MAX_RECEPTION; count++) {
|
||||
for (uint8_t count = 0; count < psbParams.maxPacketsPerCycle; count++) {
|
||||
ReturnValue_t status = psbParams.reqQueue->receiveMessage(&message);
|
||||
if (status == MessageQueueIF::EMPTY) {
|
||||
break;
|
||||
@@ -98,7 +98,7 @@ ReturnValue_t PusServiceBase::initialize() {
|
||||
}
|
||||
if (psbParams.reqQueue == nullptr) {
|
||||
ownedQueue = true;
|
||||
psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(PSB_DEFAULT_QUEUE_DEPTH);
|
||||
psbParams.reqQueue = QueueFactory::instance()->createMessageQueue(psbParams.requestQueueDepth);
|
||||
} else {
|
||||
ownedQueue = false;
|
||||
}
|
||||
|
@@ -20,6 +20,14 @@ class StorageManagerIF;
|
||||
* Configuration parameters for the PUS Service Base
|
||||
*/
|
||||
struct PsbParams {
|
||||
static constexpr uint8_t PSB_DEFAULT_QUEUE_DEPTH = 10;
|
||||
/**
|
||||
* This constant sets the maximum number of packets accepted per call.
|
||||
* Remember that one packet must be completely handled in one
|
||||
* #handleRequest call.
|
||||
*/
|
||||
static constexpr uint8_t MAX_PACKETS_PER_CYCLE = 10;
|
||||
|
||||
PsbParams() = default;
|
||||
PsbParams(uint16_t apid, AcceptsTelemetryIF* tmReceiver) : apid(apid), tmReceiver(tmReceiver) {}
|
||||
PsbParams(const char* name, uint16_t apid, AcceptsTelemetryIF* tmReceiver)
|
||||
@@ -32,6 +40,9 @@ struct PsbParams {
|
||||
object_id_t objectId = objects::NO_OBJECT;
|
||||
uint16_t apid = 0;
|
||||
uint8_t serviceId = 0;
|
||||
uint32_t requestQueueDepth = PSB_DEFAULT_QUEUE_DEPTH;
|
||||
uint32_t maxPacketsPerCycle = MAX_PACKETS_PER_CYCLE;
|
||||
|
||||
/**
|
||||
* The default destination ID for generated telemetry. If this is not set, @initialize of PSB
|
||||
* will attempt to find a suitable object with the object ID @PusServiceBase::packetDestination
|
||||
@@ -100,14 +111,6 @@ class PusServiceBase : public ExecutableObjectIF,
|
||||
friend void Factory::setStaticFrameworkObjectIds();
|
||||
|
||||
public:
|
||||
/**
|
||||
* This constant sets the maximum number of packets accepted per call.
|
||||
* Remember that one packet must be completely handled in one
|
||||
* #handleRequest call.
|
||||
*/
|
||||
static constexpr uint8_t PUS_SERVICE_MAX_RECEPTION = 10;
|
||||
static constexpr uint8_t PSB_DEFAULT_QUEUE_DEPTH = 10;
|
||||
|
||||
/**
|
||||
* @brief The passed values are set, but inter-object initialization is
|
||||
* done in the initialize method.
|
||||
|
@@ -23,18 +23,8 @@ TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDes
|
||||
|
||||
TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
|
||||
|
||||
ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerCycle) {
|
||||
if (sentPacketsPerCycle <= LIMIT_STORED_DATA_SENT_PER_CYCLE) {
|
||||
this->sentPacketsPerCycle = sentPacketsPerCycle;
|
||||
return returnvalue::OK;
|
||||
} else {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "TmTcBridge::setNumberOfSentPacketsPerCycle: Number of "
|
||||
<< "packets sent per cycle exceeds limits. "
|
||||
<< "Keeping default value." << std::endl;
|
||||
#endif
|
||||
return returnvalue::FAILED;
|
||||
}
|
||||
void TmTcBridge::setNumberOfSentPacketsPerCycle(uint32_t sentPacketsPerCycle_) {
|
||||
this->sentPacketsPerCycle = sentPacketsPerCycle_;
|
||||
}
|
||||
|
||||
ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored) {
|
||||
@@ -144,8 +134,8 @@ ReturnValue_t TmTcBridge::handleTmQueue() {
|
||||
#endif
|
||||
#endif /* FSFW_VERBOSE_LEVEL >= 3 */
|
||||
|
||||
if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) {
|
||||
ReturnValue_t result = storeDownlinkData(&message);
|
||||
if (!communicationLinkUp or packetSentCounter >= sentPacketsPerCycle) {
|
||||
result = storeDownlinkData(&message);
|
||||
if (result != returnvalue::OK) {
|
||||
tmStore->deleteData(message.getStorageId());
|
||||
}
|
||||
|
@@ -15,10 +15,9 @@ class TmTcBridge : public AcceptsTelemetryIF,
|
||||
public ExecutableObjectIF,
|
||||
public SystemObject {
|
||||
public:
|
||||
static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15;
|
||||
static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500;
|
||||
|
||||
static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5;
|
||||
static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 20;
|
||||
static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10;
|
||||
|
||||
TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
|
||||
@@ -32,7 +31,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
|
||||
* @return -@c returnvalue::OK if value was set successfully
|
||||
* -@c returnvalue::FAILED otherwise, stored value stays the same
|
||||
*/
|
||||
ReturnValue_t setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerCycle);
|
||||
void setNumberOfSentPacketsPerCycle(uint32_t sentPacketsPerCycle);
|
||||
|
||||
/**
|
||||
* Set number of packets sent per performOperation().Please note that this
|
||||
@@ -151,7 +150,7 @@ class TmTcBridge : public AcceptsTelemetryIF,
|
||||
* This FIFO can be used to store downlink data which can not be sent at the moment.
|
||||
*/
|
||||
DynamicFIFO<store_address_t>* tmFifo = nullptr;
|
||||
uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;
|
||||
uint32_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE;
|
||||
unsigned int maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED;
|
||||
};
|
||||
|
||||
|
@@ -1,6 +1,8 @@
|
||||
#include <fsfw_hal/linux/serial/helper.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <termios.h>
|
||||
|
||||
#include "FSFWConfig.h"
|
||||
#include "fsfw/serviceinterface.h"
|
||||
|
||||
void serial::setMode(struct termios& options, UartModes mode) {
|
||||
@@ -108,7 +110,7 @@ void serial::setBaudrate(struct termios& options, UartBaudRate baud) {
|
||||
#endif // ! __APPLE__
|
||||
default:
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl;
|
||||
sif::warning << "serial::configureBaudrate: Baudrate not supported" << std::endl;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
@@ -153,15 +155,17 @@ int serial::readCountersAndErrors(int serialPort, serial_icounter_struct& icount
|
||||
}
|
||||
|
||||
void serial::setStopbits(struct termios& options, StopBits bits) {
|
||||
// Regular case: One stop bit.
|
||||
options.c_cflag &= ~CSTOPB;
|
||||
if (bits == StopBits::TWO_STOP_BITS) {
|
||||
// Use two stop bits
|
||||
options.c_cflag |= CSTOPB;
|
||||
} else {
|
||||
// Clear stop field, only one stop bit used in communication
|
||||
options.c_cflag &= ~CSTOPB;
|
||||
}
|
||||
}
|
||||
|
||||
void serial::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); }
|
||||
|
||||
void serial::flushTxBuf(int fd) { tcflush(fd, TCOFLUSH); }
|
||||
|
||||
void serial::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); }
|
||||
|
||||
|
@@ -65,6 +65,7 @@ void setParity(struct termios& options, Parity parity);
|
||||
void ignoreCtrlLines(struct termios& options);
|
||||
|
||||
void flushRxBuf(int fd);
|
||||
void flushTxBuf(int fd);
|
||||
void flushTxRxBuf(int fd);
|
||||
|
||||
int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter);
|
||||
|
@@ -282,7 +282,7 @@ ReturnValue_t SpiComIF::handlePollingSendOperation(uint8_t *recvPtr, SPI_HandleT
|
||||
#endif
|
||||
#endif
|
||||
spiCookie.setTransferState(spi::TransferStates::FAILURE);
|
||||
return spi::HAL_TIMEOUT_RETVAL;
|
||||
return spi::TIMEOUT;
|
||||
}
|
||||
case (HAL_ERROR):
|
||||
default: {
|
||||
@@ -296,7 +296,7 @@ ReturnValue_t SpiComIF::handlePollingSendOperation(uint8_t *recvPtr, SPI_HandleT
|
||||
#endif
|
||||
#endif
|
||||
spiCookie.setTransferState(spi::TransferStates::FAILURE);
|
||||
return spi::HAL_ERROR_RETVAL;
|
||||
return spi::GENERIC_ERROR;
|
||||
}
|
||||
}
|
||||
return returnvalue::OK;
|
||||
|
@@ -9,11 +9,6 @@
|
||||
|
||||
namespace spi {
|
||||
|
||||
static constexpr uint8_t HAL_SPI_ID = CLASS_ID::HAL_SPI;
|
||||
static constexpr ReturnValue_t HAL_TIMEOUT_RETVAL = returnvalue::makeCode(HAL_SPI_ID, 0);
|
||||
static constexpr ReturnValue_t HAL_BUSY_RETVAL = returnvalue::makeCode(HAL_SPI_ID, 1);
|
||||
static constexpr ReturnValue_t HAL_ERROR_RETVAL = returnvalue::makeCode(HAL_SPI_ID, 2);
|
||||
|
||||
enum class TransferStates { IDLE, WAIT, SUCCESS, FAILURE };
|
||||
|
||||
enum SpiBus { SPI_1, SPI_2 };
|
||||
|
@@ -40,10 +40,10 @@ TEST_CASE("Reserved Message Parser", "[cfdp]") {
|
||||
serLen = 0;
|
||||
ReturnValue_t result = creator.serializeBe(buffer.data(), serLen, buffer.size());
|
||||
CHECK(result == returnvalue::OK);
|
||||
arrayprinter::print(buffer.data(), serLen);
|
||||
|
||||
MessageToUserTlv msgToUser;
|
||||
CHECK(msgToUser.deSerializeBe(buffer.data(), serLen, buffer.size()) == OK);
|
||||
ReservedMessageParser parser(ipcStore, msgQueue, destQueueId);
|
||||
REQUIRE(parser.parse(buffer.data(), serLen) == OK);
|
||||
REQUIRE(parser.parse(&msgToUser, 1) == OK);
|
||||
CommandMessage msg;
|
||||
CHECK(msgQueue.wasMessageSent());
|
||||
CHECK(msgQueue.numberOfSentMessages() == 1);
|
||||
|
Reference in New Issue
Block a user