refactored TM funnels to allow multiple TM recipients
This commit is contained in:
351
mission/tmtc/CcsdsIpCoreHandler.cpp
Normal file
351
mission/tmtc/CcsdsIpCoreHandler.cpp
Normal file
@ -0,0 +1,351 @@
|
||||
#include "CcsdsIpCoreHandler.h"
|
||||
|
||||
#include <linux/ipcore/PdecHandler.h>
|
||||
#include <linux/ipcore/PtmeConfig.h>
|
||||
|
||||
#include "fsfw/events/EventManagerIF.h"
|
||||
#include "fsfw/ipc/QueueFactory.h"
|
||||
#include "fsfw/objectmanager/ObjectManager.h"
|
||||
#include "fsfw/serialize/SerializeAdapter.h"
|
||||
#include "fsfw/serviceinterface/ServiceInterface.h"
|
||||
#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
|
||||
#include "mission/devices/devicedefinitions/SyrlinksDefinitions.h"
|
||||
|
||||
CcsdsIpCoreHandler::CcsdsIpCoreHandler(object_id_t objectId, object_id_t ptmeId,
|
||||
object_id_t tcDestination, PtmeConfig* ptmeConfig,
|
||||
GpioIF* gpioIF, gpioId_t enTxClock, gpioId_t enTxData,
|
||||
uint32_t transmitterTimeout)
|
||||
: SystemObject(objectId),
|
||||
ptmeId(ptmeId),
|
||||
tcDestination(tcDestination),
|
||||
parameterHelper(this),
|
||||
actionHelper(this, nullptr),
|
||||
ptmeConfig(ptmeConfig),
|
||||
gpioIF(gpioIF),
|
||||
enTxClock(enTxClock),
|
||||
enTxData(enTxData),
|
||||
transmitterTimeout(transmitterTimeout) {
|
||||
commandQueue = QueueFactory::instance()->createMessageQueue(QUEUE_SIZE);
|
||||
auto mqArgs = MqArgs(objectId, static_cast<void*>(this));
|
||||
eventQueue =
|
||||
QueueFactory::instance()->createMessageQueue(10, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs);
|
||||
}
|
||||
|
||||
CcsdsIpCoreHandler::~CcsdsIpCoreHandler() {}
|
||||
|
||||
ReturnValue_t CcsdsIpCoreHandler::performOperation(uint8_t operationCode) {
|
||||
checkEvents();
|
||||
readCommandQueue();
|
||||
handleTelemetry();
|
||||
handleTelecommands();
|
||||
checkTxTimer();
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleTelemetry() {
|
||||
VirtualChannelMapIter iter;
|
||||
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
||||
iter->second->performOperation();
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleTelecommands() {}
|
||||
|
||||
ReturnValue_t CcsdsIpCoreHandler::initialize() {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
PtmeIF* ptme = ObjectManager::instance()->get<PtmeIF>(ptmeId);
|
||||
if (ptme == nullptr) {
|
||||
sif::warning << "Invalid PTME object" << std::endl;
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
AcceptsTelecommandsIF* tcDistributor =
|
||||
ObjectManager::instance()->get<AcceptsTelecommandsIF>(tcDestination);
|
||||
if (tcDistributor == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CcsdsHandler::initialize: Invalid TC Distributor object" << std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
tcDistributorQueueId = tcDistributor->getRequestQueue();
|
||||
|
||||
result = parameterHelper.initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
result = actionHelper.initialize(commandQueue);
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
|
||||
VirtualChannelMapIter iter;
|
||||
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
||||
result = iter->second->initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
iter->second->setPtmeObject(ptme);
|
||||
}
|
||||
|
||||
EventManagerIF* manager = ObjectManager::instance()->get<EventManagerIF>(objects::EVENT_MANAGER);
|
||||
if (manager == nullptr) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CcsdsHandler::initialize: Invalid event manager" << std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
result = manager->registerListener(eventQueue->getId());
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::warning << "CcsdsHandler::initialize: Failed to register CCSDS handler as event "
|
||||
"listener"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
;
|
||||
}
|
||||
result = manager->subscribeToEventRange(eventQueue->getId(),
|
||||
event::getEventId(PdecHandler::CARRIER_LOCK),
|
||||
event::getEventId(PdecHandler::BIT_LOCK_PDEC));
|
||||
if (result != returnvalue::OK) {
|
||||
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||||
sif::error << "CcsdsHandler::initialize: Failed to subscribe to events from PDEC "
|
||||
"handler"
|
||||
<< std::endl;
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
result = ptmeConfig->initialize();
|
||||
if (result != returnvalue::OK) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
if (result != returnvalue::OK) {
|
||||
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||||
}
|
||||
|
||||
#if OBSW_SYRLINKS_SIMULATED == 1
|
||||
// Update data on rising edge
|
||||
ptmeConfig->invertTxClock(false);
|
||||
transmitterCountdown.setTimeout(transmitterTimeout);
|
||||
linkState = UP;
|
||||
forwardLinkstate();
|
||||
#endif /* OBSW_SYRLINKS_SIMULATED == 1*/
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::readCommandQueue(void) {
|
||||
CommandMessage commandMessage;
|
||||
ReturnValue_t result = returnvalue::FAILED;
|
||||
|
||||
result = commandQueue->receiveMessage(&commandMessage);
|
||||
if (result == returnvalue::OK) {
|
||||
result = parameterHelper.handleParameterMessage(&commandMessage);
|
||||
if (result == returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
result = actionHelper.handleActionMessage(&commandMessage);
|
||||
if (result == returnvalue::OK) {
|
||||
return;
|
||||
}
|
||||
CommandMessage reply;
|
||||
reply.setReplyRejected(CommandMessage::UNKNOWN_COMMAND, commandMessage.getCommand());
|
||||
commandQueue->reply(&reply);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
MessageQueueId_t CcsdsIpCoreHandler::getCommandQueue() const { return commandQueue->getId(); }
|
||||
|
||||
void CcsdsIpCoreHandler::addVirtualChannel(VcId_t vcId, VirtualChannel* virtualChannel) {
|
||||
if (vcId > common::NUMBER_OF_VIRTUAL_CHANNELS) {
|
||||
sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel ID" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
if (virtualChannel == nullptr) {
|
||||
sif::warning << "CcsdsHandler::addVirtualChannel: Invalid virtual channel interface"
|
||||
<< std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
auto status = virtualChannelMap.emplace(vcId, virtualChannel);
|
||||
if (status.second == false) {
|
||||
sif::warning << "CcsdsHandler::addVirtualChannel: Failed to add virtual channel to "
|
||||
"virtual channel map"
|
||||
<< std::endl;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
MessageQueueId_t CcsdsIpCoreHandler::getReportReceptionQueue(uint8_t virtualChannel) const {
|
||||
if (virtualChannel < common::NUMBER_OF_VIRTUAL_CHANNELS) {
|
||||
auto iter = virtualChannelMap.find(virtualChannel);
|
||||
if (iter != virtualChannelMap.end()) {
|
||||
return iter->second->getReportReceptionQueue();
|
||||
} else {
|
||||
sif::warning << "CcsdsHandler::getReportReceptionQueue: Virtual channel with ID "
|
||||
<< static_cast<unsigned int>(virtualChannel) << " not in virtual channel map"
|
||||
<< std::endl;
|
||||
return MessageQueueIF::NO_QUEUE;
|
||||
}
|
||||
} else {
|
||||
sif::debug << "CcsdsHandler::getReportReceptionQueue: Invalid virtual channel requested";
|
||||
}
|
||||
return MessageQueueIF::NO_QUEUE;
|
||||
}
|
||||
|
||||
ReturnValue_t CcsdsIpCoreHandler::getParameter(uint8_t domainId, uint8_t uniqueIdentifier,
|
||||
ParameterWrapper* parameterWrapper,
|
||||
const ParameterWrapper* newValues,
|
||||
uint16_t startAtIndex) {
|
||||
return returnvalue::OK;
|
||||
}
|
||||
|
||||
uint32_t CcsdsIpCoreHandler::getIdentifier() const { return 0; }
|
||||
|
||||
MessageQueueId_t CcsdsIpCoreHandler::getRequestQueue() const {
|
||||
// Forward packets directly to TC distributor
|
||||
return tcDistributorQueueId;
|
||||
}
|
||||
|
||||
ReturnValue_t CcsdsIpCoreHandler::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
|
||||
const uint8_t* data, size_t size) {
|
||||
ReturnValue_t result = returnvalue::OK;
|
||||
switch (actionId) {
|
||||
case SET_LOW_RATE: {
|
||||
result = ptmeConfig->setRate(RATE_100KBPS);
|
||||
break;
|
||||
}
|
||||
case SET_HIGH_RATE: {
|
||||
result = ptmeConfig->setRate(RATE_500KBPS);
|
||||
break;
|
||||
}
|
||||
case ARBITRARY_RATE: {
|
||||
uint32_t bitrate = 0;
|
||||
SerializeAdapter::deSerialize(&bitrate, &data, &size, SerializeIF::Endianness::BIG);
|
||||
result = ptmeConfig->setRate(bitrate);
|
||||
break;
|
||||
}
|
||||
case EN_TRANSMITTER: {
|
||||
enableTransmit();
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
case DISABLE_TRANSMITTER: {
|
||||
disableTransmit();
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
case ENABLE_TX_CLK_MANIPULATOR: {
|
||||
result = ptmeConfig->configTxManipulator(true);
|
||||
break;
|
||||
}
|
||||
case DISABLE_TX_CLK_MANIPULATOR: {
|
||||
result = ptmeConfig->configTxManipulator(false);
|
||||
break;
|
||||
}
|
||||
case UPDATE_ON_RISING_EDGE: {
|
||||
result = ptmeConfig->invertTxClock(false);
|
||||
break;
|
||||
}
|
||||
case UPDATE_ON_FALLING_EDGE: {
|
||||
result = ptmeConfig->invertTxClock(true);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return COMMAND_NOT_IMPLEMENTED;
|
||||
}
|
||||
if (result != returnvalue::OK) {
|
||||
return result;
|
||||
}
|
||||
return EXECUTION_FINISHED;
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::checkEvents() {
|
||||
EventMessage event;
|
||||
for (ReturnValue_t result = eventQueue->receiveMessage(&event); result == returnvalue::OK;
|
||||
result = eventQueue->receiveMessage(&event)) {
|
||||
switch (event.getMessageId()) {
|
||||
case EventMessage::EVENT_MESSAGE:
|
||||
handleEvent(&event);
|
||||
break;
|
||||
default:
|
||||
sif::debug << "CcsdsHandler::checkEvents: Did not subscribe to this event message"
|
||||
<< std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleEvent(EventMessage* eventMessage) {
|
||||
Event event = eventMessage->getEvent();
|
||||
switch (event) {
|
||||
case PdecHandler::BIT_LOCK_PDEC: {
|
||||
handleBitLockEvent();
|
||||
break;
|
||||
}
|
||||
case PdecHandler::CARRIER_LOCK: {
|
||||
handleCarrierLockEvent();
|
||||
break;
|
||||
}
|
||||
default:
|
||||
sif::debug << "CcsdsHandler::handleEvent: Did not subscribe to this event" << std::endl;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleBitLockEvent() {
|
||||
if (transmitterCountdown.isBusy()) {
|
||||
// Transmitter already enabled
|
||||
return;
|
||||
}
|
||||
enableTransmit();
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::handleCarrierLockEvent() {
|
||||
if (!enableTxWhenCarrierLock) {
|
||||
return;
|
||||
}
|
||||
enableTransmit();
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::forwardLinkstate() {
|
||||
VirtualChannelMapIter iter;
|
||||
for (iter = virtualChannelMap.begin(); iter != virtualChannelMap.end(); iter++) {
|
||||
iter->second->setLinkState(linkState);
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::enableTransmit() {
|
||||
if (transmitterCountdown.isBusy()) {
|
||||
// Transmitter already enabled
|
||||
return;
|
||||
}
|
||||
#ifndef TE0720_1CFA
|
||||
gpioIF->pullHigh(enTxClock);
|
||||
gpioIF->pullHigh(enTxData);
|
||||
#endif
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::checkTxTimer() {
|
||||
if (linkState == DOWN) {
|
||||
return;
|
||||
}
|
||||
if (transmitterCountdown.hasTimedOut()) {
|
||||
disableTransmit();
|
||||
}
|
||||
}
|
||||
|
||||
void CcsdsIpCoreHandler::disableTransmit() {
|
||||
#ifndef TE0720_1CFA
|
||||
gpioIF->pullLow(enTxClock);
|
||||
gpioIF->pullLow(enTxData);
|
||||
#endif
|
||||
linkState = DOWN;
|
||||
forwardLinkstate();
|
||||
transmitterCountdown.setTimeout(0);
|
||||
}
|
||||
|
||||
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }
|
||||
Reference in New Issue
Block a user