eive-obsw/mission/tmtc/CcsdsIpCoreHandler.cpp
Robin Mueller 5a54d4e1ed
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good
afmt
2023-02-02 18:22:28 +01:00

450 lines
14 KiB
C++

#include "CcsdsIpCoreHandler.h"
#include <fsfw/subsystem/helper.h>
#include <linux/ipcore/PdecHandler.h>
#include <linux/ipcore/PtmeConfig.h>
#include <mission/config/comCfg.h>
#include "eive/definitions.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),
modeHelper(this),
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;
}
result = modeHelper.initialize();
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;
}
transmitterCountdown.setTimeout(transmitterTimeout);
transmitterCountdown.timeOut();
#if OBSW_SYRLINKS_SIMULATED == 1
// Update data on rising edge
ptmeConfig->invertTxClock(false);
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;
}
result = modeHelper.handleModeCommand(&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 > config::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 < config::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: {
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW);
result = ptmeConfig->setRate(RATE_100KBPS);
break;
}
case SET_HIGH_RATE: {
submode = static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH);
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();
if (mode == HasModesIF::MODE_OFF) {
mode = HasModesIF::MODE_ON;
}
return EXECUTION_FINISHED;
}
case DISABLE_TRANSMITTER: {
disableTransmit();
if (mode == HasModesIF::MODE_ON) {
mode = HasModesIF::MODE_OFF;
}
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
linkState = UP;
forwardLinkstate();
transmitterCountdown.resetTimer();
}
void CcsdsIpCoreHandler::checkTxTimer() {
if (linkState == DOWN) {
return;
}
if (transmitterCountdown.hasTimedOut()) {
disableTransmit();
// TODO: set mode to off (move timer to subsystem)
}
}
void CcsdsIpCoreHandler::getMode(Mode_t* mode, Submode_t* submode) {
*mode = this->mode;
*submode = this->submode;
}
ReturnValue_t CcsdsIpCoreHandler::checkModeCommand(Mode_t mode, Submode_t submode,
uint32_t* msToReachTheMode) {
if (mode == HasModesIF::MODE_ON) {
if (submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH) and
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW) and
submode != static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
return HasModesIF::INVALID_SUBMODE;
}
} else if (mode != HasModesIF::MODE_OFF) {
return returnvalue::FAILED;
}
*msToReachTheMode = 2000;
return returnvalue::OK;
}
void CcsdsIpCoreHandler::startTransition(Mode_t mode, Submode_t submode) {
auto rateHigh = [&]() {
ReturnValue_t result = ptmeConfig->setRate(RATE_500KBPS);
if (result == returnvalue::OK) {
this->mode = HasModesIF::MODE_ON;
}
};
auto rateLow = [&]() {
ReturnValue_t result = ptmeConfig->setRate(RATE_100KBPS);
if (result == returnvalue::OK) {
this->mode = HasModesIF::MODE_ON;
}
};
if (mode == HasModesIF::MODE_ON) {
enableTransmit();
if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_DEFAULT)) {
com::Datarate currentDatarate = com::getCurrentDatarate();
if (currentDatarate == com::Datarate::LOW_RATE_MODULATION_BPSK) {
rateLow();
} else if (currentDatarate == com::Datarate::HIGH_RATE_MODULATION_0QPSK) {
rateHigh();
}
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_HIGH)) {
rateHigh();
} else if (submode == static_cast<Submode_t>(com::CcsdsSubmode::DATARATE_LOW)) {
rateLow();
}
} else if (mode == HasModesIF::MODE_OFF) {
disableTransmit();
this->mode = HasModesIF::MODE_OFF;
}
this->submode = submode;
modeHelper.modeChanged(mode, submode);
announceMode(false);
}
void CcsdsIpCoreHandler::announceMode(bool recursive) { triggerEvent(MODE_INFO, mode, submode); }
void CcsdsIpCoreHandler::disableTransmit() {
#ifndef TE0720_1CFA
gpioIF->pullLow(enTxClock);
gpioIF->pullLow(enTxData);
#endif
linkState = DOWN;
forwardLinkstate();
transmitterCountdown.timeOut();
}
const char* CcsdsIpCoreHandler::getName() const { return "CCSDS Handler"; }
const HasHealthIF* CcsdsIpCoreHandler::getOptHealthIF() const { return nullptr; }
const HasModesIF& CcsdsIpCoreHandler::getModeIF() const { return *this; }
ReturnValue_t CcsdsIpCoreHandler::connectModeTreeParent(HasModeTreeChildrenIF& parent) {
return modetree::connectModeTreeParent(parent, *this, nullptr, modeHelper);
}
ModeTreeChildIF& CcsdsIpCoreHandler::getModeTreeChildIF() { return *this; }
object_id_t CcsdsIpCoreHandler::getObjectId() const { return SystemObject::getObjectId(); }