2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/tmtcservices/TmTcBridge.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/globalfunctions/arrayprinter.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/ipc/QueueFactory.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
2021-07-13 20:58:45 +02:00
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2021-03-27 10:40:38 +01:00
|
|
|
#define TMTCBRIDGE_WIRETAPPING 0
|
|
|
|
|
2022-09-14 20:10:41 +02:00
|
|
|
TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination,
|
|
|
|
object_id_t tmStoreId, object_id_t tcStoreId)
|
2022-02-02 10:29:30 +01:00
|
|
|
: SystemObject(objectId),
|
2022-09-14 20:10:41 +02:00
|
|
|
name(name),
|
2022-02-02 10:29:30 +01:00
|
|
|
tmStoreId(tmStoreId),
|
|
|
|
tcStoreId(tcStoreId),
|
|
|
|
tcDestination(tcDestination)
|
2020-08-13 20:53:35 +02:00
|
|
|
|
|
|
|
{
|
2023-02-07 12:45:29 +01:00
|
|
|
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
|
|
|
|
tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(
|
|
|
|
TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs);
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); }
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerCycle) {
|
|
|
|
if (sentPacketsPerCycle <= LIMIT_STORED_DATA_SENT_PER_CYCLE) {
|
|
|
|
this->sentPacketsPerCycle = sentPacketsPerCycle;
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::warning << "TmTcBridge::setNumberOfSentPacketsPerCycle: Number of "
|
|
|
|
<< "packets sent per cycle exceeds limits. "
|
|
|
|
<< "Keeping default value." << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2023-02-07 12:45:29 +01:00
|
|
|
ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored) {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (maxNumberOfPacketsStored <= LIMIT_DOWNLINK_PACKETS_STORED) {
|
|
|
|
this->maxNumberOfPacketsStored = maxNumberOfPacketsStored;
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::warning << "TmTcBridge::setMaxNumberOfPacketsStored: Number of "
|
|
|
|
<< "packets stored exceeds limits. "
|
|
|
|
<< "Keeping default value." << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t TmTcBridge::initialize() {
|
2022-02-02 10:29:30 +01:00
|
|
|
tcStore = ObjectManager::instance()->get<StorageManagerIF>(tcStoreId);
|
|
|
|
if (tcStore == nullptr) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TmTcBridge::initialize: TC store invalid. Make sure"
|
|
|
|
"it is created and set up properly."
|
|
|
|
<< std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
|
|
|
tmStore = ObjectManager::instance()->get<StorageManagerIF>(tmStoreId);
|
|
|
|
if (tmStore == nullptr) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TmTcBridge::initialize: TM store invalid. Make sure"
|
|
|
|
"it is created and set up properly."
|
|
|
|
<< std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-09-14 20:10:41 +02:00
|
|
|
auto* tcDistributor = ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
|
2022-02-02 10:29:30 +01:00
|
|
|
if (tcDistributor == nullptr) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TmTcBridge::initialize: TC Distributor invalid" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
tmFifo = new DynamicFIFO<store_address_t>(maxNumberOfPacketsStored);
|
2020-09-06 15:48:30 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
tmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t TmTcBridge::performOperation(uint8_t operationCode) {
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t result;
|
|
|
|
result = handleTc();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "TmTcBridge::performOperation: "
|
|
|
|
<< "Error handling TCs" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
result = handleTm();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::debug << "TmTcBridge::performOperation: "
|
|
|
|
<< "Error handling TMs" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return result;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t TmTcBridge::handleTc() { return returnvalue::OK; }
|
2020-08-13 20:53:35 +02:00
|
|
|
|
|
|
|
ReturnValue_t TmTcBridge::handleTm() {
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t status = returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t result = handleTmQueue();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TmTcBridge::handleTm: Error handling TM queue with error code 0x" << std::hex
|
|
|
|
<< result << std::dec << "!" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (tmStored and communicationLinkUp and (packetSentCounter < sentPacketsPerCycle)) {
|
|
|
|
result = handleStoredTm();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TmTcBridge::handleTm: Error handling stored TMs!" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
packetSentCounter = 0;
|
|
|
|
return status;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t TmTcBridge::handleTmQueue() {
|
2022-02-02 10:29:30 +01:00
|
|
|
TmTcMessage message;
|
|
|
|
const uint8_t* data = nullptr;
|
|
|
|
size_t size = 0;
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t status = returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
for (ReturnValue_t result = tmTcReceptionQueue->receiveMessage(&message);
|
2022-08-16 01:08:26 +02:00
|
|
|
result == returnvalue::OK; result = tmTcReceptionQueue->receiveMessage(&message)) {
|
2021-01-29 17:16:34 +01:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 3
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::info << "Sent packet counter: " << static_cast<int>(packetSentCounter) << std::endl;
|
2021-01-29 17:16:34 +01:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printInfo("Sent packet counter: %d\n", packetSentCounter);
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2021-01-29 17:16:34 +01:00
|
|
|
#endif /* FSFW_VERBOSE_LEVEL >= 3 */
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) {
|
2023-02-08 20:38:32 +01:00
|
|
|
ReturnValue_t result = storeDownlinkData(&message);
|
|
|
|
if (result != returnvalue::OK) {
|
|
|
|
tmStore->deleteData(message.getStorageId());
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
result = tmStore->getData(message.getStorageId(), &data, &size);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
2023-02-08 20:38:32 +01:00
|
|
|
tmStore->deleteData(message.getStorageId());
|
2022-02-02 10:29:30 +01:00
|
|
|
continue;
|
|
|
|
}
|
|
|
|
|
|
|
|
result = sendTm(data, size);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
|
|
|
} else {
|
|
|
|
packetSentCounter++;
|
|
|
|
}
|
2023-02-08 20:38:32 +01:00
|
|
|
tmStore->deleteData(message.getStorageId());
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
return status;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) {
|
2022-07-25 11:15:45 +02:00
|
|
|
store_address_t storeId;
|
2022-02-02 10:29:30 +01:00
|
|
|
if (tmFifo == nullptr) {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
if (tmFifo->full()) {
|
2023-02-07 12:45:29 +01:00
|
|
|
if (warningSwitch) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2023-02-07 12:45:29 +01:00
|
|
|
sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number "
|
|
|
|
"of stored packet IDs reached!"
|
|
|
|
<< std::endl;
|
2021-05-05 15:59:41 +02:00
|
|
|
#else
|
2023-02-07 12:45:29 +01:00
|
|
|
sif::printWarning(
|
|
|
|
"TmTcBridge::storeDownlinkData: TM downlink max. number "
|
|
|
|
"of stored packet IDs reached!\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2023-02-07 12:45:29 +01:00
|
|
|
warningSwitch = false;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
if (overwriteOld) {
|
|
|
|
tmFifo->retrieve(&storeId);
|
|
|
|
tmStore->deleteData(storeId);
|
|
|
|
} else {
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
storeId = message->getStorageId();
|
|
|
|
tmFifo->insert(storeId);
|
|
|
|
tmStored = true;
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t TmTcBridge::handleStoredTm() {
|
2022-08-16 01:08:26 +02:00
|
|
|
ReturnValue_t status = returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
while (not tmFifo->empty() and packetSentCounter < sentPacketsPerCycle) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
// sif::info << "TMTC Bridge: Sending stored TM data. There are "
|
|
|
|
// << (int) tmFifo->size() << " left to send\r\n" << std::flush;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2020-09-06 15:48:30 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
store_address_t storeId;
|
|
|
|
const uint8_t* data = nullptr;
|
|
|
|
size_t size = 0;
|
|
|
|
tmFifo->retrieve(&storeId);
|
|
|
|
ReturnValue_t result = tmStore->getData(storeId, &data, &size);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
|
|
|
}
|
|
|
|
|
|
|
|
result = sendTm(data, size);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "TMTC Bridge: Could not send stored downlink data" << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
status = result;
|
|
|
|
}
|
|
|
|
packetSentCounter++;
|
|
|
|
|
|
|
|
if (tmFifo->empty()) {
|
2023-02-07 12:45:29 +01:00
|
|
|
warningSwitch = true;
|
2022-02-02 10:29:30 +01:00
|
|
|
tmStored = false;
|
|
|
|
}
|
|
|
|
tmStore->deleteData(storeId);
|
|
|
|
}
|
|
|
|
return status;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void TmTcBridge::registerCommConnect() {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (not communicationLinkUp) {
|
|
|
|
communicationLinkUp = true;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
void TmTcBridge::registerCommDisconnect() {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (communicationLinkUp) {
|
|
|
|
communicationLinkUp = false;
|
|
|
|
}
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-11-15 11:40:26 +01:00
|
|
|
MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) const {
|
2022-02-02 10:29:30 +01:00
|
|
|
return tmTcReceptionQueue->getId();
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void TmTcBridge::printData(uint8_t* data, size_t dataLen) { arrayprinter::print(data, dataLen); }
|
2020-08-13 20:53:35 +02:00
|
|
|
|
2022-09-14 20:10:41 +02:00
|
|
|
uint32_t TmTcBridge::getIdentifier() const {
|
2022-02-02 10:29:30 +01:00
|
|
|
// This is no PUS service, so we just return 0
|
|
|
|
return 0;
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
|
|
|
|
2022-09-14 20:10:41 +02:00
|
|
|
MessageQueueId_t TmTcBridge::getRequestQueue() const {
|
2022-02-02 10:29:30 +01:00
|
|
|
// Default implementation: Relay TC messages to TC distributor directly.
|
|
|
|
return tmTcReceptionQueue->getDefaultDestination();
|
2020-08-13 20:53:35 +02:00
|
|
|
}
|
2020-09-06 15:48:30 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
void TmTcBridge::setFifoToOverwriteOldData(bool overwriteOld) { this->overwriteOld = overwriteOld; }
|
2022-09-14 20:10:41 +02:00
|
|
|
|
|
|
|
const char* TmTcBridge::getName() const { return name; }
|