2022-07-19 18:13:25 +02:00
|
|
|
#include "fsfw/tcdistribution/PusDistributor.h"
|
2020-10-01 13:23:06 +02:00
|
|
|
|
2022-07-27 10:49:49 +02:00
|
|
|
#include "definitions.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2022-07-29 12:03:14 +02:00
|
|
|
#include "fsfw/tcdistribution/CcsdsDistributorIF.h"
|
2021-07-13 20:22:54 +02:00
|
|
|
#include "fsfw/tmtcservices/PusVerificationReport.h"
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-27 17:13:42 +02:00
|
|
|
#define PUS_DISTRIBUTOR_DEBUGGING 0
|
2021-04-07 12:03:28 +02:00
|
|
|
|
2022-07-27 11:26:47 +02:00
|
|
|
PusDistributor::PusDistributor(uint16_t setApid, object_id_t setObjectId,
|
2022-07-29 12:03:14 +02:00
|
|
|
CcsdsDistributorIF* distributor, StorageManagerIF* store_)
|
2022-08-01 11:00:36 +02:00
|
|
|
: TcDistributorBase(setObjectId),
|
2022-07-19 18:13:25 +02:00
|
|
|
store(store_),
|
|
|
|
checker(setApid, ccsds::PacketType::TC),
|
2022-07-27 14:40:51 +02:00
|
|
|
ccsdsDistributor(distributor),
|
2022-08-16 01:08:26 +02:00
|
|
|
tcStatus(returnvalue::FAILED) {}
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
PusDistributor::~PusDistributor() = default;
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
ReturnValue_t PusDistributor::selectDestination(MessageQueueId_t& destId) {
|
2021-04-07 12:03:28 +02:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1 && PUS_DISTRIBUTOR_DEBUGGING == 1
|
2022-07-27 17:00:43 +02:00
|
|
|
store_address_t storeId = currentMessage.getStorageId();
|
|
|
|
sif::debug << "PUSDistributor::handlePacket received: " << storeId.poolIndex << ", "
|
|
|
|
<< storeId.packetIndex << std::endl;
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-07-27 17:00:43 +02:00
|
|
|
// TODO: Need to set the data
|
|
|
|
const uint8_t* packetPtr = nullptr;
|
|
|
|
size_t packetLen = 0;
|
2022-08-22 16:35:53 +02:00
|
|
|
ReturnValue_t result =
|
|
|
|
store->getData(currentMessage.getStorageId(), &packetPtr, &packetLen) != returnvalue::OK;
|
2022-08-22 16:21:41 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-08-01 11:00:36 +02:00
|
|
|
tcStatus = PACKET_LOST;
|
|
|
|
return result;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
result = reader.setReadOnlyData(packetPtr, packetLen);
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
tcStatus = PACKET_LOST;
|
2022-08-01 11:00:36 +02:00
|
|
|
return result;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
|
|
|
// CRC check done by checker
|
|
|
|
result = reader.parseDataWithoutCrcCheck();
|
2022-08-16 01:08:26 +02:00
|
|
|
if (result != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
tcStatus = PACKET_LOST;
|
2022-08-01 11:00:36 +02:00
|
|
|
return result;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
|
2022-07-27 17:00:43 +02:00
|
|
|
if (reader.getFullData() != nullptr) {
|
|
|
|
tcStatus = checker.checkPacket(reader, reader.getFullPacketLen());
|
2022-08-16 01:08:26 +02:00
|
|
|
if (tcStatus != returnvalue::OK) {
|
2022-07-27 17:00:43 +02:00
|
|
|
checkerFailurePrinter();
|
2020-10-01 13:23:06 +02:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
uint8_t pusId = reader.getService();
|
|
|
|
auto iter = receiverMap.find(pusId);
|
|
|
|
if (iter == receiverMap.end()) {
|
|
|
|
tcStatus = DESTINATION_NOT_FOUND;
|
2021-04-07 12:03:28 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-08-01 11:00:36 +02:00
|
|
|
sif::debug << "PUSDistributor::handlePacket: Destination not found" << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-08-01 11:00:36 +02:00
|
|
|
sif::printDebug("PUSDistributor::handlePacket: Destination not found\n");
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif /* !FSFW_CPP_OSTREAM_ENABLED == 1 */
|
2020-10-01 13:23:06 +02:00
|
|
|
#endif
|
2022-08-01 11:00:36 +02:00
|
|
|
}
|
|
|
|
destId = iter->second.destId;
|
2022-07-27 17:00:43 +02:00
|
|
|
} else {
|
2022-08-01 11:00:36 +02:00
|
|
|
tcStatus = PACKET_LOST;
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
return tcStatus;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
ReturnValue_t PusDistributor::registerService(const AcceptsTelecommandsIF& service) {
|
|
|
|
uint16_t serviceId = service.getIdentifier();
|
2021-04-07 12:03:28 +02:00
|
|
|
#if PUS_DISTRIBUTOR_DEBUGGING == 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::info << "Service ID: " << static_cast<int>(serviceId) << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printInfo("Service ID: %d\n", static_cast<int>(serviceId));
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-08-01 11:00:36 +02:00
|
|
|
MessageQueueId_t queue = service.getRequestQueue();
|
|
|
|
auto returnPair = receiverMap.emplace(serviceId, ServiceInfo(service.getName(), queue));
|
2022-02-02 10:29:30 +01:00
|
|
|
if (not returnPair.second) {
|
2021-04-07 12:03:28 +02:00
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "PUSDistributor::registerService: Service ID already"
|
|
|
|
" exists in map"
|
|
|
|
<< std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("PUSDistributor::registerService: Service ID already exists in map\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2021-04-07 12:03:28 +02:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
return SERVICE_ID_ALREADY_EXISTS;
|
|
|
|
}
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::OK;
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
MessageQueueId_t PusDistributor::getRequestQueue() const { return tcQueue->getId(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
ReturnValue_t PusDistributor::callbackAfterSending(ReturnValue_t queueStatus) {
|
2022-08-16 01:08:26 +02:00
|
|
|
if (queueStatus != returnvalue::OK) {
|
2022-02-02 10:29:30 +01:00
|
|
|
tcStatus = queueStatus;
|
|
|
|
}
|
2022-08-22 16:23:36 +02:00
|
|
|
if (tcStatus != returnvalue::OK) {
|
2022-08-18 11:19:42 +02:00
|
|
|
verifyChannel->sendFailureReport({tcverif::ACCEPTANCE_FAILURE, reader, tcStatus});
|
2022-02-02 10:29:30 +01:00
|
|
|
// A failed packet is deleted immediately after reporting,
|
|
|
|
// otherwise it will block memory.
|
2022-07-19 18:13:25 +02:00
|
|
|
store->deleteData(currentMessage.getStorageId());
|
2022-08-16 01:08:26 +02:00
|
|
|
return returnvalue::FAILED;
|
2022-02-02 10:29:30 +01:00
|
|
|
} else {
|
2022-08-18 11:19:42 +02:00
|
|
|
verifyChannel->sendSuccessReport({tcverif::ACCEPTANCE_SUCCESS, reader});
|
2022-08-22 16:23:36 +02:00
|
|
|
return returnvalue::OK;
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
|
|
|
|
2022-08-01 11:00:36 +02:00
|
|
|
uint32_t PusDistributor::getIdentifier() const { return checker.getApid(); }
|
2016-06-15 23:48:41 +02:00
|
|
|
|
2022-07-19 18:13:25 +02:00
|
|
|
ReturnValue_t PusDistributor::initialize() {
|
2022-07-27 14:40:51 +02:00
|
|
|
if (store == nullptr) {
|
|
|
|
store = ObjectManager::instance()->get<StorageManagerIF>(objects::TC_STORE);
|
|
|
|
if (store == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2022-07-27 17:00:43 +02:00
|
|
|
if (ccsdsDistributor == 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 << "PUSDistributor::initialize: Packet source invalid" << std::endl;
|
|
|
|
sif::error << " Make sure it exists and implements CCSDSDistributorIF!" << std::endl;
|
2021-04-07 12:03:28 +02:00
|
|
|
#else
|
2022-07-19 18:13:25 +02:00
|
|
|
sif::printError("PusDistributor::initialize: Packet source invalid\n");
|
2022-07-29 12:03:14 +02:00
|
|
|
sif::printError("Make sure it exists and implements CcsdsDistributorIF\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-07-27 14:40:51 +02:00
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
2022-07-27 11:26:47 +02:00
|
|
|
}
|
2022-07-27 17:00:43 +02:00
|
|
|
if (verifyChannel == nullptr) {
|
2022-08-17 11:39:15 +02:00
|
|
|
verifyChannel =
|
|
|
|
ObjectManager::instance()->get<VerificationReporterIF>(objects::VERIFICATION_REPORTER);
|
2022-07-27 17:00:43 +02:00
|
|
|
if (verifyChannel == nullptr) {
|
|
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
|
|
|
}
|
|
|
|
}
|
2022-08-01 17:16:37 +02:00
|
|
|
return ccsdsDistributor->registerApplication(CcsdsDistributorIF::DestInfo(*this, false));
|
2016-06-15 23:48:41 +02:00
|
|
|
}
|
2022-07-27 17:00:43 +02:00
|
|
|
|
|
|
|
void PusDistributor::checkerFailurePrinter() const {
|
|
|
|
#if FSFW_VERBOSE_LEVEL >= 1
|
2022-08-01 11:00:36 +02:00
|
|
|
const char* reason = "Unknown reason";
|
2022-08-08 12:31:19 +02:00
|
|
|
if (tcStatus == tmtcdistrib::INCORRECT_CHECKSUM) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Checksum Error";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (tcStatus == tmtcdistrib::INCORRECT_PRIMARY_HEADER) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Incorrect Primary Header";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (tcStatus == tmtcdistrib::INVALID_APID) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Illegal APID";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (tcStatus == tmtcdistrib::INCORRECT_SECONDARY_HEADER) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Incorrect Secondary Header";
|
2022-08-03 15:12:29 +02:00
|
|
|
} else if (tcStatus == tmtcdistrib::INCOMPLETE_PACKET) {
|
2022-08-01 11:00:36 +02:00
|
|
|
reason = "Incomplete packet";
|
2022-07-27 17:00:43 +02:00
|
|
|
}
|
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-08-01 11:00:36 +02:00
|
|
|
sif::warning << "PUSDistributor::handlePacket: Check failed: " << reason << std::endl;
|
2022-07-27 17:00:43 +02:00
|
|
|
#else
|
2022-08-01 11:00:36 +02:00
|
|
|
sif::printWarning("PUSDistributor::handlePacket: Check failed: %s\n", reason);
|
2022-07-27 17:00:43 +02:00
|
|
|
#endif
|
|
|
|
#endif
|
|
|
|
}
|
2022-08-01 11:00:36 +02:00
|
|
|
|
|
|
|
const char* PusDistributor::getName() const { return "PUS Distributor"; }
|