fsfw/tmtcservices/TmTcBridge.cpp

193 lines
5.5 KiB
C++
Raw Normal View History

2019-12-26 19:47:46 +01:00
#include <framework/tmtcservices/TmTcBridge.h>
#include <framework/ipc/QueueFactory.h>
#include <framework/tmtcservices/AcceptsTelecommandsIF.h>
#include <framework/serviceinterface/ServiceInterfaceStream.h>
2020-06-23 10:45:47 +02:00
#include <framework/globalfunctions/arrayprinter.h>
2019-12-26 19:47:46 +01:00
TmTcBridge::TmTcBridge(object_id_t objectId_,
object_id_t ccsdsPacketDistributor_): SystemObject(objectId_),
2020-04-23 10:20:19 +02:00
ccsdsPacketDistributor(ccsdsPacketDistributor_)
{
TmTcReceptionQueue = QueueFactory::instance()->
createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH);
2019-12-26 19:47:46 +01:00
}
TmTcBridge::~TmTcBridge() {}
2019-12-26 19:47:46 +01:00
ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(
uint8_t sentPacketsPerCycle) {
2020-04-23 10:20:19 +02:00
if(sentPacketsPerCycle <= LIMIT_STORED_DATA_SENT_PER_CYCLE) {
this->sentPacketsPerCycle = sentPacketsPerCycle;
return RETURN_OK;
}
else {
2020-06-23 10:45:47 +02:00
sif::warning << "TmTcBridge: Number of packets sent per cycle "
2020-04-23 10:24:34 +02:00
"exceeds limits. Keeping default value." << std::endl;
return RETURN_FAILED;
}
}
2020-04-23 10:20:19 +02:00
ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(
uint8_t maxNumberOfPacketsStored) {
if(maxNumberOfPacketsStored <= LIMIT_DOWNLINK_PACKETS_STORED) {
this->maxNumberOfPacketsStored = maxNumberOfPacketsStored;
return RETURN_OK;
}
else {
2020-06-23 10:45:47 +02:00
sif::warning << "TmTcBridge: Number of packets stored "
2020-04-23 10:24:34 +02:00
"exceeds limits. Keeping default value." << std::endl;
2020-04-23 10:20:19 +02:00
return RETURN_FAILED;
}
}
2019-12-26 19:47:46 +01:00
ReturnValue_t TmTcBridge::initialize() {
tcStore = objectManager->get<StorageManagerIF>(objects::TC_STORE);
if (tcStore == NULL) {
return RETURN_FAILED;
}
tmStore = objectManager->get<StorageManagerIF>(objects::TM_STORE);
if (tmStore == NULL) {
return RETURN_FAILED;
}
AcceptsTelecommandsIF* tcDistributor =
objectManager->get<AcceptsTelecommandsIF>(ccsdsPacketDistributor);
if (tcDistributor == NULL) {
return RETURN_FAILED;
}
TmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue());
return RETURN_OK;
}
ReturnValue_t TmTcBridge::performOperation(uint8_t operationCode) {
ReturnValue_t result;
result = handleTc();
if(result != RETURN_OK) {
2020-06-23 10:45:47 +02:00
sif::error << "TMTC Bridge: Error handling TCs" << std::endl;
2019-12-26 19:47:46 +01:00
}
result = handleTm();
if (result != RETURN_OK) {
2020-06-23 10:45:47 +02:00
sif::error << "TMTC Bridge: Error handling TMs" << std::endl;
2019-12-26 19:47:46 +01:00
}
return result;
}
ReturnValue_t TmTcBridge::handleTc() {
uint8_t * recvBuffer = nullptr;
size_t recvLen = 0;
ReturnValue_t result = receiveTc(&recvBuffer, &recvLen);
2019-12-26 19:47:46 +01:00
return result;
}
ReturnValue_t TmTcBridge::handleTm() {
2020-04-23 10:32:05 +02:00
ReturnValue_t result = handleTmQueue();
2019-12-26 19:47:46 +01:00
if(result != RETURN_OK) {
2020-06-23 10:45:47 +02:00
sif::error << "TMTC Bridge: Reading TM Queue failed" << std::endl;
2019-12-26 19:47:46 +01:00
return RETURN_FAILED;
}
2020-06-23 10:45:47 +02:00
if(tmStored and communicationLinkUp) {
2020-04-23 10:20:19 +02:00
result = handleStoredTm();
2019-12-26 19:47:46 +01:00
}
return result;
}
2020-04-23 10:32:05 +02:00
ReturnValue_t TmTcBridge::handleTmQueue() {
2019-12-26 19:47:46 +01:00
TmTcMessage message;
2020-06-23 10:45:47 +02:00
const uint8_t* data = nullptr;
size_t size = 0;
2019-12-26 19:47:46 +01:00
for (ReturnValue_t result = TmTcReceptionQueue->receiveMessage(&message);
result == RETURN_OK; result = TmTcReceptionQueue->receiveMessage(&message))
{
2020-01-02 21:12:21 +01:00
if(communicationLinkUp == false) {
2019-12-26 19:47:46 +01:00
result = storeDownlinkData(&message);
return result;
}
result = tmStore->getData(message.getStorageId(), &data, &size);
if (result != HasReturnvaluesIF::RETURN_OK) {
continue;
}
result = sendTm(data, size);
if (result != RETURN_OK) {
2020-06-23 10:45:47 +02:00
sif::error << "TMTC Bridge: Could not send TM packet"<< std::endl;
2019-12-26 19:47:46 +01:00
tmStore->deleteData(message.getStorageId());
return result;
}
tmStore->deleteData(message.getStorageId());
}
return RETURN_OK;
}
ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) {
//debug << "TMTC Bridge: Comm Link down. "
// "Saving packet ID to be sent later\r\n" << std::flush;
store_address_t storeId = 0;
2019-12-26 19:47:46 +01:00
2020-06-23 10:45:47 +02:00
if(tmFifo.full()) {
sif::error << "TMTC Bridge: TM downlink max. number of stored packet IDs "
"reached! Overwriting old data" << std::endl;
2020-06-23 10:45:47 +02:00
tmFifo.retrieve(&storeId);
tmStore->deleteData(storeId);
2019-12-26 19:47:46 +01:00
}
storeId = message->getStorageId();
2020-06-23 10:45:47 +02:00
tmFifo.insert(storeId);
2019-12-26 19:47:46 +01:00
tmStored = true;
return RETURN_OK;
}
2020-04-23 10:20:19 +02:00
ReturnValue_t TmTcBridge::handleStoredTm() {
2019-12-26 19:47:46 +01:00
uint8_t counter = 0;
ReturnValue_t result = RETURN_OK;
2020-06-23 10:45:47 +02:00
while(not tmFifo.empty() and counter < sentPacketsPerCycle) {
//info << "TMTC Bridge: Sending stored TM data. There are "
// << (int) fifo.size() << " left to send\r\n" << std::flush;
2019-12-26 19:47:46 +01:00
store_address_t storeId;
2020-06-23 10:45:47 +02:00
const uint8_t* data = nullptr;
size_t size = 0;
tmFifo.retrieve(&storeId);
2019-12-26 19:47:46 +01:00
result = tmStore->getData(storeId, &data, &size);
2020-04-22 18:21:50 +02:00
2019-12-26 19:47:46 +01:00
sendTm(data,size);
2020-04-22 18:21:50 +02:00
2019-12-26 19:47:46 +01:00
if(result != RETURN_OK) {
2020-06-23 10:45:47 +02:00
sif::error << "TMTC Bridge: Could not send stored downlink data"
2020-04-18 13:35:41 +02:00
<< std::endl;
2019-12-26 19:47:46 +01:00
result = RETURN_FAILED;
}
counter ++;
2020-06-23 10:45:47 +02:00
if(tmFifo.empty()) {
2019-12-26 19:47:46 +01:00
tmStored = false;
}
tmStore->deleteData(storeId);
}
return result;
}
void TmTcBridge::registerCommConnect() {
2020-04-23 10:24:34 +02:00
if(not communicationLinkUp) {
2020-04-23 10:22:25 +02:00
//info << "TMTC Bridge: Registered Comm Link Connect" << std::endl;
2019-12-26 19:47:46 +01:00
communicationLinkUp = true;
}
}
void TmTcBridge::registerCommDisconnect() {
2020-04-23 10:22:25 +02:00
//info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl;
2019-12-26 19:47:46 +01:00
if(communicationLinkUp) {
communicationLinkUp = false;
}
}
MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) {
return TmTcReceptionQueue->getId();
}
2020-04-23 10:20:19 +02:00
void TmTcBridge::printData(uint8_t * data, size_t dataLen) {
2020-06-23 10:45:47 +02:00
arrayprinter::print(data, dataLen);
2019-12-26 19:47:46 +01:00
}